TEMP: auto-trim

msg/RateCtrlStatus.msg

@@ -5,3 +5,5 @@ float32 rollspeed_integ
 float32 pitchspeed_integ
 float32 yawspeed_integ
 float32 wheel_rate_integ	# FW only and optional
+
+float32[3] trim

src/modules/fw_rate_control/FixedwingRateControl.cpp

@@ -276,12 +276,14 @@ void FixedwingRateControl::Run()
 			/* reset integrals where needed */
 			if (_rates_sp.reset_integral) {
 				_rate_control.resetIntegral();
+				_auto_trim.reset(Vector3f());
 			}
 
 			// Reset integrators if the aircraft is on ground or not in a state where the fw attitude controller is run
 			if (_landed || !_in_fw_or_transition_wo_tailsitter_transition) {
 
 				_rate_control.resetIntegral();
+				_auto_trim.reset(Vector3f());
 			}
 
 			// Update saturation status from control allocation feedback
@@ -322,30 +324,30 @@ void FixedwingRateControl::Run()
 			}
 
 			/* bi-linear interpolation over airspeed for actuator trim scheduling */
-			float trim_roll = _param_trim_roll.get();
-			float trim_pitch = _param_trim_pitch.get();
-			float trim_yaw = _param_trim_yaw.get();
+			Vector3f trim(_param_trim_roll.get(), _param_trim_pitch.get(), _param_trim_yaw.get());
 
 			if (airspeed < _param_fw_airspd_trim.get()) {
-				trim_roll += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(),
-							 _param_fw_dtrim_r_vmin.get(),
-							 0.0f);
-				trim_pitch += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(),
-							  _param_fw_dtrim_p_vmin.get(),
-							  0.0f);
-				trim_yaw += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(),
-							_param_fw_dtrim_y_vmin.get(),
-							0.0f);
+				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_roll += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
-							 _param_fw_dtrim_r_vmax.get());
-				trim_pitch += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
-							  _param_fw_dtrim_p_vmax.get());
-				trim_yaw += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
-							_param_fw_dtrim_y_vmax.get());
+				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());
 			}
 
+			trim += _auto_trim.getState();
+
 			if (_vcontrol_mode.flag_control_rates_enabled) {
 				_rates_sp_sub.update(&_rates_sp);
 
@@ -364,27 +366,33 @@ void FixedwingRateControl::Run()
 				const float pitch_feedforward = _param_fw_pr_ff.get() * _airspeed_scaling * body_rates_setpoint(1);
 				const float yaw_feedforward = _param_fw_yr_ff.get() * _airspeed_scaling * body_rates_setpoint(2);
 
-				const float roll_u = angular_acceleration_setpoint(0) * _airspeed_scaling * _airspeed_scaling + roll_feedforward;
-				const float pitch_u = angular_acceleration_setpoint(1) * _airspeed_scaling * _airspeed_scaling + pitch_feedforward;
+				Vector3f control_u{};
+				control_u(0) = angular_acceleration_setpoint(0) * _airspeed_scaling * _airspeed_scaling + roll_feedforward;
+				control_u(1) = angular_acceleration_setpoint(1) * _airspeed_scaling * _airspeed_scaling + pitch_feedforward;
 
 				// Special case yaw in Acro: if the parameter FW_ACRO_YAW_CTL is not set then don't control yaw
-				float yaw_u = 0.f;
-
 				if (_vcontrol_mode.flag_control_attitude_enabled || _param_fw_acro_yaw_en.get()) {
-					yaw_u = angular_acceleration_setpoint(2) * _airspeed_scaling * _airspeed_scaling + yaw_feedforward;
+					control_u(2) = angular_acceleration_setpoint(2) * _airspeed_scaling * _airspeed_scaling + yaw_feedforward;
 
 				} else {
-					yaw_u = _manual_control_setpoint.yaw * _param_fw_man_y_sc.get();
+					control_u(2) = _manual_control_setpoint.yaw * _param_fw_man_y_sc.get();
 					_rate_control.resetIntegral(2);
 				}
 
-				if (!PX4_ISFINITE(roll_u) || !PX4_ISFINITE(pitch_u) || !PX4_ISFINITE(yaw_u)) {
+				if (!control_u.isAllFinite()) {
 					_rate_control.resetIntegral();
 				}
 
-				_vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(roll_u) ? math::constrain(roll_u + trim_roll, -1.f, 1.f) : trim_roll;
-				_vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(pitch_u) ? math::constrain(pitch_u + trim_pitch, -1.f, 1.f) : trim_pitch;
-				_vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(yaw_u) ? math::constrain(yaw_u + trim_yaw, -1.f, 1.f) : trim_yaw;
+				_vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(control_u(0)) ? math::constrain(control_u(0) + trim(0), -1.f,
+								  1.f) : trim(0);
+				_vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(control_u(1)) ? math::constrain(control_u(1) + trim(1), -1.f,
+								  1.f) : trim(1);
+				_vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(control_u(2)) ? math::constrain(control_u(2) + trim(2), -1.f,
+								  1.f) : trim(2);
+
+				// TODO: finite checks
+				_auto_trim.setParameters(dt, _kAutoTrimTimeConstant);
+				_auto_trim.update(control_u + _auto_trim.getState());
 
 				/* 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;
@@ -407,6 +415,7 @@ void FixedwingRateControl::Run()
 			// publish rate controller status
 			rate_ctrl_status_s rate_ctrl_status{};
 			_rate_control.getRateControlStatus(rate_ctrl_status);
+			trim.copyTo(rate_ctrl_status.trim);
 			rate_ctrl_status.timestamp = hrt_absolute_time();
 
 			_rate_ctrl_status_pub.publish(rate_ctrl_status);
@@ -414,6 +423,7 @@ void FixedwingRateControl::Run()
 		} else {
 			// full manual
 			_rate_control.resetIntegral();
+			_auto_trim.reset(Vector3f());
 		}
 
 		// Add feed-forward from roll control output to yaw control output

src/modules/fw_rate_control/FixedwingRateControl.hpp

@@ -36,6 +36,7 @@
 #include <lib/rate_control/rate_control.hpp>
 
 #include <drivers/drv_hrt.h>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
 #include <lib/mathlib/mathlib.h>
 #include <lib/parameters/param.h>
 #include <lib/perf/perf_counter.h>
@@ -132,6 +133,9 @@ private:
 
 	hrt_abstime _last_run{0};
 
+	AlphaFilter<matrix::Vector3f> _auto_trim{};
+	static constexpr float _kAutoTrimTimeConstant{30.f};
+
 	float _airspeed_scaling{1.0f};
 
 	bool _landed{true};