FixedWingPositionControl: handle VEHICLE_CMD_DO_CHANGE_SPEED

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -257,6 +257,18 @@ FixedwingPositionControl::vehicle_command_poll()
 
 				abort_landing(true);
 			}
+
+		} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED
+			   && (uint8_t)vehicle_command.param1 == vehicle_command_s::SPEED_TYPE_AIRSPEED && vehicle_command.param2 > 0) {
+
+			if (_control_mode_current == FW_POSCTRL_MODE_AUTO) {
+				_pos_sp_triplet.current.cruising_speed = vehicle_command.param2;
+
+			} else if (_control_mode_current == FW_POSCTRL_MODE_MANUAL_ALTITUDE
+				   || _control_mode_current == FW_POSCTRL_MODE_MANUAL_POSITION) {
+				_commanded_airspeed_setpoint = vehicle_command.param2;
+			}
+
 		}
 	}
 }
@@ -399,6 +411,9 @@ FixedwingPositionControl::get_manual_airspeed_setpoint()
 					   (_param_fw_airspd_max.get() - _param_fw_airspd_trim.get()) *
 					   (_manual_control_setpoint_airspeed * 2 - 1);
 		}
+
+	} else if (PX4_ISFINITE(_commanded_airspeed_setpoint)) {
+		altctrl_airspeed = _commanded_airspeed_setpoint;
 	}
 
 	return altctrl_airspeed;

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -266,6 +266,7 @@ private:
 
 	float _manual_control_setpoint_altitude{0.0f};
 	float _manual_control_setpoint_airspeed{0.0f};
+	float _commanded_airspeed_setpoint{(float)NAN};		///< airspeed setpoint for manual modes commanded via MAV_CMD_DO_CHANGE_SPEED
 
 	hrt_abstime _time_in_fixed_bank_loiter{0};