mirror of https://github.com/ArduPilot/ardupilot
simple patch to improve alt hold based on stability patch
This commit is contained in:
parent
2268646815
commit
e8a14c8d93
|
@ -7,7 +7,6 @@ Authors: Jason Short
|
|||
Based on code and ideas from the Arducopter team: Jose Julio, Randy Mackay, Jani Hirvinen
|
||||
Thanks to: Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert, Benjamin Pelletier
|
||||
|
||||
|
||||
This firmware is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
|
@ -1398,14 +1397,14 @@ adjust_altitude()
|
|||
// we remove 0 to 100 PWM from hover
|
||||
manual_boost = g.rc_3.control_in - 180;
|
||||
manual_boost = max(-120, manual_boost);
|
||||
g.throttle_cruise += (g.pi_alt_hold.get_integrator() * g.pi_throttle.kP() + g.pi_throttle.get_integrator());
|
||||
g.throttle_cruise += (g.pi_alt_hold.get_integrator());
|
||||
g.pi_alt_hold.reset_I();
|
||||
g.pi_throttle.reset_I();
|
||||
|
||||
}else if (g.rc_3.control_in >= 650){
|
||||
// we add 0 to 100 PWM to hover
|
||||
manual_boost = g.rc_3.control_in - 650;
|
||||
g.throttle_cruise += (g.pi_alt_hold.get_integrator() * g.pi_throttle.kP() + g.pi_throttle.get_integrator());
|
||||
g.throttle_cruise += (g.pi_alt_hold.get_integrator());
|
||||
g.pi_alt_hold.reset_I();
|
||||
g.pi_throttle.reset_I();
|
||||
|
||||
|
|
|
@ -99,17 +99,33 @@ get_stabilize_yaw(int32_t target_angle)
|
|||
}
|
||||
|
||||
#define ALT_ERROR_MAX 300
|
||||
static int
|
||||
static int16_t
|
||||
get_nav_throttle(int32_t z_error)
|
||||
{
|
||||
int16_t rate_error;
|
||||
|
||||
bool calc_i = (abs(z_error) < ALT_ERROR_MAX);
|
||||
|
||||
// limit error to prevent I term run up
|
||||
z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX);
|
||||
int rate_error = g.pi_alt_hold.get_pi(z_error, .1, calc_i); //_p = .85
|
||||
|
||||
// desired rate
|
||||
rate_error = g.pi_alt_hold.get_pi(z_error, .1, calc_i); //_p = .85
|
||||
|
||||
// get I-term controller - will fix up later
|
||||
int16_t iterm = g.pi_alt_hold.get_integrator();
|
||||
|
||||
// remove iterm from PI output
|
||||
rate_error -= iterm;
|
||||
|
||||
// calculate rate error
|
||||
rate_error = rate_error - climb_rate;
|
||||
|
||||
// limit the rate
|
||||
return constrain((int)g.pi_throttle.get_pi(rate_error, .1), -120, 180);
|
||||
rate_error = constrain((int)g.pi_throttle.get_pi(rate_error, .1), -120, 180);
|
||||
|
||||
// output control:
|
||||
return rate_error + iterm;
|
||||
}
|
||||
|
||||
static int
|
||||
|
|
|
@ -628,7 +628,7 @@
|
|||
|
||||
// RATE control
|
||||
#ifndef THROTTLE_P
|
||||
# define THROTTLE_P 0.5 //
|
||||
# define THROTTLE_P 0.4 //
|
||||
#endif
|
||||
#ifndef THROTTLE_I
|
||||
# define THROTTLE_I 0.0 //
|
||||
|
|
Loading…
Reference in New Issue