From 9d533ba5dd51843127e19e7366215d97122308f9 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 7 Dec 2021 16:17:37 +0100 Subject: [PATCH] Navigator: enable DO_CHANGE_SPEED for outside of mission - update _mission_cruising_speed_mc/_fw also if DO_CHANGE_SPEED command is received outside of mission (e.g. while Loitering doing an Orbit) - if vehicle is in AUTO_LOITER when receiving the change speed, then immediately apply it by doing a reposition without updating any other field than cruising_speed and cruising_throttle -when RTL is activated reset the cruising speed and throttle Signed-off-by: Silvan Fuhrer --- src/modules/navigator/mission.cpp | 5 ----- src/modules/navigator/navigator_main.cpp | 17 +++++++++++++++-- src/modules/navigator/rtl.cpp | 4 ++++ 3 files changed, 19 insertions(+), 7 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 84c6d0a2a1a4..77186d89f9c2 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -96,11 +96,6 @@ void Mission::mission_init() void Mission::on_inactive() { - /* We need to reset the mission cruising speed, otherwise the - * mission velocity which might have been set using mission items - * is used for missions such as RTL. */ - _navigator->set_cruising_speed(); - // if we were executing an landing but have been inactive for 2 seconds, then make the landing invalid // this prevents RTL to just continue at the current mission index if (_navigator->getMissionLandingInProgress() && (hrt_absolute_time() - _time_mission_deactivated) > 2_s) { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a14ae1fb01b6..4bcc0d0dc936 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -498,6 +498,19 @@ Navigator::run() } } + if (get_vstatus()->nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) { + + // publish new reposition setpoint with updated speed/throtte when DO_CHANGE_SPEED + // was received in AUTO_LOITER mode + + position_setpoint_triplet_s *rep = get_reposition_triplet(); + + // set repo setpoint to current, and only change speed and throttle fields + *rep = *(get_position_setpoint_triplet()); + rep->current.cruising_speed = get_cruising_speed(); + rep->current.cruising_throttle = get_cruising_throttle(); + } + // TODO: handle responses for supported DO_CHANGE_SPEED options? publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); @@ -1000,7 +1013,7 @@ 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 (is_planned_mission() && _mission_cruising_speed_mc > 0.0f) { + if (_mission_cruising_speed_mc > 0.0f) { return _mission_cruising_speed_mc; } else { @@ -1008,7 +1021,7 @@ Navigator::get_cruising_speed() } } else { - if (is_planned_mission() && _mission_cruising_speed_fw > 0.0f) { + if (_mission_cruising_speed_fw > 0.0f) { return _mission_cruising_speed_fw; } else { diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 80ecaa646e6c..28d9420ed920 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -287,6 +287,10 @@ void RTL::on_activation() setClimbAndReturnDone(_rtl_state > RTL_STATE_RETURN); + // reset cruising speed and throttle to default for RTL + _navigator->set_cruising_speed(); + _navigator->set_cruising_throttle(); + set_rtl_item(); }