From f892a624b785980d4dbf9752610fc72531800c20 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 12 Apr 2022 16:35:02 +0200 Subject: [PATCH] FlightModeManager/FixedwingPositionControl: robustify vehicle command parameter casting --- src/modules/flight_mode_manager/FlightModeManager.cpp | 5 +++-- src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp | 5 +++-- src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp | 2 +- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 5b5453d6f9..64f6b319a7 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -444,8 +444,9 @@ void FlightModeManager::handleCommand() } else if (_current_task.task) { // check for other commands not related to task switching - if (command.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED && command.param2 >= 0 - && (uint8_t)command.param1 == vehicle_command_s::SPEED_TYPE_GROUNDSPEED) { + if ((command.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) + && (static_cast(command.param1 + .5f) == vehicle_command_s::SPEED_TYPE_GROUNDSPEED) + && (command.param2 > 0.f)) { _current_task.task->setCruisingSpeed(command.param2); } } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index c017cb5c2a..905667253d 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -258,8 +258,9 @@ 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) { + } else if ((vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) + && (static_cast(vehicle_command.param1 + .5f) == vehicle_command_s::SPEED_TYPE_AIRSPEED) + && (vehicle_command.param2 > 0.f)) { if (_control_mode_current == FW_POSCTRL_MODE_AUTO) { _pos_sp_triplet.current.cruising_speed = vehicle_command.param2; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 31560215dd..d86211e568 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -266,7 +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 + float _commanded_airspeed_setpoint{NAN}; ///< airspeed setpoint for manual modes commanded via MAV_CMD_DO_CHANGE_SPEED hrt_abstime _time_in_fixed_bank_loiter{0};