mirror of https://github.com/ArduPilot/ardupilot
AC_AttitudeControl: use new lowpass filter
This commit is contained in:
parent
76da2868d0
commit
87500d9d70
|
@ -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));
|
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));
|
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
|
// add feed forward and final output
|
||||||
roll_out = roll_pd + roll_i + roll_ff;
|
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
|
// add feed forward
|
||||||
yaw_out = pd + i + ff;
|
yaw_out = pd + i + ff;
|
||||||
|
@ -369,12 +369,3 @@ float AC_AttitudeControl_Heli::get_boosted_throttle(float throttle_in)
|
||||||
_angle_boost = 0;
|
_angle_boost = 0;
|
||||||
return throttle_in;
|
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);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
|
@ -27,7 +27,10 @@ public:
|
||||||
AC_AttitudeControl(ahrs, aparm, motors,
|
AC_AttitudeControl(ahrs, aparm, motors,
|
||||||
p_angle_roll, p_angle_pitch, p_angle_yaw,
|
p_angle_roll, p_angle_pitch, p_angle_yaw,
|
||||||
pid_rate_roll, pid_rate_pitch, pid_rate_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);
|
AP_Param::setup_object_defaults(this, var_info);
|
||||||
}
|
}
|
||||||
|
@ -45,8 +48,6 @@ public:
|
||||||
// use_flybar_passthrough - controls whether we pass-through control inputs to swash-plate
|
// 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 use_flybar_passthrough(bool passthrough) { _flags_heli.flybar_passthrough = passthrough; }
|
||||||
|
|
||||||
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[];
|
||||||
|
|
||||||
|
@ -80,9 +81,9 @@ private:
|
||||||
// LPF filters to act on Rate Feedforward terms to linearize output.
|
// LPF filters to act on Rate Feedforward terms to linearize output.
|
||||||
// Due to complicated aerodynamic effects, feedforwards acting too fast can lead
|
// Due to complicated aerodynamic effects, feedforwards acting too fast can lead
|
||||||
// to jerks on rate change requests.
|
// to jerks on rate change requests.
|
||||||
LowPassFilterInt32 pitch_feedforward_filter;
|
LowPassFilterFloat pitch_feedforward_filter;
|
||||||
LowPassFilterInt32 roll_feedforward_filter;
|
LowPassFilterFloat roll_feedforward_filter;
|
||||||
LowPassFilterInt32 yaw_feedforward_filter;
|
LowPassFilterFloat yaw_feedforward_filter;
|
||||||
|
|
||||||
// pass through for roll and pitch
|
// pass through for roll and pitch
|
||||||
int16_t _passthrough_roll;
|
int16_t _passthrough_roll;
|
||||||
|
|
|
@ -82,7 +82,7 @@ void AC_PosControl::set_dt(float delta_sec)
|
||||||
_pid_accel_z.set_dt(_dt);
|
_pid_accel_z.set_dt(_dt);
|
||||||
|
|
||||||
// update rate z-axis velocity error and accel error filters
|
// 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)
|
/// 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;
|
_flags.reset_rate_to_accel_z = false;
|
||||||
} else {
|
} else {
|
||||||
// calculate rate error and filter with cut off frequency of 2 Hz
|
// 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
|
// calculate p
|
||||||
|
|
Loading…
Reference in New Issue