slew_rate: pass vector by const ref

Same as outer if

boards/px4/fmu-v5x/default.px4board

@@ -53,7 +53,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
-CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE=n
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y

boards/px4/fmu-v6x/default.px4board

@@ -13,7 +13,7 @@ CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y
 CONFIG_DRIVERS_BAROMETER_MS5611=y
 CONFIG_DRIVERS_CAMERA_CAPTURE=y
-CONFIG_DRIVERS_CAMERA_TRIGGER=y
+CONFIG_DRIVERS_CAMERA_TRIGGER=n
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DSHOT=y

msg/AutoTrimStatus.msg

@@ -0,0 +1,17 @@
+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 @@ public:
 	 * 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 @@ public:
 	 * @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 @@ protected:
 	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();
 
@@ -321,78 +335,56 @@ 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());
+			_trim.setAirspeed(airspeed);
+			_trim_slew.update(_trim.getTrim(), dt);
 
-			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);
+			_rates_sp_sub.update(&_rates_sp);
 
-			} 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());
+			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);
 			}
 
-			if (_vcontrol_mode.flag_control_rates_enabled) {
-				_rates_sp_sub.update(&_rates_sp);
+			// 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);
 
-				Vector3f body_rates_setpoint = Vector3f(_rates_sp.roll, _rates_sp.pitch, _rates_sp.yaw);
+			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;
 
-				// 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);
-				}
+			Vector3f control_u = angular_acceleration_setpoint * _airspeed_scaling * _airspeed_scaling + feedforward;
 
-				// 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);
+			// 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);
+			}
 
-				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;
+			if (control_u.isAllFinite()) {
+				matrix::constrain(control_u + _trim_slew.getState(), -1.f, 1.f).copyTo(_vehicle_torque_setpoint.xyz);
 
-				Vector3f control_u = angular_acceleration_setpoint * _airspeed_scaling * _airspeed_scaling + feedforward;
+			} else {
+				_rate_control.resetIntegral();
+				_trim_slew.getState().copyTo(_vehicle_torque_setpoint.xyz);
+			}
 
