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

AC_AttControl: remove scaling for centi-degrees and legacy motor input

This commit is contained in:
Leonard Hall 2016-02-17 11:33:04 +09:00 committed by Randy Mackay
parent 70f81ee338
commit b30606bb22
1 changed files with 12 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -515,9 +515,9 @@ float AC_AttitudeControl::rate_bf_to_motor_roll(float rate_target_rads)
float current_rate_rads = _ahrs.get_gyro().x;
float rate_error_rads = rate_target_rads - current_rate_rads;
// For legacy reasons, we convert to centi-degrees before inputting to the PID
get_rate_roll_pid().set_input_filter_d(degrees(rate_error_rads)*100.0f);
get_rate_roll_pid().set_desired_rate(degrees(rate_target_rads)*100.0f);
// pass error to PID controller
get_rate_roll_pid().set_input_filter_d(rate_error_rads);
get_rate_roll_pid().set_desired_rate(rate_target_rads);
float integrator = get_rate_roll_pid().get_integrator();
@ -527,7 +527,7 @@ float AC_AttitudeControl::rate_bf_to_motor_roll(float rate_target_rads)
}
// Compute output in range -1 ~ +1
float output = (get_rate_roll_pid().get_p() + integrator + get_rate_roll_pid().get_d()) / 4500.0f;
float output = get_rate_roll_pid().get_p() + integrator + get_rate_roll_pid().get_d();
// Constrain output
return constrain_float(output, -1.0f, 1.0f);
@ -538,9 +538,9 @@ float AC_AttitudeControl::rate_bf_to_motor_pitch(float rate_target_rads)
float current_rate_rads = _ahrs.get_gyro().y;
float rate_error_rads = rate_target_rads - current_rate_rads;
// For legacy reasons, we convert to centi-degrees before inputting to the PID
get_rate_pitch_pid().set_input_filter_d(degrees(rate_error_rads)*100.0f);
get_rate_pitch_pid().set_desired_rate(degrees(rate_target_rads)*100.0f);
// pass error to PID controller
get_rate_pitch_pid().set_input_filter_d(rate_error_rads);
get_rate_pitch_pid().set_desired_rate(rate_target_rads);
float integrator = get_rate_pitch_pid().get_integrator();
@ -550,7 +550,7 @@ float AC_AttitudeControl::rate_bf_to_motor_pitch(float rate_target_rads)
}
// Compute output in range -1 ~ +1
float output = (get_rate_pitch_pid().get_p() + integrator + get_rate_pitch_pid().get_d()) / 4500.0f;
float output = get_rate_pitch_pid().get_p() + integrator + get_rate_pitch_pid().get_d();
// Constrain output
return constrain_float(output, -1.0f, 1.0f);
@ -561,9 +561,9 @@ float AC_AttitudeControl::rate_bf_to_motor_yaw(float rate_target_rads)
float current_rate_rads = _ahrs.get_gyro().z;
float rate_error_rads = rate_target_rads - current_rate_rads;
// For legacy reasons, we convert to centi-degrees before inputting to the PID
get_rate_yaw_pid().set_input_filter_all(degrees(rate_error_rads)*100.0f);
get_rate_yaw_pid().set_desired_rate(degrees(rate_target_rads)*100.0f);
// pass error to PID controller
get_rate_yaw_pid().set_input_filter_all(rate_error_rads);
get_rate_yaw_pid().set_desired_rate(rate_target_rads);
float integrator = get_rate_yaw_pid().get_integrator();
@ -573,7 +573,7 @@ float AC_AttitudeControl::rate_bf_to_motor_yaw(float rate_target_rads)
}
// Compute output in range -1 ~ +1
float output = (get_rate_yaw_pid().get_p() + integrator + get_rate_yaw_pid().get_d()) / 4500.0f;
float output = get_rate_yaw_pid().get_p() + integrator + get_rate_yaw_pid().get_d();
// Constrain output
return constrain_float(output, -1.0f, 1.0f);