Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: use new lowpass filter

This commit is contained in:
Jonathan Challinger 2015-04-16 12:35:28 -07:00 committed by Randy Mackay
parent 76da2868d0
commit 87500d9d70
3 changed files with 12 additions and 20 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

@ -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);
}

13
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
View File

@ -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;

4
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -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