FW auto-trimming

msg/AutoTrimStatus.msg

@@ -0,0 +1,18 @@
+# Auto trimming state and current estimate
+uint64 timestamp             # Time since system start (microseconds)
+uint64 timestamp_sample
+
+float32[3] trim_estimate     # Roll/pitch/yaw trim value obtained from torque setpoints averaged over several seconds
+float32[3] trim_estimate_var
+float32[3] trim_test         # Same as trim_estimate but done on a shorter period. Used to validate the trim estimate.
+float32[3] trim_test_var
+float32[3] trim_validated    # Final roll/pitch/yaw trim estimate, verified by trim_test
+
+uint8 STATE_IDLE = 0
+uint8 STATE_SAMPLING = 1
+uint8 STATE_SAMPLING_TEST = 2
+uint8 STATE_VERIFICATION = 3
+uint8 STATE_COMPLETE = 4
+uint8 STATE_FAIL = 5
+
+uint8 state

msg/CMakeLists.txt

@@ -52,6 +52,7 @@ set(msg_files
 	ArmingCheckReply.msg
 	ArmingCheckRequest.msg
 	AutotuneAttitudeControlStatus.msg
+	AutoTrimStatus.msg
 	BatteryStatus.msg
 	Buffer128.msg
 	ButtonEvent.msg

src/lib/mathlib/math/WelfordMeanVector.hpp

@@ -39,6 +39,8 @@
 
 #pragma once
 
+#include <lib/mathlib/mathlib.h>
+
 namespace math
 {
 

src/lib/slew_rate/SlewRate.hpp

@@ -56,13 +56,13 @@ class SlewRate
 	 * Set maximum rate of change for the value
 	 * @param slew_rate maximum rate of change
 	 */
-	void setSlewRate(const Type slew_rate) { _slew_rate = slew_rate; }
+	void setSlewRate(const Type &slew_rate) { _slew_rate = slew_rate; }
 
 	/**
 	 * Set value ignoring slew rate for initialization purpose
 	 * @param value new applied value
 	 */
-	void setForcedValue(const Type value) { _value = value; }
+	void setForcedValue(const Type &value) { _value = value; }
 
 	/**
 	 * Get value from last update of the slew rate
@@ -76,7 +76,7 @@ class SlewRate
 	 * @param deltatime time in seconds since last update
 	 * @return actual value that complies with the slew rate
 	 */
-	Type update(const Type new_value, const float deltatime)
+	Type update(const Type &new_value, const float deltatime)
 	{
 		// Limit the rate of change of the value
 		const Type dvalue_desired = new_value - _value;
@@ -90,3 +90,14 @@ class SlewRate
 	Type _slew_rate{}; ///< maximum rate of change for the value
 	Type _value{}; ///< state to keep last value of the slew rate
 };
+
+template<>
+inline matrix::Vector3f SlewRate<matrix::Vector3f>::update(const matrix::Vector3f &new_value, const float deltatime)
+{
+	// Limit the rate of change of the value
+	const matrix::Vector3f dvalue_desired = new_value - _value;
+	const matrix::Vector3f dvalue_max = _slew_rate * deltatime;
+	const matrix::Vector3f dvalue = matrix::constrain(dvalue_desired, -dvalue_max, dvalue_max);
+	_value += dvalue;
+	return _value;
+}

src/modules/fw_rate_control/CMakeLists.txt

@@ -30,6 +30,7 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
+add_subdirectory(fw_trim)
 
 px4_add_module(
 	MODULE modules__fw_rate_control
@@ -38,6 +39,7 @@ px4_add_module(
 		FixedwingRateControl.cpp
 		FixedwingRateControl.hpp
 	DEPENDS
+		FwTrim
 		px4_work_queue
 		RateControl
 		SlewRate

src/modules/fw_rate_control/FixedwingRateControl.cpp

@@ -37,7 +37,6 @@ using namespace time_literals;
 using namespace matrix;
 
 using math::constrain;
-using math::interpolate;
 using math::radians;
 
 FixedwingRateControl::FixedwingRateControl(bool vtol) :
@@ -54,6 +53,7 @@ FixedwingRateControl::FixedwingRateControl(bool vtol) :
 	parameters_update();
 
 	_rate_ctrl_status_pub.advertise();
+	_trim_slew.setSlewRate(Vector3f(0.01f, 0.01f, 0.01f));
 }
 
 FixedwingRateControl::~FixedwingRateControl()
@@ -96,6 +96,12 @@ FixedwingRateControl::parameters_update()
 	return PX4_OK;
 }
 
+void
+FixedwingRateControl::save_params()
+{
+	_trim.saveParams();
+}
+
 void
 FixedwingRateControl::vehicle_manual_poll()
 {
@@ -127,13 +133,14 @@ FixedwingRateControl::vehicle_manual_poll()
 
 			} else {
 				// Manual/direct control, filled in FW-frame. Note that setpoints will get transformed to body frame prior publishing.
+				const Vector3f trim = _trim_slew.getState();
 
 				_vehicle_torque_setpoint.xyz[0] = math::constrain(_manual_control_setpoint.roll * _param_fw_man_r_sc.get() +
-								  _param_trim_roll.get(), -1.f, 1.f);
+								  trim(0), -1.f, 1.f);
 				_vehicle_torque_setpoint.xyz[1] = math::constrain(-_manual_control_setpoint.pitch * _param_fw_man_p_sc.get() +
-								  _param_trim_pitch.get(), -1.f, 1.f);
+								  trim(1), -1.f, 1.f);
 				_vehicle_torque_setpoint.xyz[2] = math::constrain(_manual_control_setpoint.yaw * _param_fw_man_y_sc.get() +
-								  _param_trim_yaw.get(), -1.f, 1.f);
+								  trim(2), -1.f, 1.f);
 
 				_vehicle_thrust_setpoint.xyz[0] = math::constrain((_manual_control_setpoint.throttle + 1.f) * .5f, 0.f, 1.f);
 			}
