Fixed-wing Position controller: add modes for auto altitude and auto descend

- bit of clean up
- add GPS failsafe mode auto_altitude, that will keep current altitude with a fixed-bank angle
for some time, then switches to auto_descend that will descend with constant sink rate
of 0.5m/s
- params controlling GPS failsafe are not FW params: NAV_GPSF_R --> FW_GPSF_R and
NAV_GPSF_LT --> FW_GPSF_LT

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/lib/parameters/param_translation.cpp

@@ -195,6 +195,20 @@ bool param_modify_on_import(bson_node_t node)
 		}
 	}
 
+	// 2021-10-21: translate NAV_GPSF_LT to FW_GPSF_LT and NAV_GPSF_R to FW_GPSF_R
+	{
+		if (strcmp("NAV_GPSF_LT", node->name) == 0) {
+			strcpy(node->name, "FW_GPSF_LT");
+			PX4_INFO("copying  %s -> %s", "NAV_GPSF_LT", "FW_GPSF_LT");
+		}
+
+		if (strcmp("NAV_GPSF_R", node->name) == 0) {
+			strcpy(node->name, "FW_GPSF_R");
+			PX4_INFO("copying and inverting sign %s -> %s", "NAV_GPSF_R", "FW_GPSF_R");
+		}
+	}
+
+
 	// translate (SPI) calibration ID parameters. This can be removed after the next release (current release=1.10)
 
 	if (node->type != BSON_INT32) {

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -236,7 +236,6 @@ FixedwingPositionControl::vehicle_command_poll()
 				abort_landing(true);
 			}
 		}
-
 	}
 }
 
@@ -493,7 +492,7 @@ FixedwingPositionControl::status_publish()
 
 	pos_ctrl_status.timestamp = hrt_absolute_time();
 
-	pos_ctrl_status.type = _type;
+	pos_ctrl_status.type = _position_sp_type;
 
 	_pos_ctrl_status_pub.publish(pos_ctrl_status);
 }
@@ -671,7 +670,7 @@ FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitch_lim
 }
 
 void
-FixedwingPositionControl::set_control_mode_current(bool pos_sp_curr_valid)
+FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid)
 {
 	/* only run position controller in fixed-wing mode and during transitions for VTOL */
 	if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.in_transition_mode) {
@@ -679,11 +678,41 @@ FixedwingPositionControl::set_control_mode_current(bool pos_sp_curr_valid)
 		return; // do not publish the setpoint
 	}
 
