mirror of https://github.com/ArduPilot/ardupilot
AC_AttitudeControl: use set_desired_rate() on PID controllers
this sets them up for logging of PIDs
This commit is contained in:
parent
3ec8857fbc
commit
521dae1c65
|
@ -576,6 +576,7 @@ float AC_AttitudeControl::rate_bf_to_motor_roll(float rate_target_cds)
|
|||
// calculate error and call pid controller
|
||||
rate_error = rate_target_cds - current_rate;
|
||||
_pid_rate_roll.set_input_filter_d(rate_error);
|
||||
_pid_rate_roll.set_desired_rate(rate_target_cds);
|
||||
|
||||
// get p value
|
||||
p = _pid_rate_roll.get_p();
|
||||
|
@ -609,6 +610,7 @@ float AC_AttitudeControl::rate_bf_to_motor_pitch(float rate_target_cds)
|
|||
// calculate error and call pid controller
|
||||
rate_error = rate_target_cds - current_rate;
|
||||
_pid_rate_pitch.set_input_filter_d(rate_error);
|
||||
_pid_rate_pitch.set_desired_rate(rate_target_cds);
|
||||
|
||||
// get p value
|
||||
p = _pid_rate_pitch.get_p();
|
||||
|
@ -642,6 +644,7 @@ float AC_AttitudeControl::rate_bf_to_motor_yaw(float rate_target_cds)
|
|||
// calculate error and call pid controller
|
||||
rate_error = rate_target_cds - current_rate;
|
||||
_pid_rate_yaw.set_input_filter_all(rate_error);
|
||||
_pid_rate_yaw.set_desired_rate(rate_target_cds);
|
||||
|
||||
// get p value
|
||||
p = _pid_rate_yaw.get_p();
|
||||
|
|
|
@ -112,7 +112,9 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target
|
|||
|
||||
// input to PID controller
|
||||
_pid_rate_roll.set_input_filter_all(rate_roll_error);
|
||||
_pid_rate_roll.set_desired_rate(rate_roll_target_cds);
|
||||
_pid_rate_pitch.set_input_filter_all(rate_pitch_error);
|
||||
_pid_rate_pitch.set_desired_rate(rate_pitch_target_cds);
|
||||
|
||||
// call p and d controllers
|
||||
roll_pd = _pid_rate_roll.get_p() + _pid_rate_roll.get_d();
|
||||
|
@ -276,6 +278,7 @@ float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_cds)
|
|||
|
||||
// send input to PID controller
|
||||
_pid_rate_yaw.set_input_filter_all(rate_error);
|
||||
_pid_rate_yaw.set_desired_rate(rate_target_cds);
|
||||
|
||||
// get p and d
|
||||
pd = _pid_rate_yaw.get_p() + _pid_rate_yaw.get_d();
|
||||
|
|
|
@ -409,6 +409,7 @@ void AC_PosControl::accel_to_throttle(float accel_target_z)
|
|||
|
||||
// set input to PID
|
||||
_pid_accel_z.set_input_filter_d(_accel_error.z);
|
||||
_pid_accel_z.set_desired_rate(accel_target_z);
|
||||
|
||||
// separately calculate p, i, d values for logging
|
||||
p = _pid_accel_z.get_p();
|
||||
|
|
Loading…
Reference in New Issue