AC_AttitudeControl: add lead filter

3drobotics · Nov 10, 2015 · 1e03992 · 1e03992
1 parent f091060
commit 1e03992
Show file tree

Hide file tree

Showing 2 changed files with 54 additions and 2 deletions.
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
@@ -56,6 +56,41 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] PROGMEM = {
     // @User: Advanced
     AP_GROUPINFO("ACCEL_P_MAX", 7, AC_AttitudeControl, _accel_pitch_max, AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT),
 
+    // @Param: LEAD_ROLL_W
+    // @DisplayName: Roll lead omega
+    // @Description: Omega for roll lead filter (hz)
+    // @Units: Hz
+    // @Range: 0 100
+    // @Increment: 0.1
+    // @User: Advanced
+    AP_GROUPINFO("LEAD_RLL_W", 8, AC_AttitudeControl, _roll_lead_w, 0),
+
+    // @Param: LEAD_ROLL_R
+    // @DisplayName: Roll lead ratio
+    // @Description: Ratio for roll lead filter
+    // @Range: 1 2
+    // @Increment: 0.01
+    // @User: Advanced
+    AP_GROUPINFO("LEAD_RLL_R", 9, AC_AttitudeControl, _roll_lead_r, 1),
+
+    // @Param: LEAD_PIT_W
+    // @DisplayName: Pitch lead omega
+    // @Description: Omega for pitch lead filter (hz)
+    // @Units: Hz
+    // @Range: 0 100
+    // @Increment: 0.1
+    // @User: Advanced
+    AP_GROUPINFO("LEAD_PIT_W", 10, AC_AttitudeControl, _pitch_lead_w, 0),
+
+    // @Param: LEAD_PIT_R
+    // @DisplayName: Pitch lead ratio
+    // @Description: Ratio for pitch lead filter
+    // @Range: 1 2
+    // @Increment: 0.01
+    // @User: Advanced
+    AP_GROUPINFO("LEAD_PIT_R", 11, AC_AttitudeControl, _pitch_lead_r, 1),
+
+
     AP_GROUPEND
 };
 
@@ -577,10 +612,14 @@ float AC_AttitudeControl::rate_bf_to_motor_roll(float rate_target_cds)
 
     // get current rate
     // To-Do: make getting gyro rates more efficient?
-    current_rate = (_ahrs.get_gyro_for_control().x * AC_ATTITUDE_CONTROL_DEGX100);
+    current_rate = _ahrs.get_gyro_for_control().x * AC_ATTITUDE_CONTROL_DEGX100;
 
     // calculate error and call pid controller
     rate_error = rate_target_cds - current_rate;
+
+    _roll_lead_filt.set_params(_roll_lead_w*2*M_PI, _roll_lead_r, 1.0f/_dt);
+    rate_error = _roll_lead_filt.apply(rate_error);
+
     _pid_rate_roll.set_input_filter_d(rate_error);
 
     // get p value
@@ -612,10 +651,14 @@ float AC_AttitudeControl::rate_bf_to_motor_pitch(float rate_target_cds)
 
     // get current rate
     // To-Do: make getting gyro rates more efficient?
-    current_rate = (_ahrs.get_gyro_for_control().y * AC_ATTITUDE_CONTROL_DEGX100);
+    current_rate = _ahrs.get_gyro_for_control().y * AC_ATTITUDE_CONTROL_DEGX100;
 
     // calculate error and call pid controller
     rate_error = rate_target_cds - current_rate;
+
+    _pitch_lead_filt.set_params(_pitch_lead_w*2*M_PI, _pitch_lead_r, 1.0f/_dt);
+    rate_error = _pitch_lead_filt.apply(rate_error);
+
     _pid_rate_pitch.set_input_filter_d(rate_error);
 
     // get p value

diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h
@@ -14,6 +14,7 @@
 #include <AP_Motors.h>
 #include <AC_PID.h>
 #include <AC_P.h>
+#include <LeadFilter.h>
 
 // To-Do: change the name or move to AP_Math?
 #define AC_ATTITUDE_CONTROL_DEGX100 5729.57795f                 // constant to convert from radians to centi-degrees
@@ -265,6 +266,14 @@ class AC_AttitudeControl {
     AP_Float            _accel_yaw_max;           // maximum rotation acceleration for earth-frame yaw axis
     AP_Int8             _rate_bf_ff_enabled;    // Enable/Disable body frame rate feed forward
 
+    AP_Float _roll_lead_w;
+    AP_Float _roll_lead_r;
+    AP_Float _pitch_lead_w;
+    AP_Float _pitch_lead_r;
+
+    LeadFilter _roll_lead_filt;
+    LeadFilter _pitch_lead_filt;
+
     // internal variables
     // To-Do: make rate targets a typedef instead of Vector3f?
     float               _dt;                    // time delta in seconds