-	if ((_control_mode.flag_control_auto_enabled || _control_mode.flag_control_offboard_enabled) && pos_sp_curr_valid) {
+	if (((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled) ||
+	     _control_mode.flag_control_offboard_enabled) && pos_sp_curr_valid) {
 		_control_mode_current = FW_POSCTRL_MODE_AUTO;
 
-	} else if (_control_mode.flag_control_velocity_enabled && _control_mode.flag_control_altitude_enabled) {
+	} else if (_control_mode.flag_control_auto_enabled && _control_mode.flag_control_climb_rate_enabled) {
-		if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
+
+		// reset timer the first time we switch into this mode
+		if (_control_mode_current != FW_POSCTRL_MODE_AUTO_ALTITUDE && _control_mode_current != FW_POSCTRL_MODE_AUTO_CLIMBRATE) {
+			_time_in_fixed_bank_loiter = now;
+		}
+
+		if (hrt_elapsed_time(&_time_in_fixed_bank_loiter) < (_param_nav_gpsf_lt.get() * 1_s)
+		    && !_vehicle_status.in_transition_mode) {
+			if (_control_mode_current != FW_POSCTRL_MODE_AUTO_ALTITUDE) {
+				// Need to init because last loop iteration was in a different mode
+				_hold_alt = _current_altitude;
+				mavlink_log_critical(&_mavlink_log_pub, "Start loiter with fixed bank angle.\t");
+				events::send(events::ID("fixedwing_position_control_fb_loiter"), events::Log::Critical,
+					     "Start loiter with fixed bank angle");
+			}
+
+			_control_mode_current = FW_POSCTRL_MODE_AUTO_ALTITUDE;
+
+		} else {
+			if (_control_mode_current != FW_POSCTRL_MODE_AUTO_CLIMBRATE && !_vehicle_status.in_transition_mode) {
+				mavlink_log_critical(&_mavlink_log_pub, "Start descending.\t");
+				events::send(events::ID("fixedwing_position_control_descend"), events::Log::Critical, "Start descending");
+			}
+
+			_control_mode_current = FW_POSCTRL_MODE_AUTO_CLIMBRATE;
+		}
+
+
+	} else if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_position_enabled) {
+		if (_control_mode_current != FW_POSCTRL_MODE_MANUAL_POSITION) {
 			/* Need to init because last loop iteration was in a different mode */
 			_hold_alt = _current_altitude;
 			_hdg_hold_yaw = _yaw; // yaw is not controlled, so set setpoint to current yaw
@@ -696,15 +725,16 @@ FixedwingPositionControl::set_control_mode_current(bool pos_sp_curr_valid)
 			_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 		}
 
-		_control_mode_current = FW_POSCTRL_MODE_POSITION;
+		_control_mode_current = FW_POSCTRL_MODE_MANUAL_POSITION;
 
-	} else if (_control_mode.flag_control_altitude_enabled) {
+	} else if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_altitude_enabled) {
-		if (_control_mode_current != FW_POSCTRL_MODE_POSITION && _control_mode_current != FW_POSCTRL_MODE_ALTITUDE) {
+		if (_control_mode_current != FW_POSCTRL_MODE_MANUAL_POSITION
+		    && _control_mode_current != FW_POSCTRL_MODE_MANUAL_ALTITUDE) {
 			/* Need to init because last loop iteration was in a different mode */
 			_hold_alt = _current_altitude;
 		}
 
-		_control_mode_current = FW_POSCTRL_MODE_ALTITUDE;
+		_control_mode_current = FW_POSCTRL_MODE_MANUAL_ALTITUDE;
 
 	} else {
 		_control_mode_current = FW_POSCTRL_MODE_OTHER;
@@ -724,9 +754,6 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 	_att_sp.fw_control_yaw = false;		// by default we don't want yaw to be contoller directly with rudder
 	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;		// by default we don't use flaps
 
-	/* no throttle limit as default */
-	float throttle_max = 1.0f;
-
 	/* save time when airplane is in air */
 	if (!_was_in_air && !_landed) {
 		_was_in_air = true;
@@ -767,11 +794,9 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 		publishOrbitStatus(pos_sp_curr);
 	}
 
-	const uint8_t position_sp_type = handle_setpoint_type(pos_sp_curr.type, pos_sp_curr);
+	_position_sp_type = handle_setpoint_type(pos_sp_curr.type, pos_sp_curr);
 
-	_type = position_sp_type; // for publication
+	switch (_position_sp_type) {
-
-	switch (position_sp_type) {
 	case position_setpoint_s::SETPOINT_TYPE_IDLE:
 		_att_sp.thrust_body[0] = 0.0f;
 		_att_sp.roll_body = 0.0f;
@@ -796,12 +821,12 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 	}
 
 	/* reset landing state */
-	if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_LAND) {
+	if (_position_sp_type != position_setpoint_s::SETPOINT_TYPE_LAND) {
 		reset_landing_state();
 	}
 
 	/* reset takeoff/launch state */
-	if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
+	if (_position_sp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
 		reset_takeoff_state();
 	}
 
@@ -823,7 +848,7 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 	} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
 		   _runway_takeoff.runwayTakeoffEnabled()) {
 
-		_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, min(get_tecs_thrust(), throttle_max));
+		_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, get_tecs_thrust());
 
 	} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
 
@@ -833,10 +858,10 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 		/* Copy thrust and pitch values from tecs */
 		if (_landed) {
 			// when we are landed state we want the motor to spin at idle speed
-			_att_sp.thrust_body[0] = min(_param_fw_thr_idle.get(), throttle_max);
+			_att_sp.thrust_body[0] = min(_param_fw_thr_idle.get(), 1.f);
 
 		} else {
-			_att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max);
+			_att_sp.thrust_body[0] = get_tecs_thrust();
 		}
 	}
 
@@ -854,8 +879,68 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 	if (use_tecs_pitch) {
 		_att_sp.pitch_body = get_tecs_pitch();
 	}
+}
 
-	_last_manual = !_control_mode.flag_control_position_enabled;
+void
+FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const hrt_abstime &now)
+{
+	// only control altitude and airspeed ("fixed-bank loiter")
+
+	tecs_update_pitch_throttle(now, _hold_alt,
+				   _param_fw_airspd_trim.get(),
+				   radians(_param_fw_p_lim_min.get()),
+				   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());
+
+	_att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
+	_att_sp.yaw_body = 0.f;
+
+	if (_landed) {
+		_att_sp.thrust_body[0] = _param_fw_thr_min.get();
+
+	} else {
+		_att_sp.thrust_body[0] = min(get_tecs_thrust(), _param_fw_thr_max.get());
+	}
+
+	_att_sp.pitch_body = get_tecs_pitch();
+}
+
+void
+FixedwingPositionControl::control_auto_descend(const hrt_abstime &now)
+{
+	// only control height rate
+
+	// Hard-code descend rate to 0.5m/s. This is a compromise to give the system to recover,
+	// but not letting it drift too far away.
+	const float descend_rate = -0.5f;
+
+	tecs_update_pitch_throttle(now, _hold_alt,
+				   _param_fw_airspd_trim.get(),
+				   radians(_param_fw_p_lim_min.get()),
+				   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(),
+				   tecs_status_s::TECS_MODE_NORMAL,
+				   descend_rate);
+
+	_att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
+	_att_sp.yaw_body = 0.f;
+
+	if (_landed) {
+		_att_sp.thrust_body[0] = _param_fw_thr_min.get();
+
+	} else {
+		_att_sp.thrust_body[0] = min(get_tecs_thrust(), _param_fw_thr_max.get());
+	}
+
+	_att_sp.pitch_body = get_tecs_pitch();
 }
 
 uint8_t