@@ -256,7 +263,14 @@ void FixedwingRateControl::Run()
 		const bool is_fixed_wing = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
 		_in_fw_or_transition_wo_tailsitter_transition =  is_fixed_wing || is_in_transition_except_tailsitter;
 
-		_vehicle_control_mode_sub.update(&_vcontrol_mode);
+		{
+			const bool armed_prev = _vcontrol_mode.flag_armed;
+			_vehicle_control_mode_sub.update(&_vcontrol_mode);
+
+			if (!_vcontrol_mode.flag_armed && armed_prev) {
+				save_params();
+			}
+		}
 
 		vehicle_land_detected_poll();
 
@@ -269,9 +283,11 @@ void FixedwingRateControl::Run()
 			return;
 		}
 
+		const float airspeed = get_airspeed_and_update_scaling();
+		_trim.setAirspeed(airspeed);
+
 		if (_vcontrol_mode.flag_control_rates_enabled) {
 
-			const float airspeed = get_airspeed_and_update_scaling();
 
 			/* reset integrals where needed */
 			if (_rates_sp.reset_integral) {
@@ -321,78 +337,53 @@ void FixedwingRateControl::Run()
 				}
 			}
 
-			/* bi-linear interpolation over airspeed for actuator trim scheduling */
-			Vector3f trim(_param_trim_roll.get(), _param_trim_pitch.get(), _param_trim_yaw.get());
+			_rates_sp_sub.update(&_rates_sp);
 
-			if (airspeed < _param_fw_airspd_trim.get()) {
-				trim(0) += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(),
-						       _param_fw_dtrim_r_vmin.get(),
-						       0.0f);
-				trim(1) += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(),
-						       _param_fw_dtrim_p_vmin.get(),
-						       0.0f);
-				trim(2) += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(),
-						       _param_fw_dtrim_y_vmin.get(),
-						       0.0f);
+			Vector3f body_rates_setpoint = Vector3f(_rates_sp.roll, _rates_sp.pitch, _rates_sp.yaw);
 
-			} else {
-				trim(0) += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
-						       _param_fw_dtrim_r_vmax.get());
-				trim(1) += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
-						       _param_fw_dtrim_p_vmax.get());
-				trim(2) += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
-						       _param_fw_dtrim_y_vmax.get());
+			// Tailsitter: rotate setpoint from hover to fixed-wing frame (controller is in fixed-wing frame, interface in hover)
+			if (_vehicle_status.is_vtol_tailsitter) {
+				body_rates_setpoint = Vector3f(-_rates_sp.yaw, _rates_sp.pitch, _rates_sp.roll);
 			}
 
