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 7c6f766251
commit 2311d52d37
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
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