-				// 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);
-				}
+			/* 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;
 
-				if (control_u.isAllFinite()) {
-					matrix::constrain(control_u + trim, -1.f, 1.f).copyTo(_vehicle_torque_setpoint.xyz);
+			/* scale effort by battery status */
+			if (_param_fw_bat_scale_en.get() && _vehicle_thrust_setpoint.xyz[0] > 0.1f) {
 
-				} else {
-					_rate_control.resetIntegral();
-					trim.copyTo(_vehicle_torque_setpoint.xyz);
-				}
+				if (_battery_status_sub.updated()) {
+					battery_status_s battery_status{};
 
-				/* 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) {
-
-					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
@@ -405,6 +397,7 @@ void FixedwingRateControl::Run()
 		} else {
 			// full manual
 			_rate_control.resetIntegral();
+			_trim.reset();
 		}
 
 		// Add feed-forward from roll control output to yaw control output
@@ -432,6 +425,8 @@ 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);
 		}
 
 		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 @@ private:
 
 		(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,15 +195,14 @@ private:
 		(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);
 
 	/**
@@ -216,6 +210,8 @@ private:
 	 */
 	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 exitsting 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
 *

src/modules/fw_rate_control/fw_trim/CMakeLists.txt

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_library(FwTrim
+	FwTrim.cpp
+	FwAutoTrim.cpp
+)
+
+# target_link_libraries(FwTrim)
+target_include_directories(FwTrim PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/fw_rate_control/fw_trim/FwAutoTrim.cpp

@@ -0,0 +1,203 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "FwAutoTrim.hpp"
+
+#include <mathlib/mathlib.h>
+
+using namespace time_literals;
+using matrix::Vector3f;
+
+FwAutoTrim::FwAutoTrim(ModuleParams *parent) :
+	ModuleParams(parent)
+{
+	_auto_trim_status_pub.advertise();
+	updateParams();
+}
+
+FwAutoTrim::~FwAutoTrim()
+{
+	perf_free(_cycle_perf);
+}
+
+void FwAutoTrim::updateParams()
+{
+	ModuleParams::updateParams();
+}
+
+void FwAutoTrim::reset()
+{
+	_state = state::idle;
+	_trim_estimate.reset();
+	_trim_test.reset();
+	_trim_validated.zero();
+}
+
+void FwAutoTrim::update(const Vector3f &torque_sp, const float dt)
+{
+	if (_vehicle_land_detected_sub.updated()) {
+		vehicle_land_detected_s vehicle_land_detected;
+
+		if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
+			_landed = vehicle_land_detected.landed;
+		}
+	}
+
+	perf_begin(_cycle_perf);
+
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status;
+
+		if (_vehicle_status_sub.copy(&vehicle_status)) {
+			_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+			_fixed_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
+			_tailsitter = vehicle_status.is_vtol_tailsitter;
+		}
+	}
+
+	if (_airspeed_validated_sub.updated()) {
+		airspeed_validated_s airspeed_validated;
+
+		if (_airspeed_validated_sub.copy(&airspeed_validated)) {
+			if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)) {
+				_calibrated_airspeed_m_s = airspeed_validated.calibrated_airspeed_m_s;
+
+			} else {
+				_calibrated_airspeed_m_s = airspeed_validated.calibrated_ground_minus_wind_m_s;
+			}
+		}
+	}
+
+	if (_vehicle_attitude_sub.updated()) {
+		vehicle_attitude_s vehicle_attitude;
+
+		if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
+			const Vector3f earth_z_in_body_frame = matrix::Quatf(vehicle_attitude.q).rotateVectorInverse(Vector3f(0.f, 0.f, 1.f));
+			_cos_tilt = _tailsitter ? earth_z_in_body_frame(0) : earth_z_in_body_frame(2);
+		}
+	}
+
+	const hrt_abstime now = hrt_absolute_time();
+
+	const bool run_auto_trim = _fixed_wing
+				   && _armed
+				   && !_landed
+				   && (dt > 0.001f) && (dt < 0.1f)
+				   && torque_sp.isAllFinite()
+				   && ((_calibrated_airspeed_m_s >= _param_fw_airspd_min.get() && _calibrated_airspeed_m_s <= _param_fw_airspd_max.get())
+				       || (!PX4_ISFINITE(_calibrated_airspeed_m_s) && (_param_sys_has_num_aspd.get() == 0)))
+				   && _cos_tilt > cosf(math::radians(_kTiltMaxDeg));
+
+	if (run_auto_trim) {
+		switch (_state) {
+		default:
+
+		// fallthrough
+		case state::idle:
+			_trim_estimate.reset();
+			_trim_test.reset();
+			_state = state::sampling;
+			_state_start_time = now;
+			break;
+
+		case state::sampling:
+			// Average the torque setpoint over a long period of time
+			_trim_estimate.update(torque_sp);
+
+			if ((now - _state_start_time) > 5_s) {
+				_state = state::sampling_test;
+				_state_start_time = now;
+			}
+
+			break;
+
+		case state::sampling_test:
+			// Average a smaller amount of data to validate the trim estimate
+			_trim_test.update(torque_sp);
+
+			if ((now - _state_start_time) > 2_s) {
+				_state = state::verification;
+				_state_start_time = now;
+			}
+
+			break;
+
+		case state::verification: {
+				// Perform a statistical test between the estimated and test data
+				const float gate = 5.f;
+				const Vector3f innovation = _trim_test.mean() - _trim_estimate.mean();
+				const Vector3f innovation_variance = _trim_test.variance() + _trim_estimate.variance();
+				const float test_ratio = Vector3f(innovation.edivide(innovation_variance * gate * gate)) * innovation;
+
+				if ((test_ratio <= 1.f) && !innovation.longerThan(0.05f)) {
+					_trim_validated = _trim_estimate.mean();
+					_state = state::complete;
+					_state_start_time = now;
+
+				} else {
+					_state = state::fail;
+				}
+
+			} break;
+
+		case state::complete:
+			//TODO: depending on param, stay or restart
+			_state = state::idle;
+			break;
+		}
+
+	} else {
+		_state = state::fail;
+	}
+
+	publishStatus(now);
+
+	perf_end(_cycle_perf);
+}
+
+void FwAutoTrim::publishStatus(const hrt_abstime now)
+{
+	auto_trim_status_s status_msg{};
+
+	_trim_validated.copyTo(status_msg.trim_validated);
+	_trim_estimate.mean().copyTo(status_msg.trim_estimate);
+	_trim_estimate.variance().copyTo(status_msg.trim_estimate_var);
+	_trim_test.mean().copyTo(status_msg.trim_test);
+	_trim_test.variance().copyTo(status_msg.trim_test_var);
+	status_msg.state = static_cast<int>(_state);
+
+	status_msg.timestamp = now;
+	status_msg.timestamp_sample = now;
+
+	_auto_trim_status_pub.publish(status_msg);
+}

