Implement MAV_CMD_DO_CHANGE_SPEED throttle

msg/position_setpoint.msg

@@ -37,3 +37,4 @@ bool acceleration_valid		# true if acceleration setpoint is valid/should be used
 bool acceleration_is_force	# interprete acceleration as force
 float32 acceptance_radius   # navigation acceptance_radius if we're doing waypoint navigation
 float32 cruising_speed		# the generally desired cruising speed (not a hard constraint)
+float32 cruising_throttle	# the generally desired cruising throttle (not a hard constraint)

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -1323,6 +1323,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			mission_airspeed = _pos_sp_triplet.current.cruising_speed;
 		}
 
+		float mission_throttle = _parameters.throttle_cruise;
+
+		if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_throttle) &&
+			_pos_sp_triplet.current.cruising_throttle > 0.1f) {
+			mission_throttle = _pos_sp_triplet.current.cruising_throttle;
+		}
+
+		printf("%f \n", (double)mission_throttle);
+
 		if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
 			_att_sp.thrust = 0.0f;
 			_att_sp.roll_body = 0.0f;
@@ -1336,7 +1345,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 			tecs_update_pitch_throttle(pos_sp_triplet.current.alt, calculate_target_airspeed(mission_airspeed), eas2tas,
 						   math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
-						   _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+						   _parameters.throttle_min, _parameters.throttle_max, mission_throttle,
 						   false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
 
 		} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {

src/modules/navigator/mission_block.cpp

@@ -127,7 +127,14 @@ MissionBlock::is_mission_item_reached()
 				_navigator->set_cruising_speed(_mission_item.params[1]);
 			} else {
 				_navigator->set_cruising_speed();
+				/* if no speed target was given try to set throttle */
+				if (_mission_item.params[2] > 0.0f) {
+					_navigator->set_cruising_throttle(_mission_item.params[2] / 100);
+				} else {
+					_navigator->set_cruising_throttle();
+				}
 			}
+
 			return true;
 
 		default:
@@ -367,6 +374,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 	sp->acceptance_radius = item->acceptance_radius;
 	sp->disable_mc_yaw_control = false;
 	sp->cruising_speed = _navigator->get_cruising_speed();
+	sp->cruising_throttle = _navigator->get_cruising_throttle();
 
 	switch (item->nav_cmd) {
 	case NAV_CMD_IDLE:

src/modules/navigator/navigator.h

@@ -178,6 +178,19 @@ public:
 	 */
 	void		set_cruising_speed(float speed=-1.0f) { _mission_cruising_speed = speed; }
 
+
+	/**
+	 * Get the target throttle
+	 *
+	 * @return the desired throttle for this mission
+	 */
+	float		get_cruising_throttle();
+
+	/**
+	 * Set the target throttle
+	 */
+	void		set_cruising_throttle(float percent=-1.0f) { _mission_throttle = percent; }
+
 	/**
 	 * Get the acceptance radius given the mission item preset radius
 	 *
@@ -277,8 +290,10 @@ private:
 	
 	control::BlockParamFloat _param_cruising_speed_hover;
 	control::BlockParamFloat _param_cruising_speed_plane;
+	control::BlockParamFloat _param_cruising_throttle_plane;
 
 	float _mission_cruising_speed;
+	float _mission_throttle;
 
 	/**
 	 * Retrieve global position

src/modules/navigator/navigator_main.cpp

@@ -159,6 +159,7 @@ Navigator::Navigator() :
 	_param_rcloss_act(this, "RCL_ACT"),
 	_param_cruising_speed_hover(this, "MPC_XY_CRUISE", false),
 	_param_cruising_speed_plane(this, "FW_AIRSPD_TRIM", false),
+	_param_cruising_throttle_plane(this, "FW_THR_CRUISE", false),
 	_mission_cruising_speed(-1.0f)
 {
 	/* Create a list of our possible navigation types */
@@ -725,6 +726,17 @@ Navigator::get_cruising_speed()
 	}
 }
 
+float
+Navigator::get_cruising_throttle()
+{
+	/* Return the mission-requested cruise speed, or -1 */
+	if (_mission_throttle > 0.0f) {
+		return _mission_throttle;
+	} else {
+		return _param_cruising_throttle_plane.get();
+	}
+}
+
 float
 Navigator::get_acceptance_radius(float mission_item_radius)
 {