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

Renamed some variables to clarify functionality.

This commit is contained in:
Jason Short 2012-05-29 12:42:37 -07:00
parent 33d1b5a137
commit 2ae5d50261
1 changed files with 8 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ArduCopter/Attitude.pde
View File

@ -269,38 +269,28 @@ get_rate_yaw(int32_t target_rate)
static int16_t static int16_t
get_nav_throttle(int32_t z_error) get_nav_throttle(int32_t z_error)
{ {
//static int16_t old_output = 0; int16_t z_rate_error = 0;
int16_t rate_error = 0; int16_t z_target_speed = 0;
int16_t output = 0; int16_t output = 0;
// convert to desired Rate: // convert to desired Rate:
rate_error = g.pi_alt_hold.get_p(z_error); z_target_speed = g.pi_alt_hold.get_p(z_error);
rate_error = constrain(rate_error, -250, 250); z_target_speed = constrain(z_target_speed, -250, 250);
// limit error to prevent I term wind up // limit error to prevent I term wind up
z_error = constrain(z_error, -400, 400); z_error = constrain(z_error, -400, 400);
// compensates throttle setpoint error for hovering // 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 // calculate rate error
rate_error = rate_error - climb_rate; z_rate_error = z_target_speed - climb_rate;
// hack to see if we can smooth out oscillations
//if(rate_error < 0)
// rate_error = rate_error >> 1;
// limit the rate // limit the rate
output = constrain(g.pid_throttle.get_pid(rate_error, .02), -80, 120); output = constrain(g.pid_throttle.get_pid(z_rate_error, .02), -80, 120);
// light filter of output
//output = (old_output + output) / 2;
// save our output
//old_output = output;
// output control: // output control:
return output + iterm; return output + i_hold;
} }
// Keeps old data out of our calculation / logs // Keeps old data out of our calculation / logs