mc_pos_control: fix desired trajectory waypoint. (#10372)
mc_pos_control: fix desired trajectory waypoint. (#10372)

 - The first waypoint contains the vehicle current position and the desired velocity setpoint
PX4/Firmware,acfloria/Firmware,mje-nz/PX4-Firmware,Aerotenna/Firmware,Aerotenna/Firmware,krbeverx/Firmware,Aerotenna/Firmware,mje-nz/PX4-Firmware,krbeverx/Firmware,PX4/Firmware,mje-nz/PX4-Firmware,dagar/Firmware,mje-nz/PX4-Firmware,Aerotenna/Firmware,krbeverx/Firmware,acfloria/Firmware,PX4/Firmware,acfloria/Firmware,mje-nz/PX4-Firmware,PX4/Firmware,Aerotenna/Firmware,PX4/Firmware,dagar/Firmware,Aerotenna/Firmware,acfloria/Firmware,dagar/Firmware,acfloria/Firmware,mje-nz/PX4-Firmware,krbeverx/Firmware,krbeverx/Firmware,acfloria/Firmware,mje-nz/PX4-Firmware,dagar/Firmware,dagar/Firmware,Aerotenna/Firmware,dagar/Firmware,PX4/Firmware,krbeverx/Firmware,acfloria/Firmware,krbeverx/Firmware,PX4/Firmware,dagar/Firmware