From 87500d9d7070a1273d1127c52fd6e7a1ba8e8e60 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Thu, 16 Apr 2015 12:35:28 -0700 Subject: [PATCH] AC_AttitudeControl: use new lowpass filter --- .../AC_AttitudeControl_Heli.cpp | 15 +++------------ .../AC_AttitudeControl/AC_AttitudeControl_Heli.h | 13 +++++++------ libraries/AC_AttitudeControl/AC_PosControl.cpp | 4 ++-- 3 files changed, 12 insertions(+), 20 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 6f05fa0e12..32b93dbd30 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -202,8 +202,8 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target } } - roll_ff = roll_feedforward_filter.apply(((AC_HELI_PID&)_pid_rate_roll).get_ff(rate_roll_target_cds)); - pitch_ff = pitch_feedforward_filter.apply(((AC_HELI_PID&)_pid_rate_pitch).get_ff(rate_pitch_target_cds)); + roll_ff = roll_feedforward_filter.apply(((AC_HELI_PID&)_pid_rate_roll).get_ff(rate_roll_target_cds), _dt); + pitch_ff = pitch_feedforward_filter.apply(((AC_HELI_PID&)_pid_rate_pitch).get_ff(rate_pitch_target_cds), _dt); // add feed forward and final output roll_out = roll_pd + roll_i + roll_ff; @@ -340,7 +340,7 @@ float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_cds) } } - ff = yaw_feedforward_filter.apply(((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), _dt); // add feed forward yaw_out = pd + i + ff; @@ -369,12 +369,3 @@ float AC_AttitudeControl_Heli::get_boosted_throttle(float throttle_in) _angle_boost = 0; return throttle_in; } - -// 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); -} - diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index a6339c0923..57f12596f2 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -27,7 +27,10 @@ public: AC_AttitudeControl(ahrs, aparm, motors, p_angle_roll, p_angle_pitch, p_angle_yaw, pid_rate_roll, pid_rate_pitch, pid_rate_yaw), - _passthrough_roll(0), _passthrough_pitch(0) + _passthrough_roll(0), _passthrough_pitch(0), + pitch_feedforward_filter(AC_ATTITUDE_HELI_RATE_FF_FILTER), + roll_feedforward_filter(AC_ATTITUDE_HELI_RATE_FF_FILTER), + yaw_feedforward_filter(AC_ATTITUDE_HELI_RATE_FF_FILTER) { AP_Param::setup_object_defaults(this, var_info); } @@ -44,8 +47,6 @@ public: // use_flybar_passthrough - controls whether we pass-through control inputs to swash-plate void use_flybar_passthrough(bool passthrough) { _flags_heli.flybar_passthrough = passthrough; } - - void update_feedforward_filter_rates(float time_step); // user settable parameters static const struct AP_Param::GroupInfo var_info[]; @@ -80,9 +81,9 @@ private: // 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; + LowPassFilterFloat pitch_feedforward_filter; + LowPassFilterFloat roll_feedforward_filter; + LowPassFilterFloat yaw_feedforward_filter; // pass through for roll and pitch int16_t _passthrough_roll; diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 0fcdc95fdb..637f93ba44 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -82,7 +82,7 @@ void AC_PosControl::set_dt(float delta_sec) _pid_accel_z.set_dt(_dt); // update rate z-axis velocity error and accel error filters - _vel_error_filter.set_cutoff_frequency(_dt,POSCONTROL_VEL_ERROR_CUTOFF_FREQ); + _vel_error_filter.set_cutoff_frequency(POSCONTROL_VEL_ERROR_CUTOFF_FREQ); } /// set_dt_xy - sets time delta in seconds for horizontal controller (i.e. 50hz = 0.02) @@ -362,7 +362,7 @@ void AC_PosControl::rate_to_accel_z() _flags.reset_rate_to_accel_z = false; } else { // calculate rate error and filter with cut off frequency of 2 Hz - _vel_error.z = _vel_error_filter.apply(_vel_target.z - curr_vel.z); + _vel_error.z = _vel_error_filter.apply(_vel_target.z - curr_vel.z, _dt); } // calculate p