From 2311d52d377dfa8cf5c39deffd93b81263f9e435 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Tue, 29 May 2012 12:42:37 -0700 Subject: [PATCH] Renamed some variables to clarify functionality. --- ArduCopter/Attitude.pde | 26 ++++++++------------------ 1 file changed, 8 insertions(+), 18 deletions(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index e6df8b4120..62458efc39 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -269,38 +269,28 @@ get_rate_yaw(int32_t target_rate) static int16_t get_nav_throttle(int32_t z_error) { - //static int16_t old_output = 0; - int16_t rate_error = 0; + int16_t z_rate_error = 0; + int16_t z_target_speed = 0; int16_t output = 0; // convert to desired Rate: - rate_error = g.pi_alt_hold.get_p(z_error); - rate_error = constrain(rate_error, -250, 250); + z_target_speed = g.pi_alt_hold.get_p(z_error); + z_target_speed = constrain(z_target_speed, -250, 250); // limit error to prevent I term wind up z_error = constrain(z_error, -400, 400); // compensates throttle setpoint error for hovering - int16_t iterm = g.pi_alt_hold.get_i(z_error, .02); + int16_t i_hold = g.pi_alt_hold.get_i(z_error, .02); // calculate rate error - rate_error = rate_error - climb_rate; - - // hack to see if we can smooth out oscillations - //if(rate_error < 0) - // rate_error = rate_error >> 1; + z_rate_error = z_target_speed - climb_rate; // limit the rate - output = constrain(g.pid_throttle.get_pid(rate_error, .02), -80, 120); - - // light filter of output - //output = (old_output + output) / 2; - - // save our output - //old_output = output; + output = constrain(g.pid_throttle.get_pid(z_rate_error, .02), -80, 120); // output control: - return output + iterm; + return output + i_hold; } // Keeps old data out of our calculation / logs