AC_AttitudeControl: use Lead-Lag filter in heli

bnsgeyer · Nov 12, 2024 · a779f43 · a779f43
1 parent a2ab148
commit a779f43
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 2 deletions.
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
@@ -460,16 +460,22 @@ void AC_AttitudeControl_Heli::update_althold_lean_angle_max(float throttle_in)
 void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(const Vector3f &rate_rads, float rate_roll_target_rads, float rate_pitch_target_rads)
 {
 
+    // apply lead filter
+    roll_lead_filter.init( 2.0f, 1.2f, _dt);
+    pitch_lead_filter.init( 2.0f, 1.2f, _dt);
+
+    float roll_target = roll_lead_filter.apply(rate_roll_target_rads);
+    float pitch_target = pitch_lead_filter.apply(rate_pitch_target_rads);
     if (_flags_heli.leaky_i) {
         _pid_rate_roll.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
     }
-    float roll_pid = _pid_rate_roll.update_all(rate_roll_target_rads, rate_rads.x, _dt, _motors.limit.roll) + _actuator_sysid.x;
+    float roll_pid = _pid_rate_roll.update_all(roll_target, rate_rads.x, _dt, _motors.limit.roll) + _actuator_sysid.x;
 
     if (_flags_heli.leaky_i) {
         _pid_rate_pitch.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
     }
 
-    float pitch_pid = _pid_rate_pitch.update_all(rate_pitch_target_rads, rate_rads.y, _dt, _motors.limit.pitch) + _actuator_sysid.y;
+    float pitch_pid = _pid_rate_pitch.update_all(pitch_target, rate_rads.y, _dt, _motors.limit.pitch) + _actuator_sysid.y;
 
     // use pid library to calculate ff
     float roll_ff = _pid_rate_roll.get_ff();

diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
@@ -7,6 +7,8 @@
 #include <AP_Motors/AP_MotorsHeli.h>
 #include <AC_PID/AC_HELI_PID.h>
 #include <Filter/Filter.h>
+#include <Filter/LeadLagFilter.h>
+
 
 // default rate controller PID gains
 #define AC_ATC_HELI_RATE_RP_P                       0.024f
@@ -147,6 +149,9 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl {
     // controller, in radians in the vehicle body frame of reference.
     Vector3f            _att_error_rot_vec_rad;
 
+    LeadLagFilterFloat roll_lead_filter;
+    LeadLagFilterFloat pitch_lead_filter;
+
     // parameters
     AP_Int8         _piro_comp_enabled;             // Flybar present or not.  Affects attitude controller used during ACRO flight mode
     AP_Int16        _hover_roll_trim;               // Angle in centi-degrees used to counter tail rotor thrust in hover