FixedWingPositionControl: do not pass trim throttle via position setpoint triplet

- trim throttle is handled entirely by the position controller
- navigator should use speed setpoints
- added flag gliding_enabled in position setpoint to stills support gliding

Signed-off-by: RomanBapst <bapstroman@gmail.com>

msg/position_setpoint.msg

@@ -40,6 +40,7 @@ int8 loiter_direction		# loiter direction: 1 = CW, -1 = CCW
 float32 acceptance_radius   # navigation acceptance_radius if we're doing waypoint navigation
 
 float32 cruising_speed		# the generally desired cruising speed (not a hard constraint)
+bool gliding_enabled		# commands the vehicle to glide if the capability is available (fixed wing only)
 float32 cruising_throttle	# the generally desired cruising throttle (not a hard constraint)
 
 bool disable_weather_vane   # VTOL: disable (in auto mode) the weather vane feature that turns the nose into the wind

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -955,7 +955,6 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i
 				   radians(_param_fw_p_lim_max.get()),
 				   _param_fw_thr_min.get(),
 				   _param_fw_thr_max.get(),
-				   _param_fw_thr_cruise.get(),
 				   false,
 				   _param_fw_p_lim_min.get());
 
@@ -989,7 +988,6 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)
 				   radians(_param_fw_p_lim_max.get()),
 				   _param_fw_thr_min.get(),
 				   _param_fw_thr_max.get(),
-				   _param_fw_thr_cruise.get(),
 				   false,
 				   _param_fw_p_lim_min.get(),
 				   false,
@@ -1088,26 +1086,16 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 
 	float tecs_fw_thr_min;
 	float tecs_fw_thr_max;
-	float tecs_fw_mission_throttle;
 
-	float mission_throttle = _param_fw_thr_cruise.get();
-
-	if (PX4_ISFINITE(pos_sp_curr.cruising_throttle) &&
-	    pos_sp_curr.cruising_throttle >= 0.0f) {
-		mission_throttle = pos_sp_curr.cruising_throttle;
-	}
-
-	if (mission_throttle < _param_fw_thr_min.get()) {
+	if (pos_sp_curr.gliding_enabled) {
 		/* enable gliding with this waypoint */
 		_tecs.set_speed_weight(2.0f);
 		tecs_fw_thr_min = 0.0;
 		tecs_fw_thr_max = 0.0;
-		tecs_fw_mission_throttle = 0.0;
 
 	} else {
 		tecs_fw_thr_min = _param_fw_thr_min.get();
 		tecs_fw_thr_max = _param_fw_thr_max.get();
-		tecs_fw_mission_throttle = mission_throttle;
 	}
 
 	// waypoint is a plain navigation waypoint
@@ -1187,7 +1175,6 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 				   radians(_param_fw_p_lim_max.get()),
 				   tecs_fw_thr_min,
 				   tecs_fw_thr_max,
-				   tecs_fw_mission_throttle,
 				   false,
 				   radians(_param_fw_p_lim_min.get()));
 }
@@ -1198,26 +1185,16 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
 {
 	float tecs_fw_thr_min;
 	float tecs_fw_thr_max;
-	float tecs_fw_mission_throttle;
-
-	float mission_throttle = _param_fw_thr_cruise.get();
-
-	if (PX4_ISFINITE(pos_sp_curr.cruising_throttle) &&
-	    pos_sp_curr.cruising_throttle >= 0.0f) {
-		mission_throttle = pos_sp_curr.cruising_throttle;
-	}
 
-	if (mission_throttle < _param_fw_thr_min.get()) {
+	if (pos_sp_curr.gliding_enabled) {
 		/* enable gliding with this waypoint */
 		_tecs.set_speed_weight(2.0f);
 		tecs_fw_thr_min = 0.0;
 		tecs_fw_thr_max = 0.0;
-		tecs_fw_mission_throttle = 0.0;
 
 	} else {
 		tecs_fw_thr_min = _param_fw_thr_min.get();
 		tecs_fw_thr_max = _param_fw_thr_max.get();
-		tecs_fw_mission_throttle = mission_throttle;
 	}
 
 	// waypoint is a plain navigation waypoint
@@ -1254,7 +1231,6 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
 				   radians(_param_fw_p_lim_max.get()),
 				   tecs_fw_thr_min,
 				   tecs_fw_thr_max,
-				   tecs_fw_mission_throttle,
 				   false,
 				   radians(_param_fw_p_lim_min.get()),
 				   tecs_status_s::TECS_MODE_NORMAL,
@@ -1295,26 +1271,16 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 
 	float tecs_fw_thr_min;
 	float tecs_fw_thr_max;
-	float tecs_fw_mission_throttle;
-
-	float mission_throttle = _param_fw_thr_cruise.get();
 
-	if (PX4_ISFINITE(pos_sp_curr.cruising_throttle) &&
-	    pos_sp_curr.cruising_throttle >= 0.0f) {
-		mission_throttle = pos_sp_curr.cruising_throttle;
-	}
-
-	if (mission_throttle < _param_fw_thr_min.get()) {
+	if (pos_sp_curr.gliding_enabled) {
 		/* enable gliding with this waypoint */
 		_tecs.set_speed_weight(2.0f);
 		tecs_fw_thr_min = 0.0;
 		tecs_fw_thr_max = 0.0;
-		tecs_fw_mission_throttle = 0.0;
 
 	} else {
 		tecs_fw_thr_min = _param_fw_thr_min.get();
 		tecs_fw_thr_max = _param_fw_thr_max.get();
-		tecs_fw_mission_throttle = mission_throttle;
 	}
 
 	/* waypoint is a loiter waypoint */
@@ -1391,7 +1357,6 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 				   radians(_param_fw_p_lim_max.get()),
 				   tecs_fw_thr_min,
 				   tecs_fw_thr_max,
-				   tecs_fw_mission_throttle,
 				   false,
 				   radians(_param_fw_p_lim_min.get()));
 }
@@ -1482,7 +1447,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 					   radians(takeoff_pitch_max_deg),
 					   _param_fw_thr_min.get(),
 					   _param_fw_thr_max.get(), // XXX should we also set runway_takeoff_throttle here?
-					   _param_fw_thr_cruise.get(),
 					   _runway_takeoff.climbout(),
 					   radians(_runway_takeoff.getMinPitch(_takeoff_pitch_min.get(), _param_fw_p_lim_min.get())));
 
