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

simple patch to improve alt hold based on stability patch

Browse Source
This commit is contained in:
Jason Short 2011-12-04 20:51:34 -08:00
parent 2268646815
commit e8a14c8d93
3 changed files with 41 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopter/ArduCopter.pde
View File

@ -7,7 +7,6 @@ Authors: Jason Short
Based on code and ideas from the Arducopter team: Jose Julio, Randy Mackay, Jani Hirvinen 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 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 This firmware is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either License as published by the Free Software Foundation; either
@ -1398,14 +1397,14 @@ adjust_altitude()
// we remove 0 to 100 PWM from hover // we remove 0 to 100 PWM from hover
manual_boost = g.rc_3.control_in - 180; manual_boost = g.rc_3.control_in - 180;
manual_boost = max(-120, manual_boost); 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_alt_hold.reset_I();
g.pi_throttle.reset_I(); g.pi_throttle.reset_I();
}else if (g.rc_3.control_in >= 650){ }else if (g.rc_3.control_in >= 650){
// we add 0 to 100 PWM to hover // we add 0 to 100 PWM to hover
manual_boost = g.rc_3.control_in - 650; 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_alt_hold.reset_I();
g.pi_throttle.reset_I(); g.pi_throttle.reset_I();

22
ArduCopter/Attitude.pde
View File

@ -99,17 +99,33 @@ get_stabilize_yaw(int32_t target_angle)
} }
#define ALT_ERROR_MAX 300 #define ALT_ERROR_MAX 300
static int static int16_t
get_nav_throttle(int32_t z_error) get_nav_throttle(int32_t z_error)
{ {
int16_t rate_error;
bool calc_i = (abs(z_error) < ALT_ERROR_MAX); bool calc_i = (abs(z_error) < ALT_ERROR_MAX);
// limit error to prevent I term run up // limit error to prevent I term run up
z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX); 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; rate_error = rate_error - climb_rate;
// limit the 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 static int

2
ArduCopter/config.h
View File

@ -628,7 +628,7 @@
// RATE control // RATE control
#ifndef THROTTLE_P #ifndef THROTTLE_P
# define THROTTLE_P 0.5 // # define THROTTLE_P 0.4 //
#endif #endif
#ifndef THROTTLE_I #ifndef THROTTLE_I
# define THROTTLE_I 0.0 // # define THROTTLE_I 0.0 //