FW Position Control: add logic for speed modes Eco and Dash

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

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -399,6 +399,77 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand, const
 	return constrain(airspeed_demand, adjusted_min_airspeed, _param_fw_airspd_max.get());
 }
 
+void
+FixedwingPositionControl::updateSpeedMode()
+{
+	FW_SPEED_MODE_COMMANDED new_mode = static_cast<FW_SPEED_MODE_COMMANDED>(_fw_spd_mode_set.get());
+
+	switch (new_mode) {
+	case NORMAL:
+		_speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; //default
+		break;
+
+	case ECO_CRUISE:
+		if (_conditions_for_eco_dash_met && _tecs.get_flight_phase() == tecs_status_s::TECS_FLIGHT_PHASE_LEVEL) {
+			_speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_ECO;
+
+		} else {
+			_speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL;
+		}
+
+		break;
+
+	case ECO_FULL:
+		if (_conditions_for_eco_dash_met) {
+			_speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_ECO;
+
+		} else {
+			_speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL;
+		}
+
+		break;
+
+	case DASH_CRUISE:
+		if (_conditions_for_eco_dash_met && _tecs.get_flight_phase() == tecs_status_s::TECS_FLIGHT_PHASE_LEVEL) {
+			_speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_DASH;
+
+		} else {
+			_speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL;
+		}
+
+		break;
+
+	case DASH_FULL:
+		if (_conditions_for_eco_dash_met) {
+			_speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_DASH;
+
+		} else {
+			_speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL;
+		}
+
+		break;
+	}
+}
+
+void
+FixedwingPositionControl::check_eco_dash_allowed()
+{
+	const float altitude_amsl_min = max(_tecs.get_hgt_setpoint() - fw_alt_err_u.get(),
+					    _local_pos.ref_alt + fw_alt_min.get());
+	const float altitude_amsl_max = _tecs.get_hgt_setpoint() + fw_alt_err_o.get();
+
+	const bool altitdue_conditions_met = _current_altitude <= altitude_amsl_max
+					     && _current_altitude >= altitude_amsl_min;
+
+	// add a 10s timeout after conditions where not met
+	_conditions_for_eco_dash_met = altitdue_conditions_met && hrt_elapsed_time(&_time_conditions_not_met) > 10_s;
+
+
+	if (!altitdue_conditions_met) {
+		// reset timer
+		_time_conditions_not_met = _local_pos.timestamp;
+	}
+}
 
 void
 FixedwingPositionControl::update_wind_mode()
@@ -710,6 +781,8 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 	/* get circle mode */
 	const bool was_circle_mode = _l1_control.circle_mode();
 
+	check_eco_dash_allowed();
+	updateSpeedMode();
 	update_wind_mode();
 
 	/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -268,6 +268,23 @@ private:
 		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.
 
+	enum FW_SPEED_MODE {
+		FW_SPEED_MODE_NORMAL,
+		FW_SPEED_MODE_ECO,
+		FW_SPEED_MODE_DASH,
+	} _speed_mode_current{FW_SPEED_MODE_NORMAL};
+
+	enum FW_SPEED_MODE_COMMANDED {
+		NORMAL,
+		ECO_CRUISE,
+		ECO_FULL,
+		DASH_CRUISE,
+		DASH_FULL
+	} _speed_mode_setting{NORMAL};
+
+	bool _conditions_for_eco_dash_met{false};
+	hrt_abstime _time_conditions_not_met{0};
+
 	// wind state
 	hrt_abstime _first_time_current_mode_detected{0};		///< last time in normal wind mode
 
@@ -363,6 +380,8 @@ private:
 
 	void 		publishOrbitStatus(const position_setpoint_s pos_sp);
 
+	void		check_eco_dash_allowed();
+	void		updateSpeedMode();
 
 	void		update_wind_mode();
 
@@ -456,8 +475,11 @@ private:
 		(ParamFloat<px4::params::FW_WIND_THLD_L>) _param_fw_wind_thld_l,
 		(ParamFloat<px4::params::FW_WIND_ARSP_OF>) _param_fw_wind_arsp_of,
 
+		(ParamInt<px4::params::FW_SPD_MODE_SET>) _fw_spd_mode_set,
+		(ParamFloat<px4::params::FW_ALT_MIN>) fw_alt_min,
+		(ParamFloat<px4::params::FW_ALT_ERR_U>) fw_alt_err_u,
+		(ParamFloat<px4::params::FW_ALT_ERR_O>) fw_alt_err_o
 	)
-
 };
 
 #endif // FIXEDWINGPOSITIONCONTROL_HPP_

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -904,3 +904,59 @@ PARAM_DEFINE_FLOAT(FW_WIND_THLD_L, 0.0f);
  * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_WIND_ARSP_OF, 1.0f);
+
+/**
+ * FW Speed mode setting
+ *
+ * Setting
+ *
+ * @min 0
+ * @max 4
+ * @value 0 Normal
+ * @value 1 Eco cruise
+ * @value 2 Eco full
+ * @value 3 Dash cruise
+ * @value 4 Dash full
+ * @group FW TECS
+ */
+PARAM_DEFINE_INT32(FW_SPD_MODE_SET, 0);
+
+
+/**
+ * Max altitude undershoot for Eco/Dash
+ *
+ * Eco/Dash mode is disabled if the current altitude is more than this value below the setpoint.
+ *
+ * @min 1.0
+ * @max 50.0
+ * @decimal 1
+ * @increment 0.1
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_ALT_ERR_U, 10.f);
+
+/**
+ * Max altitude overshoot for Eco/Dash
+ *
+ * Eco/Dash mode is disabled if the current altitude is more than this value above the setpoint.
+ *
+ * @min 1.0
+ * @max 50.0
+ * @decimal 1
+ * @increment 0.1
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_ALT_ERR_O, 20.f);
+
+/**
+ * Min altitude for Eco/Dash
+ *
+ * Eco/Dash mode can be enabled if above this relative altitude to home.
+ *
+ * @min -200.0
+ * @max 200.0
+ * @decimal 1
+ * @increment 0.1
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_ALT_MIN, 50.f);