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

AC_AttConHeli: integrate PID input filter

Browse Source
This commit is contained in:
Leonard Hall 2015-01-23 19:07:41 +09:00 committed by Randy Mackay
parent 792b2a2eb3
commit 51455af51a
1 changed files with 20 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View File

@ -162,9 +162,13 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target
rate_roll_error = rate_roll_target_cds - gyro.x * AC_ATTITUDE_CONTROL_DEGX100;
rate_pitch_error = rate_pitch_target_cds - gyro.y * AC_ATTITUDE_CONTROL_DEGX100;
// input to PID controller
_pid_rate_roll.set_input_filter_all(rate_roll_error);
_pid_rate_pitch.set_input_filter_all(rate_pitch_error);
// call p and d controllers
roll_pd = _pid_rate_roll.get_p(rate_roll_error) + _pid_rate_roll.get_d(rate_roll_error, _dt);
pitch_pd = _pid_rate_pitch.get_p(rate_pitch_error) + _pid_rate_pitch.get_d(rate_pitch_error, _dt);
roll_pd = _pid_rate_roll.get_p() + _pid_rate_roll.get_d();
pitch_pd = _pid_rate_pitch.get_p() + _pid_rate_pitch.get_d();
// get roll i term
roll_i = _pid_rate_roll.get_integrator();
@ -173,13 +177,13 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target
if (!_flags_heli.limit_roll || ((roll_i>0&&rate_roll_error<0)||(roll_i<0&&rate_roll_error>0))){
if (((AP_MotorsHeli&)_motors).has_flybar()) { // Mechanical Flybars get regular integral for rate auto trim
if (rate_roll_target_cds > -50 && rate_roll_target_cds < 50){ // Frozen at high rates
roll_i = _pid_rate_roll.get_i(rate_roll_error, _dt);
roll_i = _pid_rate_roll.get_i();
}
}else{
if (_flags_heli.leaky_i){
roll_i = ((AC_HELI_PID&)_pid_rate_roll).get_leaky_i(rate_roll_error, _dt, AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
roll_i = ((AC_HELI_PID&)_pid_rate_roll).get_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
}else{
roll_i = _pid_rate_roll.get_i(rate_roll_error, _dt);
roll_i = _pid_rate_roll.get_i();
}
}
}
@ -191,13 +195,13 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target
if (!_flags_heli.limit_pitch || ((pitch_i>0&&rate_pitch_error<0)||(pitch_i<0&&rate_pitch_error>0))){
if (((AP_MotorsHeli&)_motors).has_flybar()) { // Mechanical Flybars get regular integral for rate auto trim
if (rate_pitch_target_cds > -50 && rate_pitch_target_cds < 50){ // Frozen at high rates
pitch_i = _pid_rate_pitch.get_i(rate_pitch_error, _dt);
pitch_i = _pid_rate_pitch.get_i();
}
}else{
if (_flags_heli.leaky_i) {
pitch_i = ((AC_HELI_PID&)_pid_rate_pitch).get_leaky_i(rate_pitch_error, _dt, AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
pitch_i = ((AC_HELI_PID&)_pid_rate_pitch).get_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
}else{
pitch_i = _pid_rate_pitch.get_i(rate_pitch_error, _dt);
pitch_i = _pid_rate_pitch.get_i();
}
}
}
@ -321,7 +325,12 @@ float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_cds)
// calculate error and call pid controller
rate_error = rate_target_cds - current_rate;
pd = _pid_rate_yaw.get_p(rate_error) + _pid_rate_yaw.get_d(rate_error, _dt);
// send input to PID controller
_pid_rate_yaw.set_input_filter_all(rate_error);
// get p and d
pd = _pid_rate_yaw.get_p() + _pid_rate_yaw.get_d();
// get i term
i = _pid_rate_yaw.get_integrator();
@ -329,9 +338,9 @@ float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_cds)
// update i term as long as we haven't breached the limits or the I term will certainly reduce
if (!_flags_heli.limit_yaw || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) {
if (((AP_MotorsHeli&)_motors).motor_runup_complete()) {
i = _pid_rate_yaw.get_i(rate_error, _dt);
i = _pid_rate_yaw.get_i();
} else {
i = ((AC_HELI_PID&)_pid_rate_yaw).get_leaky_i(rate_error, _dt, AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE); // If motor is not running use leaky I-term to avoid excessive build-up
i = ((AC_HELI_PID&)_pid_rate_yaw).get_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE); // If motor is not running use leaky I-term to avoid excessive build-up
}
}