Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl_Heli: Add Rate Accel FF to Yaw control

Browse Source
This commit is contained in:
Robert Lefebvre 2015-05-22 20:01:03 -04:00 committed by Andrew Tridgell
parent 366f87c91c
commit 353879cd2b
2 changed files with 12 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View File

@ -154,8 +154,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), _dt);
pitch_ff = pitch_feedforward_filter.apply(((AC_HELI_PID&)_pid_rate_pitch).get_ff(rate_pitch_target_cds), _dt);
roll_ff = roll_feedforward_filter.apply(((AC_HELI_PID&)_pid_rate_roll).get_vff(rate_roll_target_cds), _dt);
pitch_ff = pitch_feedforward_filter.apply(((AC_HELI_PID&)_pid_rate_pitch).get_vff(rate_pitch_target_cds), _dt);
// add feed forward and final output
roll_out = roll_pd + roll_i + roll_ff;
@ -262,7 +262,7 @@ static LowPassFilterFloat rate_dynamics_filter; // Rate Dynamics filter
// rate_bf_to_motor_yaw - ask the rate controller to calculate the motor outputs to achieve the target rate in centi-degrees / second
float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_cds)
{
float pd,i,ff; // used to capture pid values for logging
float pd,i,vff,aff; // used to capture pid values for logging
float current_rate; // this iteration's rate
float rate_error; // simply target_rate - current_rate
float yaw_out;
@ -292,10 +292,11 @@ 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), _dt);
vff = yaw_velocity_feedforward_filter.apply(((AC_HELI_PID&)_pid_rate_yaw).get_vff(rate_target_cds), _dt);
aff = yaw_acceleration_feedforward_filter.apply(((AC_HELI_PID&)_pid_rate_yaw).get_aff(rate_target_cds), _dt);
// add feed forward
yaw_out = pd + i + ff;
yaw_out = pd + i + vff + aff;
// constrain output and update limit flag
if (fabsf(yaw_out) > AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX) {

9
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
View File

@ -12,7 +12,8 @@
#define AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE 0.02f
#define AC_ATTITUDE_HELI_RATE_RP_FF_FILTER 5.0f
#define AC_ATTITUDE_HELI_RATE_Y_FF_FILTER 10.0f
#define AC_ATTITUDE_HELI_RATE_Y_VFF_FILTER 10.0f
#define AC_ATTITUDE_HELI_RATE_Y_AFF_FILTER 10.0f
class AC_AttitudeControl_Heli : public AC_AttitudeControl {
public:
@ -28,7 +29,8 @@ public:
_passthrough_roll(0), _passthrough_pitch(0),
pitch_feedforward_filter(AC_ATTITUDE_HELI_RATE_RP_FF_FILTER),
roll_feedforward_filter(AC_ATTITUDE_HELI_RATE_RP_FF_FILTER),
yaw_feedforward_filter(AC_ATTITUDE_HELI_RATE_Y_FF_FILTER)
yaw_velocity_feedforward_filter(AC_ATTITUDE_HELI_RATE_Y_VFF_FILTER),
yaw_acceleration_feedforward_filter(AC_ATTITUDE_HELI_RATE_Y_AFF_FILTER)
{
AP_Param::setup_object_defaults(this, var_info);
}
@ -84,7 +86,8 @@ private:
// to jerks on rate change requests.
LowPassFilterFloat pitch_feedforward_filter;
LowPassFilterFloat roll_feedforward_filter;
LowPassFilterFloat yaw_feedforward_filter;
LowPassFilterFloat yaw_velocity_feedforward_filter;
LowPassFilterFloat yaw_acceleration_feedforward_filter;
};