From 56dd1dc93077eab316395781143eff4f193a1ad6 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 8 Aug 2023 13:22:05 +0200 Subject: [PATCH] Navigator: Resume mission with last flight speed (#21714) * Navigator: DO_CHANGE_SPEED: only store sinlge cruising_speed_current_mode This stored cruising speed setpoint is reset on mode change and after a VTOL transition. * Navigator Mission: replay DO_CHANGE_SPEED items when resuming mission * Navigator: remove cruising_speed_sp_update() Speed changes in a mission are handled directly in the position controllers, and no longer in Navigator. Signed-off-by: Silvan Fuhrer --------- Signed-off-by: Silvan Fuhrer --- src/modules/navigator/mission.cpp | 46 +++++++++++------------- src/modules/navigator/mission.h | 12 ++++--- src/modules/navigator/navigator.h | 19 ++++------ src/modules/navigator/navigator_main.cpp | 39 +------------------- 4 files changed, 36 insertions(+), 80 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c260603d25..c8b2979087 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -212,7 +212,8 @@ Mission::on_activation() // we already reset the mission items _execution_mode_changed = false; - // reset the cache and fill it with the camera and gimbal items up to the previous item + // reset the cache and fill it with the items up to the previous item. The cache contains + // commands that are valid for the whole mission, not just a sinlge waypoint. if (_current_mission_index > 0) { resetItemCache(); updateCachedItemsUpToIndex(_current_mission_index - 1); @@ -303,6 +304,8 @@ Mission::on_active() replayCachedTriggerItems(); } + replayCachedSpeedChangeItems(); + /* lets check if we reached the current mission item */ if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached_or_completed()) { /* If we just completed a takeoff which was inserted before the right waypoint, @@ -324,11 +327,6 @@ Mission::on_active() } } - /* check if a cruise speed change has been commanded */ - if (_mission_type != MISSION_TYPE_NONE) { - cruising_speed_sp_update(); - } - /* see if we need to update the current yaw heading */ if (!_param_mis_mnt_yaw_ctl.get() && (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) @@ -1520,25 +1518,6 @@ Mission::heading_sp_update() } } -void -Mission::cruising_speed_sp_update() -{ - struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - const float cruising_speed = _navigator->get_cruising_speed(); - - /* Don't change setpoint if the current waypoint is not valid */ - if (!pos_sp_triplet->current.valid || - fabsf(pos_sp_triplet->current.cruising_speed - cruising_speed) < FLT_EPSILON) { - return; - } - - pos_sp_triplet->current.cruising_speed = cruising_speed; - - publish_navigator_mission_item(); - _navigator->set_position_setpoint_triplet_updated(); -} - void Mission::do_abort_landing() { @@ -2106,6 +2085,15 @@ void Mission::cacheItem(const mission_item_s &mission_item) _last_camera_trigger_item = mission_item; break; + case NAV_CMD_DO_CHANGE_SPEED: + _last_speed_change_item = mission_item; + break; + + case NAV_CMD_DO_VTOL_TRANSITION: + // delete speed changes after a VTOL transition + _last_speed_change_item = {}; + break; + default: break; } @@ -2137,6 +2125,14 @@ void Mission::replayCachedTriggerItems() } } +void Mission::replayCachedSpeedChangeItems() +{ + if (_last_speed_change_item.nav_cmd == NAV_CMD_DO_CHANGE_SPEED) { + issue_command(_last_speed_change_item); + _last_speed_change_item = {}; // delete cached item + } +} + void Mission::resetItemCache() { _last_gimbal_configure_item = {}; diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index be6b217cb5..28d1d56304 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -168,11 +168,6 @@ private: */ void heading_sp_update(); - /** - * Update the cruising speed setpoint. - */ - void cruising_speed_sp_update(); - /** * Abort landing */ @@ -290,6 +285,12 @@ private: */ void replayCachedTriggerItems(); + /** + * @brief Replay the cached speed change items and delete them afterwards + * + */ + void replayCachedSpeedChangeItems(); + /** * @brief Reset the item cache */ @@ -373,4 +374,5 @@ private: mission_item_s _last_gimbal_control_item {}; mission_item_s _last_camera_mode_item {}; mission_item_s _last_camera_trigger_item {}; + mission_item_s _last_speed_change_item {}; }; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 9151527752..41b33edab2 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -199,25 +199,21 @@ public: * * @return the desired cruising speed for this mission */ - float get_cruising_speed(); + float get_cruising_speed() { return _cruising_speed_current_mode; } /** * Set the cruising speed * - * Passing a negative value or leaving the parameter away will reset the cruising speed - * to its default value. - * - * For VTOL: sets cruising speed for current mode only (multirotor or fixed-wing). - * + * Passing a negative value will reset the cruising speed + * to its default value. Will automatically be reset to default + * on mode switch. */ - void set_cruising_speed(float speed = -1.0f); + void set_cruising_speed(float desired_speed) { _cruising_speed_current_mode = desired_speed; } /** * Reset cruising speed to default values - * - * For VTOL: resets both cruising speeds. */ - void reset_cruising_speed(); + void reset_cruising_speed() { _cruising_speed_current_mode = -1.f; } /** * Set triplets to invalid @@ -389,8 +385,7 @@ private: float _param_mpc_jerk_auto{4.f}; /**< initialized with the default jerk auto value to prevent division by 0 if the parameter is accidentally set to 0 */ float _param_mpc_acc_hor{3.f}; /**< initialized with the default horizontal acc value to prevent division by 0 if the parameter is accidentally set to 0 */ - float _mission_cruising_speed_mc{-1.0f}; - float _mission_cruising_speed_fw{-1.0f}; + float _cruising_speed_current_mode{-1.0f}; float _mission_throttle{NAN}; traffic_buffer_s _traffic_buffer{}; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 33b6c7eefb..9d9ed8c035 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -617,7 +617,7 @@ void Navigator::run() set_cruising_speed(cmd.param2); } else { - set_cruising_speed(); + reset_cruising_speed(); /* if no speed target was given try to set throttle */ if (cmd.param3 > FLT_EPSILON) { @@ -1160,43 +1160,6 @@ float Navigator::get_altitude_acceptance_radius() } } -float Navigator::get_cruising_speed() -{ - /* there are three options: The mission-requested cruise speed, or the current hover / plane speed */ - if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - if (_mission_cruising_speed_mc > 0.0f) { - return _mission_cruising_speed_mc; - - } else { - return -1.0f; - } - - } else { - if (_mission_cruising_speed_fw > 0.0f) { - return _mission_cruising_speed_fw; - - } else { - return -1.0f; - } - } -} - -void Navigator::set_cruising_speed(float speed) -{ - if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _mission_cruising_speed_mc = speed; - - } else { - _mission_cruising_speed_fw = speed; - } -} - -void Navigator::reset_cruising_speed() -{ - _mission_cruising_speed_mc = -1.0f; - _mission_cruising_speed_fw = -1.0f; -} - void Navigator::reset_triplets() { reset_position_setpoint(_pos_sp_triplet.previous);