@@ -1570,7 +1534,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 							   radians(takeoff_pitch_max_deg),
 							   _param_fw_thr_min.get(),
 							   takeoff_throttle,
-							   _param_fw_thr_cruise.get(),
 							   true,
 							   radians(_takeoff_pitch_min.get()));
 
@@ -1585,7 +1548,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 							   radians(_param_fw_p_lim_max.get()),
 							   _param_fw_thr_min.get(),
 							   takeoff_throttle,
-							   _param_fw_thr_cruise.get(),
 							   false,
 							   radians(_param_fw_p_lim_min.get()));
 			}
@@ -1805,8 +1767,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 		const float airspeed_land = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
 		float target_airspeed = get_auto_airspeed_setpoint(control_interval, airspeed_land, ground_speed);
 
-		const float throttle_land = _param_fw_thr_min.get() + (_param_fw_thr_max.get() - _param_fw_thr_min.get()) * 0.1f;
-
 		/* lateral guidance */
 		if (_param_fw_use_npfg.get()) {
 			_npfg.setAirspeedNom(target_airspeed * _eas2tas);
@@ -1858,7 +1818,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 					   radians(_param_fw_lnd_fl_pmax.get()),
 					   0.0f,
 					   throttle_max,
-					   throttle_land,
 					   false,
 					   _land_motor_lim ? radians(_param_fw_lnd_fl_pmin.get()) : radians(_param_fw_p_lim_min.get()),
 					   true);
@@ -1962,7 +1921,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 					   radians(_param_fw_p_lim_max.get()),
 					   _param_fw_thr_min.get(),
 					   _param_fw_thr_max.get(),
-					   _param_fw_thr_cruise.get(),
 					   false,
 					   radians(_param_fw_p_lim_min.get()));
 		_att_sp.pitch_body = get_tecs_pitch();
@@ -2022,7 +1980,6 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
 				   radians(_param_fw_p_lim_max.get()),
 				   _param_fw_thr_min.get(),
 				   throttle_max,
-				   _param_fw_thr_cruise.get(),
 				   false,
 				   pitch_limit_min,
 				   false,
@@ -2152,7 +2109,6 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 				   radians(_param_fw_p_lim_max.get()),
 				   _param_fw_thr_min.get(),
 				   throttle_max,
-				   _param_fw_thr_cruise.get(),
 				   false,
 				   pitch_limit_min,
 				   false,
@@ -2598,7 +2554,7 @@ float FixedwingPositionControl::compensateTrimThrottleForDensityAndWeight(float
 void
 FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
 		float pitch_min_rad, float pitch_max_rad,
-		float throttle_min, float throttle_max, float throttle_cruise,
+		float throttle_min, float throttle_max,
 		bool climbout_mode, float climbout_pitch_min_rad,
 		bool disable_underspeed_detection, float hgt_rate_sp)
 {
@@ -2682,7 +2638,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 				    climbout_pitch_min_rad - radians(_param_fw_psp_off.get()),
 				    throttle_min,
 				    throttle_max,
-				    throttle_cruise,
+				    throttle_trim_comp,
 				    pitch_min_rad - radians(_param_fw_psp_off.get()),
 				    pitch_max_rad - radians(_param_fw_psp_off.get()),
 				    _param_climbrate_target.get(),

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -628,7 +628,6 @@ private:
 	 * @param pitch_max_rad Nominal pitch angle command maximum [rad]
 	 * @param throttle_min Minimum throttle command [0,1]
 	 * @param throttle_max Maximum throttle command [0,1]
-	 * @param throttle_cruise Throttle required for level flight at cruising airspeed [0,1]
 	 * @param climbout_mode True if TECS should engage climbout mode
 	 * @param climbout_pitch_min_rad Minimum pitch angle command in climbout mode [rad]
 	 * @param disable_underspeed_detection True if underspeed detection should be disabled
@@ -636,7 +635,7 @@ private:
 	 */
 	void tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
 					float pitch_min_rad, float pitch_max_rad,
-					float throttle_min, float throttle_max, float throttle_cruise,
+					float throttle_min, float throttle_max,
 					bool climbout_mode, float climbout_pitch_min_rad,
 					bool disable_underspeed_detection = false, float hgt_rate_sp = NAN);
 

src/modules/navigator/mission_block.cpp

@@ -620,6 +620,10 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 	sp->cruising_speed = _navigator->get_cruising_speed();
 	sp->cruising_throttle = _navigator->get_cruising_throttle();
 
+	// for fixed wing we don't use cruising_throttle directly anymore, instead we command airspeed setpoints via cruising_speed
+	// we still use cruising throttle here to determine if gliding is enabled
+	sp->gliding_enabled = (_navigator->get_cruising_throttle() < FLT_EPSILON);
+
 	switch (item.nav_cmd) {
 	case NAV_CMD_IDLE:
 		sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE;