Support gliding setpoints for autonomous soaring

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -668,8 +668,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 		float mission_throttle = _param_fw_thr_cruise.get();
 
 		if (PX4_ISFINITE(pos_sp_curr.cruising_throttle) &&
-		    pos_sp_curr.cruising_throttle > 0.01f) {
-
+		    pos_sp_curr.cruising_throttle >= 0.0f) {
 			mission_throttle = pos_sp_curr.cruising_throttle;
 		}
 
@@ -684,16 +683,34 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 			_att_sp.roll_body = _l1_control.get_roll_setpoint();
 			_att_sp.yaw_body = _l1_control.nav_bearing();
 
+			float tecs_fw_thr_min;
+			float tecs_fw_thr_max;
+			float tecs_fw_mission_throttle;
+
+			if (mission_throttle < _param_fw_thr_min.get()) {
+				/* 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;
+			}
+
 			tecs_update_pitch_throttle(now, pos_sp_curr.alt,
 						   calculate_target_airspeed(mission_airspeed, ground_speed),
 						   radians(_param_fw_p_lim_min.get()) - radians(_param_fw_psp_off.get()),
 						   radians(_param_fw_p_lim_max.get()) - radians(_param_fw_psp_off.get()),
-						   _param_fw_thr_min.get(),
-						   _param_fw_thr_max.get(),
-						   mission_throttle,
+						   tecs_fw_thr_min,
+						   tecs_fw_thr_max,
+						   tecs_fw_mission_throttle,
 						   false,
 						   radians(_param_fw_p_lim_min.get()));
 
+
 		} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
 
 			/* waypoint is a loiter waypoint */
@@ -741,13 +758,31 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 				_tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get());
 			}
 
+
+			float tecs_fw_thr_min;
+			float tecs_fw_thr_max;
+			float tecs_fw_mission_throttle;
+
+			if (mission_throttle < _param_fw_thr_min.get()) {
+				/* 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 = _param_fw_thr_cruise.get();
+			}
+
 			tecs_update_pitch_throttle(now, alt_sp,
 						   calculate_target_airspeed(mission_airspeed, ground_speed),
 						   radians(_param_fw_p_lim_min.get()) - radians(_param_fw_psp_off.get()),
 						   radians(_param_fw_p_lim_max.get()) - radians(_param_fw_psp_off.get()),
-						   _param_fw_thr_min.get(),
-						   _param_fw_thr_max.get(),
-						   _param_fw_thr_cruise.get(),
+						   tecs_fw_thr_min,
+						   tecs_fw_thr_max,
+						   tecs_fw_mission_throttle,
 						   false,
 						   radians(_param_fw_p_lim_min.get()));
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -811,6 +811,10 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 		bool is_land_sp = (bool)(set_position_target_local_ned.type_mask & 0x2000);
 		bool is_loiter_sp = (bool)(set_position_target_local_ned.type_mask & 0x3000);
 		bool is_idle_sp = (bool)(set_position_target_local_ned.type_mask & 0x4000);
+		bool is_gliding_sp = (bool)(set_position_target_local_ned.type_mask &
+					    (POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_Z_IGNORE
+					     | POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_VZ_IGNORE
+					     | POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_AZ_IGNORE));
 
 		offboard_control_mode.timestamp = hrt_absolute_time();
 		_offboard_control_mode_pub.publish(offboard_control_mode);
@@ -851,6 +855,9 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					} else if (is_idle_sp) {
 						pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
 
+					} else if (is_gliding_sp) {
+						pos_sp_triplet.current.cruising_throttle = 0.0f;
+
 					} else {
 						pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 					}

src/modules/navigator/mission.cpp

@@ -1912,6 +1912,7 @@ bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const posit
 		(p1->acceleration_is_force == p2->acceleration_is_force) &&
 		(fabsf(p1->acceptance_radius - p2->acceptance_radius) < FLT_EPSILON) &&
 		(fabsf(p1->cruising_speed - p2->cruising_speed) < FLT_EPSILON) &&
-		(fabsf(p1->cruising_throttle - p2->cruising_throttle) < FLT_EPSILON));
+		((fabsf(p1->cruising_throttle - p2->cruising_throttle) < FLT_EPSILON) || (!PX4_ISFINITE(p1->cruising_throttle)
+				&& !PX4_ISFINITE(p2->cruising_throttle))));
 
 }

src/modules/navigator/navigator.h

@@ -240,7 +240,7 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
 	/**
 	 * Set the target throttle
 	 */
-	void		set_cruising_throttle(float throttle = -1.0f) { _mission_throttle = throttle; }
+	void		set_cruising_throttle(float throttle = NAN) { _mission_throttle = throttle; }
 
 	/**
 	 * Get the acceptance radius given the mission item preset radius
@@ -393,7 +393,7 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
 
 	float _mission_cruising_speed_mc{-1.0f};
 	float _mission_cruising_speed_fw{-1.0f};
-	float _mission_throttle{-1.0f};
+	float _mission_throttle{NAN};
 
 	// update subscriptions
 	void		params_update();

src/modules/navigator/navigator_main.cpp

@@ -907,7 +907,7 @@ Navigator::get_cruising_throttle()
 		return _mission_throttle;
 
 	} else {
-		return -1.0f;
+		return NAN;
 	}
 }