mirror of https://github.com/ArduPilot/ardupilot
simple patch to improve alt hold based on stability patch
This commit is contained in:
Jason Short
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
|
||||
|
|
@ -440,32 +439,32 @@ static byte throttle_mode;
|
|||
static boolean takeoff_complete; // Flag for using take-off controls
|
||||
static boolean land_complete;
|
||||
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 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 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
|
||||
|
||||
// Loiter management
|
||||
// -----------------
|
||||
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 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_total; // deg : how many times to loiter * 360
|
||||
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 unsigned loiter_time_max; // millis : how long to stay in LOITER mode
|
||||
|
||||
|
||||
// these are the values for navigation control functions
|
||||
// ----------------------------------------------------
|
||||
static int32_t nav_roll; // deg * 100 : target roll angle
|
||||
static int32_t nav_pitch; // deg * 100 : target pitch angle
|
||||
static int32_t nav_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_lon; // for error calcs
|
||||
static int16_t nav_throttle; // 0-1000 for throttle control
|
||||
static int16_t crosstrack_error;
|
||||
static int32_t nav_roll; // deg * 100 : target roll angle
|
||||
static int32_t nav_pitch; // deg * 100 : target pitch angle
|
||||
static int32_t nav_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_lon; // for error calcs
|
||||
static int16_t nav_throttle; // 0-1000 for throttle control
|
||||
static int16_t crosstrack_error;
|
||||
|
||||
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
|
||||
|
|
@ -490,12 +489,12 @@ static int32_t wp_totalDistance; // meters - distance between old and next w
|
|||
|
||||
// repeating event control
|
||||
// -----------------------
|
||||
static byte event_id; // what to do - see defines
|
||||
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 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_undo_value; // the value used to undo commands
|
||||
static byte event_id; // what to do - see defines
|
||||
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 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_undo_value; // the value used to undo commands
|
||||
//static byte repeat_forever;
|
||||
//static byte undo_event; // counter for timing the undo
|
||||
|
||||
|
|
@ -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