From d41de33a85f13ad0fb1162244a8933dbeb6ccb90 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 28 Mar 2022 11:41:43 +0300 Subject: [PATCH] FlightModeManager: handle MAV_CMD_DO_CHANGE_SPEED - support setting the cruise speed of the auto flight task via command Signed-off-by: RomanBapst --- src/modules/flight_mode_manager/FlightModeManager.cpp | 6 ++++++ .../flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp | 10 ++++++++-- .../flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp | 11 ++++++++++- .../tasks/FlightTask/FlightTask.hpp | 2 ++ 4 files changed, 26 insertions(+), 3 deletions(-) diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 1bb6483acc..1babed001d 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -441,6 +441,12 @@ void FlightModeManager::handleCommand() command_ack.target_component = command.source_component; command_ack.timestamp = hrt_absolute_time(); _vehicle_command_ack_pub.publish(command_ack); + } 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) { + _current_task.task->setCruisingSpeed(command.param2); + } } } } diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index d1105c681c..ec098255bd 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -81,6 +81,7 @@ bool FlightTaskAuto::activate(const vehicle_local_position_setpoint_s &last_setp _yaw_sp_prev = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw; _updateTrajConstraints(); _is_emergency_braking_active = false; + _commanded_speed_ts = 0; return ret; } @@ -344,8 +345,13 @@ bool FlightTaskAuto::_evaluateTriplets() _type = (WaypointType)_sub_triplet_setpoint.get().current.type; - // Always update cruise speed since that can change without waypoint changes. - _mc_cruise_speed = _sub_triplet_setpoint.get().current.cruising_speed; + // Override a commanded cruise speed if the cruise speed from the triplet is valid and more recent + const float cruise_speed_from_triplet = _sub_triplet_setpoint.get().current.cruising_speed; + + if (PX4_ISFINITE(cruise_speed_from_triplet) && cruise_speed_from_triplet >= 0 + && _sub_triplet_setpoint.get().current.timestamp > _commanded_speed_ts) { + _mc_cruise_speed = cruise_speed_from_triplet; + } if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f)) { // If no speed is planned use the default cruise speed as limit diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index 2e87a701f0..45f242c632 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -92,6 +92,13 @@ public: bool updateInitialize() override; bool update() override; + bool setCruisingSpeed(const float cruising_speed_m_s) override + { + _mc_cruise_speed = cruising_speed_m_s; + _commanded_speed_ts = hrt_absolute_time(); + return true; + } + /** * Sets an external yaw handler which can be used to implement a different yaw control strategy. */ @@ -128,7 +135,7 @@ protected: matrix::Vector3f _target{}; /**< Target waypoint (local frame).*/ matrix::Vector3f _next_wp{}; /**< The next waypoint after target (local frame). If no next setpoint is available, next is set to target. */ bool _next_was_valid{false}; - float _mc_cruise_speed{0.0f}; /**< Requested cruise speed. If not valid, default cruise speed is used. */ + float _mc_cruise_speed{-1.0f}; /**< Requested cruise speed. If not valid, default cruise speed is used. */ WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */ uORB::SubscriptionData _sub_home_position{ORB_ID(home_position)}; @@ -201,6 +208,8 @@ private: _triplet_next_wp; /**< next triplet from navigator which may differ from the intenal one (_next_wp) depending on the vehicle state.*/ matrix::Vector3f _closest_pt; /**< closest point to the vehicle position on the line previous - target */ + hrt_abstime _commanded_speed_ts{0}; + MapProjection _reference_position{}; /**< Class used to project lat/lon setpoint into local frame. */ float _reference_altitude{NAN}; /**< Altitude relative to ground. */ hrt_abstime _time_stamp_reference{0}; /**< time stamp when last reference update occured. */ diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp index 7619b2f7c0..a592ff2185 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp @@ -175,6 +175,8 @@ public: _acceleration_setpoint_feedback = acc_sp; } + virtual bool setCruisingSpeed(const float cruising_speed_m_s) { return false; } + protected: uORB::SubscriptionData _sub_vehicle_local_position{ORB_ID(vehicle_local_position)}; uORB::SubscriptionData _sub_home_position{ORB_ID(home_position)};