From 1e039923234ad9a7dde3771c97166af4198099e3 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 9 Nov 2015 18:46:06 -0800 Subject: [PATCH] AC_AttitudeControl: add lead filter --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 47 ++++++++++++++++++- .../AC_AttitudeControl/AC_AttitudeControl.h | 9 ++++ 2 files changed, 54 insertions(+), 2 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 1567c131a7..f1db6b860a 100644 --- 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 index d7dc4b8f21..7f5d27db26 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -14,6 +14,7 @@ #include #include #include +#include // 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