gimbal: add filter

Author: perrrre

src/modules/gimbal/gimbal.cpp

@@ -556,6 +556,7 @@ void update_params(ParameterHandles &param_handles, Parameters &params)
 	param_get(param_handles.mnt_rc_in_mode, &params.mnt_rc_in_mode);
 	param_get(param_handles.mnt_lnd_p_min, &params.mnt_lnd_p_min);
 	param_get(param_handles.mnt_lnd_p_max, &params.mnt_lnd_p_max);
+	param_get(param_handles.mnt_tau, &params.mnt_tau);
 }
 
 bool initialize_params(ParameterHandles &param_handles, Parameters &params)
@@ -579,6 +580,7 @@ bool initialize_params(ParameterHandles &param_handles, Parameters &params)
 	param_handles.mnt_rc_in_mode = param_find("MNT_RC_IN_MODE");
 	param_handles.mnt_lnd_p_min = param_find("MNT_LND_P_MIN");
 	param_handles.mnt_lnd_p_max = param_find("MNT_LND_P_MAX");
+	param_handles.mnt_tau = param_find("MNT_TAU");
 
 	if (param_handles.mnt_mode_in == PARAM_INVALID ||
 	    param_handles.mnt_mode_out == PARAM_INVALID ||
@@ -598,7 +600,8 @@ bool initialize_params(ParameterHandles &param_handles, Parameters &params)
 	    param_handles.mnt_rate_yaw == PARAM_INVALID ||
 	    param_handles.mnt_rc_in_mode == PARAM_INVALID ||
 	    param_handles.mnt_lnd_p_min == PARAM_INVALID ||
-	    param_handles.mnt_lnd_p_max == PARAM_INVALID
+	    param_handles.mnt_lnd_p_max == PARAM_INVALID ||
+	    param_handles.mnt_tau == PARAM_INVALID
 	   ) {
 		return false;
 	}

src/modules/gimbal/gimbal_params.c

@@ -224,6 +224,19 @@ PARAM_DEFINE_FLOAT(MNT_RATE_PITCH, 30.0f);
  */
 PARAM_DEFINE_FLOAT(MNT_RATE_YAW, 30.0f);
 
+/**
+ * Alpha filter time constant defining the convergence rate (in seconds) for open-loop AUX mount control.
+ *
+ * Use when no angular position feedback is available.
+ * With MNT_MODE_OUT set to AUX, the mount operates in open-loop and directly commands the servo output.
+ * Parameters must be tuned for the specific servo to approximate its speed and response.
+ *
+ * @min 0.0
+ * @group Mount
+ */
+PARAM_DEFINE_FLOAT(MNT_TAU, 0.3f);
+
+
 /**
  * Input mode for RC gimbal input
  *

src/modules/gimbal/gimbal_params.h

@@ -75,6 +75,7 @@ struct Parameters {
 	int32_t mnt_rc_in_mode;
 	float mnt_lnd_p_min;
 	float mnt_lnd_p_max;
+	float mnt_tau;
 };
 
 struct ParameterHandles {
@@ -97,6 +98,7 @@ struct ParameterHandles {
 	param_t mnt_rc_in_mode;
 	param_t mnt_lnd_p_min;
 	param_t mnt_lnd_p_max;
+	param_t mnt_tau;
 };
 
 } /* namespace gimbal */

src/modules/gimbal/output.cpp

@@ -286,6 +286,16 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t)
 							    math::radians(_parameters.mnt_lnd_p_max));
 		}
 	}
+
+	_angle_outputs_filtered.setParameters(dt, _parameters.mnt_tau);
+	matrix::Vector3f filtered_outputs = _angle_outputs_filtered.update(matrix::Vector3f(_angle_outputs));
+
+	for (int i = 0; i < 3; i++) {
+		_angle_outputs[i] = filtered_outputs(i);
+	}
+
+	set_last_valid_setpoint(compensate, euler_vehicle);
+
 }
 
 void OutputBase::set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize)

src/modules/gimbal/output.h

@@ -38,6 +38,8 @@
 #include "gimbal_params.h"
 #include <drivers/drv_hrt.h>
 #include <lib/geo/geo.h>
+#include <mathlib/math/filter/AlphaFilter.hpp>
+#include <matrix/Vector3.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/mount_orientation.h>
@@ -92,6 +94,7 @@ class OutputBase
 	// Pitch and role are by default aligned with the horizon.
 	// Yaw follows the vehicle (not lock/absolute mode).
 	bool _absolute_angle[3] = {true, true, false };
+	AlphaFilter<matrix::Vector3f> _angle_outputs_filtered;
 
 	/** calculate the _angle_outputs (with speed) and stabilize if needed */
 	void _calculate_angle_output(const hrt_abstime &t);