src/modules/fw_rate_control/fw_trim/FwAutoTrim.hpp

@@ -0,0 +1,111 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <drivers/drv_hrt.h>
+#include <lib/hysteresis/hysteresis.h>
+#include <lib/perf/perf_counter.h>
+#include <lib/mathlib/math/WelfordMeanVector.hpp>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/airspeed_validated.h>
+#include <uORB/topics/auto_trim_status.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_status.h>
+
+using namespace time_literals;
+
+class FwAutoTrim : public ModuleParams
+{
+public:
+	FwAutoTrim(ModuleParams *parent);
+	~FwAutoTrim();
+
+	void reset();
+	void update(const matrix::Vector3f &torque_sp, float dt);
+
+	const matrix::Vector3f &getTrim() const { return _trim_validated; }
+
+protected:
+	void updateParams() override;
+
+private:
+	void publishStatus(const hrt_abstime now);
+
+	uORB::Publication<auto_trim_status_s> _auto_trim_status_pub{ORB_ID(auto_trim_status)};
+
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
+	uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+
+	enum class state {
+		idle = auto_trim_status_s::STATE_IDLE,
+		sampling = auto_trim_status_s::STATE_SAMPLING,
+		sampling_test = auto_trim_status_s::STATE_SAMPLING_TEST,
+		verification = auto_trim_status_s::STATE_VERIFICATION,
+		complete = auto_trim_status_s::STATE_COMPLETE,
+		fail = auto_trim_status_s::STATE_FAIL,
+	} _state{state::idle};
+
+	hrt_abstime _state_start_time{0};
+
+	bool _armed{false};
+	bool _landed{false};
+	bool _fixed_wing{false};
+	bool _tailsitter{false};
+	float _calibrated_airspeed_m_s{0.f};
+	float _cos_tilt{0.f};
+
+	math::WelfordMeanVector<float, 3> _trim_estimate{};
+	math::WelfordMeanVector<float, 3> _trim_test{};
+
+	matrix::Vector3f _trim_validated{};
+
+	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, "[AutoTrim] cycle time")};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::SYS_HAS_NUM_ASPD>) _param_sys_has_num_aspd,
+		(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
+		(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max
+	)
+
+	static constexpr float _kTiltMaxDeg = 10.f;
+};

src/modules/fw_rate_control/fw_trim/FwTrim.cpp

