FW Position Control: add Eco mode in TECS and separate Eco climbrate

Separate settings in Eco mode (beside airspeed):
- FW_T_CLMB_R_SP_E (slower climb for more efficiency)
- FW_T_ALT_TC_E (larger to reduce motor action due to altitude errors)
- FW_T_SPDWEIGHT_E (more weight to airspeed to reduce motor action due to altitude errors)

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

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -131,6 +131,9 @@ FixedwingPositionControl::parameters_update()
 	_tecs.set_speed_derivative_time_constant(_param_tas_rate_time_const.get());
 	_tecs.set_seb_rate_ff_gain(_param_seb_rate_ff.get());
 
+	_tecs.set_speed_weight_eco(_param_fw_t_spdweight_eco.get());
+	_tecs.set_height_error_time_constant_eco(_param_fw_t_h_error_tc_eco.get());
+
 
 	// Landing slope
 	/* check if negative value for 2/3 of flare altitude is set for throttle cut */
@@ -662,7 +665,8 @@ FixedwingPositionControl::getManualHeightRateSetpoint()
 	} else if (_manual_control_setpoint_altitude < - deadBand) {
 		/* pitching up */
 		float pitch = -(_manual_control_setpoint_altitude + deadBand) / factor;
-		const float climb_rate_target = _param_climbrate_target.get();
+		const float climb_rate_target = _speed_mode_current == FW_SPEED_MODE::FW_SPEED_MODE_ECO ?
+						_param_fw_t_clmb_r_sp_eco.get() : _param_climbrate_target.get();
 
 		height_rate_setpoint = pitch * climb_rate_target;
 	}
@@ -2261,6 +2265,9 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
 		}
 	}
 
+	const float climb_rate_target = _speed_mode_current == FW_SPEED_MODE::FW_SPEED_MODE_ECO ?
+					_param_fw_t_clmb_r_sp_eco.get() : _param_climbrate_target.get();
+
 	_tecs.update_pitch_throttle(_pitch - radians(_param_fw_psp_off.get()),
 				    _current_altitude, alt_sp,
 				    airspeed_sp, _airspeed, _eas2tas,
@@ -2269,7 +2276,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
 				    throttle_min, throttle_max, throttle_cruise,
 				    pitch_min_rad - radians(_param_fw_psp_off.get()),
 				    pitch_max_rad - radians(_param_fw_psp_off.get()),
-				    _param_climbrate_target.get(), _param_sinkrate_target.get(), hgt_rate_sp);
+				    climb_rate_target, _param_sinkrate_target.get(), hgt_rate_sp,
+				    _speed_mode_current == FW_SPEED_MODE::FW_SPEED_MODE_ECO);
 }
 
 void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_sp)

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -471,7 +471,10 @@ private:
 		(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
+		(ParamFloat<px4::params::FW_ALT_ERR_O>) fw_alt_err_o,
+		(ParamFloat<px4::params::FW_T_ALT_TC_E>) _param_fw_t_h_error_tc_eco,
+		(ParamFloat<px4::params::FW_T_SPDWEIGHT_E>) _param_fw_t_spdweight_eco,
+		(ParamFloat<px4::params::FW_T_CLMB_R_SP_E>) _param_fw_t_clmb_r_sp_eco
 	)
 };
 

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -928,3 +928,40 @@ PARAM_DEFINE_FLOAT(FW_ALT_ERR_O, 20.f);
  * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_ALT_MIN, 50.f);
+
+/**
+ * Eco Mode: Speed <--> Altitude priority
+ *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 1
+ * @increment 1.0
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT_E, 1.8f);
+
+/**
+ * Eco Mode: Altitude error time constant.
+ *
+ * Normally larger than FW_T_ALT_TC to have looser altitude control and in turn less throttle changes.
+ *
+ * @min 2.0
+ * @decimal 1
+ * @increment 0.5
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_ALT_TC_E, 10.0f);
+
+/**
+ * Eco target climbrate.
+ *
+ * The rate at which the vehicle will climb in autonomous modes to achieve altitude setpoints in Eco mode.
+ *
+ * @unit m/s
+ * @min 0.5
+ * @max 15
+ * @decimal 2
+ * @increment 0.01
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_CLMB_R_SP_E, 2.0f);