Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Added a small D term on alt hold

Browse Source
This commit is contained in:
Jason Short 2012-01-12 22:28:12 -08:00
parent 6b85adb460
commit 4459fc4e34
1 changed files with 7 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
ArduCopter/Attitude.pde
View File

@ -166,6 +166,7 @@ static int16_t
get_nav_throttle(int32_t z_error)
{
static int16_t old_output = 0;
static int16_t rate_d = 0;
int16_t rate_error;
int16_t output;
@ -185,6 +186,12 @@ get_nav_throttle(int32_t z_error)
// limit the rate - iterm is not used
output = constrain((int)g.pi_throttle.get_p(rate_error), -160, 180);
// a positive climb rate means we're going up
rate_d = ((rate_d + climb_rate)>>1) * 1; // replace with gain
// slight adjustment to alt hold output
output -= rate_d;
// light filter of output
output = (old_output * 3 + output) / 4;
@ -222,15 +229,6 @@ get_rate_yaw(int32_t target_rate)
return (int)constrain(target_rate, -2500, 2500);
}
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
// Keeps outdated data out of our calculations
//static void reset_hold_I(void)
//{
// g.pi_loiter_lat.reset_I();
// g.pi_loiter_lon.reset_I();
//}
// Keeps old data out of our calculation / logs
static void reset_nav(void)
{