aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_AttControl_Heli: Add LPF filter to Rate Feedforward terms

Browse Source
This commit is contained in:
Robert Lefebvre 2014-05-10 10:17:32 -04:00 committed by Randy Mackay
parent 8eb557d1e0
commit 26be7aed97
2 changed files with 24 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View File

@ -139,8 +139,8 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target
} }
} }
roll_ff = ((AC_HELI_PID&)_pid_rate_roll).get_ff(rate_roll_target_cds); roll_ff = roll_feedforward_filter.apply(((AC_HELI_PID&)_pid_rate_roll).get_ff(rate_roll_target_cds));
pitch_ff = ((AC_HELI_PID&)_pid_rate_pitch).get_ff(rate_pitch_target_cds); pitch_ff = pitch_feedforward_filter.apply(((AC_HELI_PID&)_pid_rate_pitch).get_ff(rate_pitch_target_cds));
// add feed forward and final output // add feed forward and final output
roll_out = roll_pd + roll_i + roll_ff; roll_out = roll_pd + roll_i + roll_ff;
@ -272,7 +272,7 @@ float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_cds)
} }
} }
ff = ((AC_HELI_PID&)_pid_rate_yaw).get_ff(rate_target_cds); ff = yaw_feedforward_filter.apply(((AC_HELI_PID&)_pid_rate_yaw).get_ff(rate_target_cds));
// add feed forward // add feed forward
yaw_out = pd + i + ff; yaw_out = pd + i + ff;
@ -301,3 +301,12 @@ int16_t AC_AttitudeControl_Heli::get_angle_boost(int16_t throttle_pwm)
_angle_boost = 0; _angle_boost = 0;
return throttle_pwm; return throttle_pwm;
} }
// update_feedforward_filter_rate - Sets LPF cutoff frequency
void AC_AttitudeControl_Heli::update_feedforward_filter_rates(float time_step)
{
pitch_feedforward_filter.set_cutoff_frequency(time_step, AC_ATTITUDE_HELI_RATE_FF_FILTER);
roll_feedforward_filter.set_cutoff_frequency(time_step, AC_ATTITUDE_HELI_RATE_FF_FILTER);
yaw_feedforward_filter.set_cutoff_frequency(time_step, AC_ATTITUDE_HELI_RATE_FF_FILTER);
}

11
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
View File

@ -8,11 +8,13 @@
#include <AC_AttitudeControl.h> #include <AC_AttitudeControl.h>
#include <AC_HELI_PID.h> #include <AC_HELI_PID.h>
#include <FILTER.h>
#define AC_ATTITUDE_HELI_ROLL_FF 0.0f #define AC_ATTITUDE_HELI_ROLL_FF 0.0f
#define AC_ATTITUDE_HELI_PITCH_FF 0.0f #define AC_ATTITUDE_HELI_PITCH_FF 0.0f
#define AC_ATTITUDE_HELI_YAW_FF 0.0f #define AC_ATTITUDE_HELI_YAW_FF 0.0f
#define AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE 0.02f #define AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE 0.02f
#define AC_ATTITUDE_HELI_RATE_FF_FILTER 5.0f
class AC_AttitudeControl_Heli : public AC_AttitudeControl { class AC_AttitudeControl_Heli : public AC_AttitudeControl {
public: public:
@ -37,6 +39,8 @@ public:
// use_leaky_i - controls whether we use leaky i term for body-frame to motor output stage // use_leaky_i - controls whether we use leaky i term for body-frame to motor output stage
void use_leaky_i(bool leaky_i) { _flags_heli.leaky_i = leaky_i; } void use_leaky_i(bool leaky_i) { _flags_heli.leaky_i = leaky_i; }
void update_feedforward_filter_rates(float time_step);
// user settable parameters // user settable parameters
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
@ -65,6 +69,13 @@ private:
// get_angle_boost - calculate total body frame throttle required to produce the given earth frame throttle // get_angle_boost - calculate total body frame throttle required to produce the given earth frame throttle
virtual int16_t get_angle_boost(int16_t throttle_pwm); virtual int16_t get_angle_boost(int16_t throttle_pwm);
// LPF filters to act on Rate Feedforward terms to linearize output.
// Due to complicated aerodynamic effects, feedforwards acting too fast can lead
// to jerks on rate change requests.
LowPassFilterInt32 pitch_feedforward_filter;
LowPassFilterInt32 roll_feedforward_filter;
LowPassFilterInt32 yaw_feedforward_filter;
}; };
#endif //AC_ATTITUDECONTROL_HELI_H #endif //AC_ATTITUDECONTROL_HELI_H