Allow for immediate cruise speed changes at any time during a mission.

src/modules/navigator/mission.cpp

@@ -230,6 +230,12 @@ 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_yawmode.get() != MISSION_YAWMODE_NONE
 	     && _param_yawmode.get() < MISSION_YAWMODE_MAX
@@ -1106,6 +1112,24 @@ Mission::altitude_sp_foh_reset()
 	_min_current_sp_distance_xy = FLT_MAX;
 }
 
+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;
+
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
 void
 Mission::do_abort_landing()
 {

src/modules/navigator/mission.h

@@ -162,6 +162,11 @@ private:
 	 */
 	void altitude_sp_foh_reset();
 
+	/**
+	 * Update the cruising speed setpoint.
+	 */
+	void cruising_speed_sp_update();
+
 	/**
 	 * Abort landing
 	 */
@@ -274,7 +279,6 @@ private:
 		WORK_ITEM_TYPE_TRANSITON_BEFORE_LAND,	/**<  */
 		WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION	/**<  */
 	} _work_item_type;	/**< current type of work to do (sub mission item) */
-
 };
 
 #endif

src/modules/navigator/navigator_main.cpp

@@ -757,7 +757,7 @@ Navigator::get_cruising_speed()
 {
 	/* there are three options: The mission-requested cruise speed, or the current hover / plane speed */
 	if (_vstatus.is_rotary_wing) {
-		if (_mission_cruising_speed_mc > FLT_EPSILON
+		if (_mission_cruising_speed_mc > 0.0f
 		    && _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
 			return _mission_cruising_speed_mc;
 
@@ -766,7 +766,7 @@ Navigator::get_cruising_speed()
 		}
 
 	} else {
-		if (_mission_cruising_speed_fw > FLT_EPSILON
+		if (_mission_cruising_speed_fw > 0.0f
 		    && _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
 			return _mission_cruising_speed_fw;