@@ -0,0 +1,150 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "FwTrim.hpp"
+
+#include <mathlib/mathlib.h>
+
+using namespace time_literals;
+using matrix::Vector3f;
+using math::interpolate;
+
+FwTrim::FwTrim(ModuleParams *parent) :
+	ModuleParams(parent)
+{
+	updateParams();
+	_airspeed = _param_fw_airspd_trim.get();
+}
+
+void FwTrim::updateParams()
+{
+	ModuleParams::updateParams();
+	updateParameterizedTrim();
+}
+
+void FwTrim::saveParams()
+{
+	const Vector3f autotrim = _auto_trim.getTrim();
+	bool updated = false;
+	const Vector3f trim_prev(_param_trim_roll.get(), _param_trim_pitch.get(), _param_trim_yaw.get());
+
+	if (_param_fw_atrim_mode.get() == static_cast<int32_t>(AutoTrimMode::kCalibration)) {
+		// Replace the current trim with the one identified during auto-trim
+		updated |= _param_trim_roll.commit_no_notification(_param_trim_roll.get() + autotrim(0));
+		updated |= _param_trim_pitch.commit_no_notification(_param_trim_pitch.get() + autotrim(1));
+		updated |= _param_trim_yaw.commit_no_notification(_param_trim_yaw.get() + autotrim(2));
+
+		if (updated) {
+			_param_fw_atrim_mode.set(static_cast<int32_t>(AutoTrimMode::kContinuous));
+			_param_fw_atrim_mode.commit();
+		}
+
+	} else  if (_param_fw_atrim_mode.get() == static_cast<int32_t>(AutoTrimMode::kContinuous)) {
+		// In continuous trim mode, limit the amount of trim that can be applied back to the parameter
+		const Vector3f constrained_autotrim = matrix::constrain(autotrim, -0.05f, 0.05f);
+		updated |= _param_trim_roll.commit_no_notification(_param_trim_roll.get() + constrained_autotrim(0));
+		updated |= _param_trim_pitch.commit_no_notification(_param_trim_pitch.get() + constrained_autotrim(1));
+		updated |= _param_trim_yaw.commit_no_notification(_param_trim_yaw.get() + constrained_autotrim(2));
+
+		if (updated) {
+			_param_trim_yaw.commit();
+		}
+
+	} else {
+		// nothing to do
+	}
+
+	if (updated) {
+		const Vector3f trim_new(_param_trim_roll.get(), _param_trim_pitch.get(), _param_trim_yaw.get());
+		PX4_INFO("Trim offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f]",
+			 (double)trim_prev(0), (double)trim_prev(1), (double)trim_prev(2),
+			 (double)trim_new(0), (double)trim_new(1), (double)trim_new(2));
+	}
+
+	_auto_trim.reset();
+}
+
+void FwTrim::reset()
+{
+	_auto_trim.reset();
+}
+
+void FwTrim::updateAutoTrim(const Vector3f &torque_sp, const float dt)
+{
+	_auto_trim.update(torque_sp - _parameterized_trim, dt);
+}
+
+Vector3f FwTrim::getTrim() const
+{
+	Vector3f trim = _parameterized_trim;
+
+	if (_param_fw_atrim_mode.get() > 0) {
+		trim +=  _auto_trim.getTrim();
+	}
+
+	return trim;
+}
+
+void FwTrim::setAirspeed(const float airspeed)
+{
+	_airspeed = airspeed;
+	updateParameterizedTrim();
+}
+
+void FwTrim::updateParameterizedTrim()
+{
+	/* bi-linear interpolation over airspeed for actuator trim scheduling */
+	Vector3f trim(_param_trim_roll.get(), _param_trim_pitch.get(), _param_trim_yaw.get());
+
+	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);
+
+	} 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());
+	}
+
+	_parameterized_trim = trim;
+}

src/modules/fw_rate_control/fw_trim/FwTrim.hpp

@@ -0,0 +1,95 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <drivers/drv_hrt.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_control_mode.h>
+
+#include "FwAutoTrim.hpp"
+
+using namespace time_literals;
+
+class FwTrim : public ModuleParams
+{
+public:
+	FwTrim(ModuleParams *parent);
+	~FwTrim() = default;
+
+	void saveParams();
+	void reset();
+	void setAirspeed(float airspeed);
+	void updateAutoTrim(const matrix::Vector3f &torque_sp, float dt);
+	matrix::Vector3f getTrim() const;
+
+protected:
+	void updateParams() override;
+
+private:
+	enum class AutoTrimMode {
+		kDisabled = 0,
+		kCalibration,
+		kContinuous
+	};
+
+	void updateParameterizedTrim();
+
+	FwAutoTrim _auto_trim{this};
+	matrix::Vector3f _parameterized_trim{};
+
+	float _airspeed{0.f};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::FW_ATRIM_MODE>) _param_fw_atrim_mode,
+
+		(ParamFloat<px4::params::TRIM_PITCH>) _param_trim_pitch,
+		(ParamFloat<px4::params::TRIM_ROLL>) _param_trim_roll,
+		(ParamFloat<px4::params::TRIM_YAW>) _param_trim_yaw,
+
+		(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_AIRSPD_TRIM>) _param_fw_airspd_trim,
+		(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
+		(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max
+	)
+};

src/modules/logger/logged_topics.cpp

@@ -51,6 +51,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("airspeed", 1000);
 	add_optional_topic("airspeed_validated", 200);
 	add_optional_topic("autotune_attitude_control_status", 100);
+	add_optional_topic("auto_trim_status");
 	add_optional_topic("camera_capture");
 	add_optional_topic("camera_trigger");
 	add_topic("cellular_status", 200);