Navigator: Add support to read cruising speed off a mission item

src/modules/navigator/mission_block.cpp

@@ -121,6 +121,13 @@ MissionBlock::is_mission_item_reached()
 				return false;
 			}
 
+		case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED:
+			// XXX not differentiating ground and airspeed yet
+			if (_mission_item.params[1] > 0.0f) {
+				_navigator->set_cruising_speed(_mission_item.params[1]);
+			}
+			return true;
+
 		default:
 			/* do nothing, this is a 3D waypoint */
 			break;
@@ -338,6 +345,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 	sp->pitch_min = item->pitch_min;
 	sp->acceptance_radius = item->acceptance_radius;
 	sp->disable_mc_yaw_control = false;
+	sp->cruising_speed = _navigator->get_cruising_speed();
 
 	switch (item->nav_cmd) {
 	case NAV_CMD_IDLE:

src/modules/navigator/navigator.h

@@ -156,6 +156,18 @@ public:
 	 */
 	float		get_acceptance_radius();
 
+	/**
+	 * Get the cruising speed
+	 *
+	 * @return the desired cruising speed for this mission
+	 */
+	float		get_cruising_speed();
+
+	/**
+	 * Set the cruising speed
+	 */
+	void		set_cruising_speed(float speed) { _mission_cruising_speed = speed; }
+
 	/**
 	 * Get the acceptance radius given the mission item preset radius
 	 *
@@ -244,6 +256,12 @@ private:
 	control::BlockParamFloat _param_acceptance_radius;	/**< acceptance for takeoff */
 	control::BlockParamInt _param_datalinkloss_obc;	/**< if true: obc mode on data link loss enabled */
 	control::BlockParamInt _param_rcloss_obc;	/**< if true: obc mode on rc loss enabled */
+	
+	control::BlockParamFloat _param_cruising_speed_hover;
+	control::BlockParamFloat _param_cruising_speed_plane;
+
+	float _mission_cruising_speed;
+
 	/**
 	 * Retrieve global position
 	 */

src/modules/navigator/navigator_main.cpp

@@ -151,7 +151,10 @@ Navigator::Navigator() :
 	_param_loiter_radius(this, "LOITER_RAD"),
 	_param_acceptance_radius(this, "ACC_RAD"),
 	_param_datalinkloss_obc(this, "DLL_OBC"),
-	_param_rcloss_obc(this, "RCL_OBC")
+	_param_rcloss_obc(this, "RCL_OBC"),
+	_param_cruising_speed_hover(this, "MPC_XY_VEL_MAX", false),
+	_param_cruising_speed_plane(this, "FW_AIRSPD_TRIM", false),
+	_mission_cruising_speed(0.0f)
 {
 	/* Create a list of our possible navigation types */
 	_navigation_mode_array[0] = &_mission;
@@ -615,6 +618,19 @@ Navigator::get_acceptance_radius()
 	return get_acceptance_radius(_param_acceptance_radius.get());
 }
 
+float
+Navigator::get_cruising_speed()
+{
+	/* there are three options: The mission-requested cruise speed, or the current hover / plane speed */
+	if (_mission_cruising_speed > 0.001f) {
+		return _mission_cruising_speed;
+	} else if (_vstatus.is_rotary_wing) {
+		return _param_cruising_speed_hover.get();
+	} else {
+		return _param_cruising_speed_plane.get();
+	}
+}
+
 float
 Navigator::get_acceptance_radius(float mission_item_radius)
 {