-			if (_vcontrol_mode.flag_control_rates_enabled) {
-				_rates_sp_sub.update(&_rates_sp);
-
-				Vector3f body_rates_setpoint = Vector3f(_rates_sp.roll, _rates_sp.pitch, _rates_sp.yaw);
-
-				// Tailsitter: rotate setpoint from hover to fixed-wing frame (controller is in fixed-wing frame, interface in hover)
-				if (_vehicle_status.is_vtol_tailsitter) {
-					body_rates_setpoint = Vector3f(-_rates_sp.yaw, _rates_sp.pitch, _rates_sp.roll);
-				}
-
-				// Run attitude RATE controllers which need the desired attitudes from above, add trim.
-				const Vector3f angular_acceleration_setpoint = _rate_control.update(rates, body_rates_setpoint, angular_accel, dt,
-						_landed);
+			// Run attitude RATE controllers which need the desired attitudes from above, add trim.
+			const Vector3f angular_acceleration_setpoint = _rate_control.update(rates, body_rates_setpoint, angular_accel, dt,
+					_landed);
 
-				const Vector3f gain_ff(_param_fw_rr_ff.get(), _param_fw_pr_ff.get(), _param_fw_yr_ff.get());
-				const Vector3f feedforward = gain_ff.emult(body_rates_setpoint) * _airspeed_scaling;
+			const Vector3f gain_ff(_param_fw_rr_ff.get(), _param_fw_pr_ff.get(), _param_fw_yr_ff.get());
+			const Vector3f feedforward = gain_ff.emult(body_rates_setpoint) * _airspeed_scaling;
 
-				Vector3f control_u = angular_acceleration_setpoint * _airspeed_scaling * _airspeed_scaling + feedforward;
+			Vector3f control_u = angular_acceleration_setpoint * _airspeed_scaling * _airspeed_scaling + feedforward;
 
-				// Special case yaw in Acro: if the parameter FW_ACRO_YAW_CTL is not set then don't control yaw
-				if (!_vcontrol_mode.flag_control_attitude_enabled && !_param_fw_acro_yaw_en.get()) {
-					control_u(2) = _manual_control_setpoint.yaw * _param_fw_man_y_sc.get();
-					_rate_control.resetIntegral(2);
-				}
+			// Special case yaw in Acro: if the parameter FW_ACRO_YAW_CTL is not set then don't control yaw
+			if (!_vcontrol_mode.flag_control_attitude_enabled && !_param_fw_acro_yaw_en.get()) {
+				control_u(2) = _manual_control_setpoint.yaw * _param_fw_man_y_sc.get();
+				_rate_control.resetIntegral(2);
+			}
 
-				if (control_u.isAllFinite()) {
-					matrix::constrain(control_u + trim, -1.f, 1.f).copyTo(_vehicle_torque_setpoint.xyz);
+			if (control_u.isAllFinite()) {
+				matrix::constrain(control_u + _trim_slew.getState(), -1.f, 1.f).copyTo(_vehicle_torque_setpoint.xyz);
 
-				} else {
-					_rate_control.resetIntegral();
-					trim.copyTo(_vehicle_torque_setpoint.xyz);
-				}
+			} else {
+				_rate_control.resetIntegral();
+				_trim_slew.getState().copyTo(_vehicle_torque_setpoint.xyz);
+			}
 
-				/* throttle passed through if it is finite */
-				_vehicle_thrust_setpoint.xyz[0] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ? _rates_sp.thrust_body[0] : 0.0f;
+			/* throttle passed through if it is finite */
+			_vehicle_thrust_setpoint.xyz[0] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ? _rates_sp.thrust_body[0] : 0.0f;
 
-				/* scale effort by battery status */
-				if (_param_fw_bat_scale_en.get() && _vehicle_thrust_setpoint.xyz[0] > 0.1f) {
+			/* scale effort by battery status */
+			if (_param_fw_bat_scale_en.get() && _vehicle_thrust_setpoint.xyz[0] > 0.1f) {
 
-					if (_battery_status_sub.updated()) {
-						battery_status_s battery_status{};
+				if (_battery_status_sub.updated()) {
+					battery_status_s battery_status{};
 
-						if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) {
-							_battery_scale = battery_status.scale;
-						}
+					if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) {
+						_battery_scale = battery_status.scale;
 					}
-
-					_vehicle_thrust_setpoint.xyz[0] *= _battery_scale;
 				}
+
+				_vehicle_thrust_setpoint.xyz[0] *= _battery_scale;
 			}
 
 			// publish rate controller status
@@ -432,6 +423,9 @@ void FixedwingRateControl::Run()
 				_vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
 				_vehicle_torque_setpoint_pub.publish(_vehicle_torque_setpoint);
 			}
