From c13726af66e73d5664d158e8824ff8287e4f1b70 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 17 Feb 2022 11:19:02 +0100 Subject: [PATCH] Enable DO_SET_SPEED commands outside of missions in other AUTO modes (#18834) * 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 * Navigator: reset cruise speed and throttle to default when VTOL-transitioning Signed-off-by: Silvan Fuhrer --- src/modules/navigator/mission.cpp | 5 ----- src/modules/navigator/navigator_main.cpp | 28 ++++++++++++++++++++++-- src/modules/navigator/rtl.cpp | 4 ++++ 3 files changed, 30 insertions(+), 7 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 84c6d0a2a1..77186d89f9 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 409be05f80..32aa236cc1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -503,6 +503,19 @@ void 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); @@ -547,6 +560,17 @@ void Navigator::run() _vehicle_roi_pub.publish(_vroi); publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) { + // reset cruise speed and throttle to default when transitioning + set_cruising_speed(); + set_cruising_throttle(); + + // need to update current setpooint with reset cruise speed and throttle + position_setpoint_triplet_s *rep = get_reposition_triplet(); + *rep = *(get_position_setpoint_triplet()); + rep->current.cruising_speed = get_cruising_speed(); + rep->current.cruising_throttle = get_cruising_throttle(); } } @@ -1016,7 +1040,7 @@ 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 (is_planned_mission() && _mission_cruising_speed_mc > 0.0f) { + if (_mission_cruising_speed_mc > 0.0f) { return _mission_cruising_speed_mc; } else { @@ -1024,7 +1048,7 @@ float 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 577c1e679f..df1d2a9613 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(); }