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

---------

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

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 = {};

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 {};
 };

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{};

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