+
+			_trim.updateAutoTrim(Vector3f(_vehicle_torque_setpoint.xyz), dt);
+			_trim_slew.update(_trim.getTrim(), dt);
 		}
 
 		updateActuatorControlsStatus(dt);

src/modules/fw_rate_control/FixedwingRateControl.hpp

@@ -69,6 +69,8 @@
 #include <uORB/topics/vehicle_thrust_setpoint.h>
 #include <uORB/topics/vehicle_torque_setpoint.h>
 
+#include "fw_trim/FwTrim.hpp"
+
 using matrix::Eulerf;
 using matrix::Quatf;
 
@@ -170,13 +172,6 @@ class FixedwingRateControl final : public ModuleBase<FixedwingRateControl>, publ
 
 		(ParamBool<px4::params::FW_BAT_SCALE_EN>) _param_fw_bat_scale_en,
 
-		(ParamFloat<px4::params::FW_DTRIM_P_VMAX>) _param_fw_dtrim_p_vmax,
-		(ParamFloat<px4::params::FW_DTRIM_P_VMIN>) _param_fw_dtrim_p_vmin,
-		(ParamFloat<px4::params::FW_DTRIM_R_VMAX>) _param_fw_dtrim_r_vmax,
-		(ParamFloat<px4::params::FW_DTRIM_R_VMIN>) _param_fw_dtrim_r_vmin,
-		(ParamFloat<px4::params::FW_DTRIM_Y_VMAX>) _param_fw_dtrim_y_vmax,
-		(ParamFloat<px4::params::FW_DTRIM_Y_VMIN>) _param_fw_dtrim_y_vmin,
-
 		(ParamFloat<px4::params::FW_MAN_P_SC>) _param_fw_man_p_sc,
 		(ParamFloat<px4::params::FW_MAN_R_SC>) _param_fw_man_r_sc,
 		(ParamFloat<px4::params::FW_MAN_Y_SC>) _param_fw_man_y_sc,
@@ -200,22 +195,23 @@ class FixedwingRateControl final : public ModuleBase<FixedwingRateControl>, publ
 		(ParamFloat<px4::params::FW_YR_P>) _param_fw_yr_p,
 		(ParamFloat<px4::params::FW_YR_D>) _param_fw_yr_d,
 
-		(ParamFloat<px4::params::TRIM_PITCH>) _param_trim_pitch,
-		(ParamFloat<px4::params::TRIM_ROLL>) _param_trim_roll,
-		(ParamFloat<px4::params::TRIM_YAW>) _param_trim_yaw,
-
 		(ParamInt<px4::params::FW_SPOILERS_MAN>) _param_fw_spoilers_man
 	)
 
 	RateControl _rate_control; ///< class for rate control calculations
 
+	FwTrim _trim{this};
+	SlewRate<matrix::Vector3f> _trim_slew{}; ///< prevents large trim changes
+
 	void updateActuatorControlsStatus(float dt);
 
 	/**
 	 * Update our local parameter cache.
 	 */
 	int		parameters_update();
 
+	void		save_params();
+
 	void		vehicle_manual_poll();
 	void		vehicle_land_detected_poll();
 

src/modules/fw_rate_control/fw_rate_control_params.c

@@ -295,6 +295,21 @@ PARAM_DEFINE_INT32(FW_BAT_SCALE_EN, 0);
  */
 PARAM_DEFINE_INT32(FW_ARSP_SCALE_EN, 1);
 
+/**
+ * Auto-Trim mode
+ *
+ * In calibration mode, the estimated trim is used to set the TRIM_ROLL/PITCH/YAW
+ * parameters after landing. The parameter is then changed to continuous mode.
+ * In continuous mode, part of the auto-trim estimated
+ * during flight is used to update the existing trim.
+ *
+ * @group FW Rate Control
+ * @value 0 Disabled
+ * @value 1 Calibration
+ * @value 2 Continuous
+ */
+PARAM_DEFINE_INT32(FW_ATRIM_MODE, 1);
+
 /**
 * Roll trim increment at minimum airspeed
 *