|
|
|
@ -330,12 +330,14 @@ get_rate_roll(int32_t target_rate)
@@ -330,12 +330,14 @@ get_rate_roll(int32_t target_rate)
|
|
|
|
|
// call pid controller |
|
|
|
|
rate_error = target_rate - current_rate; |
|
|
|
|
p = g.pid_rate_roll.get_p(rate_error); |
|
|
|
|
|
|
|
|
|
// freeze I term if we've breached roll-pitch limits |
|
|
|
|
if( motors.reached_limit(AP_MOTOR_ROLLPITCH_LIMIT) ) { |
|
|
|
|
i = g.pid_rate_roll.get_integrator(); |
|
|
|
|
}else{ |
|
|
|
|
i = g.pid_rate_roll.get_i(rate_error, G_Dt); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
d = g.pid_rate_roll.get_d(rate_error, G_Dt); |
|
|
|
|
output = p + i + d; |
|
|
|
|
|
|
|
|
@ -349,6 +351,7 @@ get_rate_roll(int32_t target_rate)
@@ -349,6 +351,7 @@ get_rate_roll(int32_t target_rate)
|
|
|
|
|
|
|
|
|
|
#if LOGGING_ENABLED == ENABLED |
|
|
|
|
static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash |
|
|
|
|
|
|
|
|
|
// log output if PID logging is on and we are tuning the rate P, I or D gains |
|
|
|
|
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_KP || g.radio_tuning == CH6_RATE_KI || g.radio_tuning == CH6_RATE_KD) ) { |
|
|
|
|
log_counter++; |
|
|
|
|