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

simple patch to improve alt hold based on stability patch

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

43
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
@ -440,32 +439,32 @@ static byte throttle_mode;
static boolean takeoff_complete; // Flag for using take-off controls static boolean takeoff_complete; // Flag for using take-off controls
static boolean land_complete; static boolean land_complete;
static int32_t old_alt; // used for managing altitude rates static int32_t old_alt; // used for managing altitude rates
static int16_t velocity_land; static int16_t velocity_land;
static byte yaw_tracking = MAV_ROI_WPNEXT; // no tracking, point at next wp, or at a target static byte yaw_tracking = MAV_ROI_WPNEXT; // no tracking, point at next wp, or at a target
static int16_t manual_boost; // used in adjust altitude to make changing alt faster static int16_t manual_boost; // used in adjust altitude to make changing alt faster
static int16_t angle_boost; // used in adjust altitude to make changing alt faster static int16_t angle_boost; // used in adjust altitude to make changing alt faster
// Loiter management // Loiter management
// ----------------- // -----------------
static int32_t original_target_bearing; // deg * 100, used to check we are not passing the WP static int32_t original_target_bearing; // deg * 100, used to check we are not passing the WP
static int32_t old_target_bearing; // used to track difference in angle static int32_t old_target_bearing; // used to track difference in angle
static int16_t loiter_total; // deg : how many times to loiter * 360 static int16_t loiter_total; // deg : how many times to loiter * 360
static int16_t loiter_sum; // deg : how far we have turned around a waypoint static int16_t loiter_sum; // deg : how far we have turned around a waypoint
static uint32_t loiter_time; // millis : when we started LOITER mode static uint32_t loiter_time; // millis : when we started LOITER mode
static unsigned loiter_time_max; // millis : how long to stay in LOITER mode static unsigned loiter_time_max; // millis : how long to stay in LOITER mode
// these are the values for navigation control functions // these are the values for navigation control functions
// ---------------------------------------------------- // ----------------------------------------------------
static int32_t nav_roll; // deg * 100 : target roll angle static int32_t nav_roll; // deg * 100 : target roll angle
static int32_t nav_pitch; // deg * 100 : target pitch angle static int32_t nav_pitch; // deg * 100 : target pitch angle
static int32_t nav_yaw; // deg * 100 : target yaw angle static int32_t nav_yaw; // deg * 100 : target yaw angle
static int32_t auto_yaw; // deg * 100 : target yaw angle static int32_t auto_yaw; // deg * 100 : target yaw angle
static int32_t nav_lat; // for error calcs static int32_t nav_lat; // for error calcs
static int32_t nav_lon; // for error calcs static int32_t nav_lon; // for error calcs
static int16_t nav_throttle; // 0-1000 for throttle control static int16_t nav_throttle; // 0-1000 for throttle control
static int16_t crosstrack_error; static int16_t crosstrack_error;
static uint32_t throttle_integrator; // used to integrate throttle output to predict battery life static uint32_t throttle_integrator; // used to integrate throttle output to predict battery life
static bool invalid_throttle; // used to control when we calculate nav_throttle static bool invalid_throttle; // used to control when we calculate nav_throttle
@ -490,12 +489,12 @@ static int32_t wp_totalDistance; // meters - distance between old and next w
// repeating event control // repeating event control
// ----------------------- // -----------------------
static byte event_id; // what to do - see defines static byte event_id; // what to do - see defines
static uint32_t event_timer; // when the event was asked for in ms static uint32_t event_timer; // when the event was asked for in ms
static uint16_t event_delay; // how long to delay the next firing of event in millis static uint16_t event_delay; // how long to delay the next firing of event in millis
static int16_t event_repeat; // how many times to fire : 0 = forever, 1 = do once, 2 = do twice static int16_t event_repeat; // how many times to fire : 0 = forever, 1 = do once, 2 = do twice
static int16_t event_value; // per command value, such as PWM for servos static int16_t event_value; // per command value, such as PWM for servos
static int16_t event_undo_value; // the value used to undo commands static int16_t event_undo_value; // the value used to undo commands
//static byte repeat_forever; //static byte repeat_forever;
//static byte undo_event; // counter for timing the undo //static byte undo_event; // counter for timing the undo
@ -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 //