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 <silvan@auterion.com>

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) {

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,15 +1013,15 @@ 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 {
 			return -1.0f;
 		}
 
 	} else {
-		if (is_planned_mission() && _mission_cruising_speed_fw > 0.0f) {
+		if (_mission_cruising_speed_fw > 0.0f) {
 			return _mission_cruising_speed_fw;
 
 		} else {

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();
 
 }