@@ -927,6 +1012,11 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
 		}
 	}
 
+	// set to type loiter during VTOL transitions in Land mode (to not start FW landing logic)
+	if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND && _vehicle_status.in_transition_mode) {
+		position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER;
+	}
+
 	return position_sp_type;
 }
 
@@ -1641,7 +1731,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
 }
 
 void
-FixedwingPositionControl::control_altitude(const hrt_abstime &now, const Vector2d &curr_pos,
+FixedwingPositionControl::control_manual_altitude(const hrt_abstime &now, const Vector2d &curr_pos,
 		const Vector2f &ground_speed)
 {
 	/* ALTITUDE CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
@@ -1693,12 +1783,10 @@ FixedwingPositionControl::control_altitude(const hrt_abstime &now, const Vector2
 	}
 
 	_att_sp.pitch_body = get_tecs_pitch();
-
-	_last_manual = !_control_mode.flag_control_position_enabled;
 }
 
 void
-FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2d &curr_pos,
+FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const Vector2d &curr_pos,
 		const Vector2f &ground_speed)
 {
 	const float dt = math::constrain((now - _control_position_last_called) * 1e-6f, 0.01f, 0.05f);
@@ -1813,8 +1901,6 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 	}
 
 	_att_sp.pitch_body = get_tecs_pitch();
-
-	_last_manual = !_control_mode.flag_control_position_enabled;
 }
 
 float
@@ -1832,7 +1918,7 @@ float
 FixedwingPositionControl::get_tecs_thrust()
 {
 	if (_is_tecs_running) {
-		return _tecs.get_throttle_setpoint();
+		return min(_tecs.get_throttle_setpoint(), 1.f);
 	}
 
 	// return 0 to prevent stale tecs state when it's not running
@@ -1959,7 +2045,7 @@ FixedwingPositionControl::Run()
 		Vector2d curr_pos(_current_latitude, _current_longitude);
 		Vector2f ground_speed(_local_pos.vx, _local_pos.vy);
 
-		set_control_mode_current(_pos_sp_triplet.current.valid);
+		set_control_mode_current(_local_pos.timestamp, _pos_sp_triplet.current.valid);
 
 		switch (_control_mode_current) {
 		case FW_POSCTRL_MODE_AUTO: {
@@ -1968,13 +2054,23 @@ FixedwingPositionControl::Run()
 				break;
 			}
 
-		case FW_POSCTRL_MODE_POSITION: {
+		case FW_POSCTRL_MODE_AUTO_ALTITUDE: {
-				control_position(_local_pos.timestamp, curr_pos, ground_speed);
+				control_auto_fixed_bank_alt_hold(_local_pos.timestamp);
 				break;
 			}
 
-		case FW_POSCTRL_MODE_ALTITUDE: {
+		case FW_POSCTRL_MODE_AUTO_CLIMBRATE: {
-				control_altitude(_local_pos.timestamp, curr_pos, ground_speed);
+				control_auto_descend(_local_pos.timestamp);
+				break;
+			}
+
+		case FW_POSCTRL_MODE_MANUAL_POSITION: {
+				control_manual_position(_local_pos.timestamp, curr_pos, ground_speed);
+				break;
+			}
+
+		case FW_POSCTRL_MODE_MANUAL_ALTITUDE: {
+				control_manual_altitude(_local_pos.timestamp, curr_pos, ground_speed);
 				break;
 			}
 
@@ -2008,7 +2104,8 @@ FixedwingPositionControl::Run()
 			if (_control_mode.flag_control_position_enabled ||
 			    _control_mode.flag_control_velocity_enabled ||
 			    _control_mode.flag_control_acceleration_enabled ||
-			    _control_mode.flag_control_altitude_enabled) {
+			    _control_mode.flag_control_altitude_enabled ||
+			    _control_mode.flag_control_climb_rate_enabled) {
 
 				const Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
 				q.copyTo(_att_sp.q_d);
@@ -2025,6 +2122,7 @@ FixedwingPositionControl::Run()
 			}
 
 			_l1_control.reset_has_guidance_updated();
+			_last_manual = !_control_mode.flag_control_position_enabled;
 		}
 
 		perf_end(_loop_perf);
@@ -2168,7 +2266,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
 	bool in_air_alt_control = (!_landed &&
 				   (_control_mode.flag_control_auto_enabled ||
 				    _control_mode.flag_control_velocity_enabled ||
-				    _control_mode.flag_control_altitude_enabled));
+				    _control_mode.flag_control_altitude_enabled ||
+				    _control_mode.flag_control_climb_rate_enabled));
 
 	/* update TECS vehicle state estimates */
 	_tecs.update_vehicle_state_estimates(_airspeed, _body_acceleration(0), (_local_pos.timestamp > 0), in_air_alt_control,

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -257,14 +257,18 @@ private:
 	float _manual_control_setpoint_altitude{0.0f};
 	float _manual_control_setpoint_airspeed{0.0f};
 
+	hrt_abstime _time_in_fixed_bank_loiter{0};
+
 	ECL_L1_Pos_Controller	_l1_control;
 	TECS			_tecs;
 
-	uint8_t _type{0};
+	uint8_t _position_sp_type{0};
 	enum FW_POSCTRL_MODE {
 		FW_POSCTRL_MODE_AUTO,
-		FW_POSCTRL_MODE_POSITION,
+		FW_POSCTRL_MODE_AUTO_ALTITUDE,
-		FW_POSCTRL_MODE_ALTITUDE,
+		FW_POSCTRL_MODE_AUTO_CLIMBRATE,
+		FW_POSCTRL_MODE_MANUAL_POSITION,
+		FW_POSCTRL_MODE_MANUAL_ALTITUDE,
 		FW_POSCTRL_MODE_OTHER
 	} _control_mode_current{FW_POSCTRL_MODE_OTHER};		///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.
 
@@ -327,6 +331,10 @@ private:
 	void		control_auto(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
 				     const position_setpoint_s &pos_sp_prev,
 				     const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
+
+	void		control_auto_fixed_bank_alt_hold(const hrt_abstime &now);
+	void		control_auto_descend(const hrt_abstime &now);
+
 	void		control_auto_position(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
 					      const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
 	void		control_auto_loiter(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
@@ -338,8 +346,8 @@ private:
 	void		control_auto_landing(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
 					     const position_setpoint_s &pos_sp_prev,
 					     const position_setpoint_s &pos_sp_curr);
-	void		control_altitude(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed);
+	void		control_manual_altitude(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed);
-	void		control_position(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed);
+	void		control_manual_position(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed);
 
 	float		get_tecs_pitch();
 	float		get_tecs_thrust();
@@ -350,7 +358,7 @@ private:
 	void		reset_takeoff_state(bool force = false);
 	void		reset_landing_state();
 	Vector2f 	get_nav_speed_2d(const Vector2f &ground_speed);
-	void		set_control_mode_current(bool pos_sp_curr_valid);
+	void		set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid);
 
 	void publishOrbitStatus(const position_setpoint_s pos_sp);
 
@@ -424,6 +432,10 @@ private:
 
 		(ParamBool<px4::params::FW_POSCTL_INV_ST>) _param_fw_posctl_inv_st,
 
+
+		(ParamInt<px4::params::FW_GPSF_LT>) _param_nav_gpsf_lt,
+		(ParamFloat<px4::params::FW_GPSF_R>) _param_nav_gpsf_r,
+
 		// external parameters
 		(ParamInt<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,
 

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -825,3 +825,32 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_R_SP, 3.0f);
  * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_SINK_R_SP, 2.0f);
+
+/**
+ * GPS failure loiter time
+ *
+ * The time in seconds the system should do open loop loiter and wait for GPS recovery
+ * before it starts descending. Set to 0 to disable. Roll angle is set to FW_GPSF_R.
+ * Does only apply for fixed-wing vehicles or VTOLs with NAV_FORCE_VT set to 0.
+ *
+ * @unit s
+ * @min 0
+ * @max 3600
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(FW_GPSF_LT, 30);
+
+/**
+ * GPS failure fixed roll angle
+ *
+ * Roll in degrees during the loiter after the vehicle has lost GPS in an auto mode (e.g. mission or loiter).
+ * Does only apply for fixed-wing vehicles or VTOLs with NAV_FORCE_VT set to 0.
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 30.0
+ * @decimal 1
+ * @increment 0.5
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(FW_GPSF_R, 15.0f);