From 68cf6868927f4e830f195fa5d3dd2248278f7ab2 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 12 Apr 2022 16:40:03 +0200 Subject: [PATCH] FlightTask: rename and move setCruisingSpeed() -> overrideCruiseSpeed() --- src/modules/flight_mode_manager/FlightModeManager.cpp | 2 +- .../flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp | 8 +++++++- .../flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp | 8 +------- .../flight_mode_manager/tasks/FlightTask/FlightTask.hpp | 3 +-- 4 files changed, 10 insertions(+), 11 deletions(-) diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 64f6b319a7..a561156b57 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -447,7 +447,7 @@ void FlightModeManager::handleCommand() 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); + _current_task.task->overrideCruiseSpeed(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 ec098255bd..f35a0b8fe1 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -223,6 +223,12 @@ bool FlightTaskAuto::update() return ret; } +void FlightTaskAuto::overrideCruiseSpeed(const float cruise_speed_m_s) +{ + _mc_cruise_speed = cruise_speed_m_s; + _commanded_speed_ts = hrt_absolute_time(); +} + void FlightTaskAuto::_prepareLandSetpoints() { _velocity_setpoint.setNaN(); // Don't take over any smoothed velocity setpoint @@ -345,7 +351,7 @@ bool FlightTaskAuto::_evaluateTriplets() _type = (WaypointType)_sub_triplet_setpoint.get().current.type; - // Override a commanded cruise speed if the cruise speed from the triplet is valid and more recent + // Prioritize cruise speed from the triplet when it's valid and more recent than the previously commanded cruise speed 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 diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index d2c1ff81b1..cac7cefc31 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -92,17 +92,11 @@ 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. */ void setYawHandler(WeatherVane *ext_yaw_handler) override {_ext_yaw_handler = ext_yaw_handler;} + void overrideCruiseSpeed(const float cruise_speed_m_s) override; protected: matrix::Vector2f _getTargetVelocityXY(); /**< only used for follow-me and only here because of legacy reason.*/ diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp index a592ff2185..d09b4dde5e 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp @@ -167,6 +167,7 @@ public: * This method does nothing, each flighttask which wants to use the yaw handler needs to override this method. */ virtual void setYawHandler(WeatherVane *ext_yaw_handler) {} + virtual void overrideCruiseSpeed(const float cruise_speed_m_s) {} void updateVelocityControllerFeedback(const matrix::Vector3f &vel_sp, const matrix::Vector3f &acc_sp) @@ -175,8 +176,6 @@ 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)};