From ca657f36ef84ffa9938d04fbd3508685d47aa7d5 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 28 Mar 2022 11:35:04 +0300 Subject: [PATCH] FixedWingPositionControl: handle VEHICLE_CMD_DO_CHANGE_SPEED Signed-off-by: RomanBapst --- .../FixedwingPositionControl.cpp | 15 +++++++++++++++ .../FixedwingPositionControl.hpp | 1 + 2 files changed, 16 insertions(+) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index b5fbc672a2..c017cb5c2a 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/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; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 1a79dfd554..31560215dd 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/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};