FwAutoTrim: save estimated trim back to params

src/modules/fw_rate_control/FixedwingRateControl.cpp

@@ -96,6 +96,12 @@ FixedwingRateControl::parameters_update()
 	return PX4_OK;
 }
 
+void
+FixedwingRateControl::save_params()
+{
+	_trim.saveParams();
+}
+
 void
 FixedwingRateControl::vehicle_manual_poll()
 {
@@ -257,8 +263,15 @@ 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;
 
+		{
+			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();
 
 		vehicle_manual_poll();

src/modules/fw_rate_control/FixedwingRateControl.hpp

@@ -212,6 +212,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/FwTrim.cpp

@@ -52,6 +52,39 @@ void FwTrim::updateParams()
 	updateParameterizedTrim();
 }
 
+void FwTrim::saveParams()
+{
+	const Vector3f autotrim = _auto_trim.getTrim();
+
+	if (_param_fw_atrim_mode.get() == static_cast<int32_t>(AutoTrimMode::kCalibration)) {
+		// Replace the current trim with the one identified during auto-trim
+		bool 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);
+		bool 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
+	}
+
+	_auto_trim.reset()
+}
+
 void FwTrim::reset()
 {
 	_auto_trim.reset();

src/modules/fw_rate_control/fw_trim/FwTrim.hpp

@@ -39,6 +39,7 @@
 #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"
 
@@ -50,6 +51,7 @@ public:
 	FwTrim(ModuleParams *parent);
 	~FwTrim() = default;
 
+	void saveParams();
 	void reset();
 	void setAirspeed(float airspeed);
 	void updateAutoTrim(const matrix::Vector3f &torque_sp, float dt);
@@ -60,6 +62,12 @@ protected:
 	void updateParams() override;
 
 private:
+	enum class AutoTrimMode {
+		kDisabled = 0,
+		kCalibration,
+		kContinuous
+	};
+
 	void updateParameterizedTrim();
 
 	FwAutoTrim _auto_trim{this};
@@ -68,6 +76,8 @@ private:
 	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,