use auto_trim on top of current trim

src/modules/fw_rate_control/FixedwingRateControl.cpp

@@ -325,9 +325,6 @@ 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());
 
-			_auto_trim_slew.update(_auto_trim.getTrim(), dt).print();
-			trim += _auto_trim_slew.getState();
-
 			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(),
@@ -348,6 +345,8 @@ void FixedwingRateControl::Run()
 						       _param_fw_dtrim_y_vmax.get());
 			}
 
+			trim += _auto_trim_slew.update(_auto_trim.getTrim(), dt);
+
 			if (_vcontrol_mode.flag_control_rates_enabled) {
 				_rates_sp_sub.update(&_rates_sp);
 
@@ -375,6 +374,7 @@ void FixedwingRateControl::Run()
 
 				if (control_u.isAllFinite()) {
 					matrix::constrain(control_u + trim, -1.f, 1.f).copyTo(_vehicle_torque_setpoint.xyz);
+					_auto_trim.update(control_u + _auto_trim_slew.getState(), dt);
 
 				} else {
 					_rate_control.resetIntegral();
@@ -409,6 +409,7 @@ void FixedwingRateControl::Run()
 		} else {
 			// full manual
 			_rate_control.resetIntegral();
+			_auto_trim.reset();
 		}
 
 		// Add feed-forward from roll control output to yaw control output
@@ -436,11 +437,6 @@ void FixedwingRateControl::Run()
 				_vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
 				_vehicle_torque_setpoint_pub.publish(_vehicle_torque_setpoint);
 			}
-
-			_auto_trim.update(_vehicle_torque_setpoint, dt);
-
-		} else {
-			_auto_trim.reset();
 		}
 
 		updateActuatorControlsStatus(dt);

src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.cpp

@@ -60,7 +60,7 @@ void FwAutoTrim::reset()
 	_state = state::idle;
 }
 
-void FwAutoTrim::update(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, const float dt)
+void FwAutoTrim::update(const Vector3f &torque_sp, const float dt)
 {
 	if (_vehicle_land_detected_sub.updated()) {
 		vehicle_land_detected_s vehicle_land_detected;
@@ -100,13 +100,12 @@ void FwAutoTrim::update(const vehicle_torque_setpoint_s &vehicle_torque_setpoint
 	}
 
 	const hrt_abstime now = hrt_absolute_time();
-	const Vector3f torque{vehicle_torque_setpoint.xyz};
 
 	const bool run_auto_trim = _fixed_wing
 				   && _armed
 				   && !_landed
 				   && (dt > 0.001f) && (dt < 0.1f)
-				   && torque.isAllFinite()
+				   && torque_sp.isAllFinite()
 				   // && _calibrated_airspeed_m_s >= _param_fw_airspd_min.get()
 				   // && _calibrated_airspeed_m_s <= _param_fw_airspd_max.get()
 				   && _cos_tilt > cosf(math::radians(_kTiltMaxDeg));
@@ -124,7 +123,7 @@ void FwAutoTrim::update(const vehicle_torque_setpoint_s &vehicle_torque_setpoint
 			break;
 
 		case state::sampling:
-			_trim_estimate.update(torque);
+			_trim_estimate.update(torque_sp);
 
 			if ((now - _state_start_time) > 5_s) {
 				_state = state::sampling_test;
@@ -134,7 +133,7 @@ void FwAutoTrim::update(const vehicle_torque_setpoint_s &vehicle_torque_setpoint
 			break;
 
 		case state::sampling_test:
-			_trim_test.update(torque);
+			_trim_test.update(torque_sp);
 
 			if ((now - _state_start_time) > 2_s) {
 				_state = state::verification;
@@ -171,17 +170,15 @@ void FwAutoTrim::update(const vehicle_torque_setpoint_s &vehicle_torque_setpoint
 		_state = state::fail;
 	}
 
-	publishStatus(vehicle_torque_setpoint.timestamp_sample);
+	publishStatus(now);
 
 	perf_end(_cycle_perf);
 }
 
-void FwAutoTrim::publishStatus(const hrt_abstime &timestamp_sample)
+void FwAutoTrim::publishStatus(const hrt_abstime now)
 {
 	auto_trim_status_s status_msg{};
 
-	status_msg.timestamp_sample = timestamp_sample;
-
 	_trim_validated.copyTo(status_msg.trim_validated);
 	_trim_estimate.mean().copyTo(status_msg.trim_estimate);
 	_trim_estimate.variance().copyTo(status_msg.trim_estimate_var);
@@ -189,7 +186,8 @@ void FwAutoTrim::publishStatus(const hrt_abstime &timestamp_sample)
 	_trim_test.variance().copyTo(status_msg.trim_test_var);
 	status_msg.state = static_cast<int>(_state);
 
-	status_msg.timestamp = hrt_absolute_time();
+	status_msg.timestamp = now;
+	status_msg.timestamp_sample = now;
 
 	_auto_trim_status_pub.publish(status_msg);
 }

src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.hpp

@@ -48,7 +48,6 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_land_detected.h>
-#include <uORB/topics/vehicle_torque_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 
 using namespace time_literals;
@@ -60,7 +59,7 @@ public:
 	~FwAutoTrim();
 
 	void reset();
-	void update(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, float dt);
+	void update(const matrix::Vector3f &torque_sp, float dt);
 	int print_status();
 
 	const matrix::Vector3f &getTrim() const { return _trim_validated; }
@@ -69,7 +68,7 @@ protected:
 	void updateParams() override;
 
 private:
-	void publishStatus(const hrt_abstime &timestamp_sample);
+	void publishStatus(const hrt_abstime now);
 
 	uORB::Publication<auto_trim_status_s> _auto_trim_status_pub{ORB_ID(auto_trim_status)};