mirror of https://github.com/ArduPilot/ardupilot
ACM -
Implemented automatic ranging of Alt Hold gains. Works well in simulator and testing. - alt hold estimation moved to 50 hz - simple fixed observer calc for smooth and accurate climb rates useful for derivative calcs - auto-reset of the I term by moving I value into throttle value. This recalcs the gain every 20seconds for battery drainage compensation in long flights. - remove filtering for Nav_throttle - added a way to lower the gain on nav_throttle for descents by / climb_rate error by 2 - seems to work OK and keeps copter from dropping like a rock when the Baro drifts quickly lower. - removed old throttle hold set point code - made throttle override for alt hold +- 200 vs 250
This commit is contained in:
Jason Short
parent
5117ddff26
commit
43b3e1ccd1
|
|
@ -568,7 +568,11 @@ static int32_t ground_pressure;
|
|||
static int16_t ground_temperature;
|
||||
// The cm we are off in altitude from next_WP.alt – Positive value means we are below the WP
|
||||
static int32_t altitude_error;
|
||||
// The cm/s we are moving up or down - Positive = UP
|
||||
// The cm/s we are moving up or down based on sensor data - Positive = UP
|
||||
static int16_t climb_rate_actual;
|
||||
// Used to dither our climb_rate over 50hz
|
||||
static int16_t climb_rate_error;
|
||||
// The cm/s we are moving up or down based on filtered data - Positive = UP
|
||||
static int16_t climb_rate;
|
||||
// The altitude as reported by Sonar in cm – Values are 20 to 700 generally.
|
||||
static int16_t sonar_alt;
|
||||
|
|
@ -578,8 +582,10 @@ static int16_t sonar_rate;
|
|||
static int32_t baro_alt;
|
||||
// The climb_rate as reported by Baro in cm/s
|
||||
static int16_t baro_rate;
|
||||
//
|
||||
// used to switch out of Manual Boost
|
||||
static boolean reset_throttle_flag;
|
||||
// used to track when to read sensors vs estimate alt
|
||||
static boolean alt_sensor_flag;
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
|
@ -690,13 +696,6 @@ static int16_t nav_throttle; // 0-1000 for throttle control
|
|||
// This is a simple counter to track the amount of throttle used during flight
|
||||
// This could be useful later in determining and debuging current usage and predicting battery life
|
||||
static uint32_t throttle_integrator;
|
||||
// This is a future value for replacing the throttle_cruise setup procedure. It's an average of throttle control
|
||||
// that is generated when the climb rate is within a certain threshold
|
||||
//static float throttle_avg = THROTTLE_CRUISE;
|
||||
// This is a flag used to trigger the updating of nav_throttle at 10hz
|
||||
static bool invalid_throttle;
|
||||
// Used to track the altitude offset for climbrate control
|
||||
//static int32_t target_altitude;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Climb rate control
|
||||
|
|
@ -1005,8 +1004,9 @@ static void medium_loop()
|
|||
// -----------------------------
|
||||
update_navigation();
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_NTUN)
|
||||
if (g.log_bitmask & MASK_LOG_NTUN && motor_armed){
|
||||
Log_Write_Nav_Tuning();
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
|
@ -1018,13 +1018,10 @@ static void medium_loop()
|
|||
|
||||
// Read altitude from sensors
|
||||
// --------------------------
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode
|
||||
update_altitude();
|
||||
#endif
|
||||
|
||||
// invalidate the throttle hold value
|
||||
// ----------------------------------
|
||||
invalid_throttle = true;
|
||||
//#if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode
|
||||
//update_altitude();
|
||||
//#endif
|
||||
alt_sensor_flag = true;
|
||||
|
||||
break;
|
||||
|
||||
|
|
@ -1045,16 +1042,13 @@ static void medium_loop()
|
|||
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED)
|
||||
Log_Write_Attitude();
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_CTUN)
|
||||
Log_Write_Control_Tuning();
|
||||
if (g.log_bitmask & MASK_LOG_MOTORS)
|
||||
Log_Write_Motors();
|
||||
}
|
||||
|
||||
// send all requested output streams with rates requested
|
||||
// between 5 and 45 Hz
|
||||
gcs_data_stream_send(5,45);
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_MOTORS)
|
||||
Log_Write_Motors();
|
||||
// send all requested output streams with rates requested
|
||||
// between 5 and 45 Hz
|
||||
gcs_data_stream_send(5,45);
|
||||
|
||||
break;
|
||||
|
||||
|
|
@ -1102,6 +1096,10 @@ static void medium_loop()
|
|||
// ---------------------------
|
||||
static void fifty_hz_loop()
|
||||
{
|
||||
// read altitude sensors or estimate altitude
|
||||
// ------------------------------------------
|
||||
update_altitude_est();
|
||||
|
||||
// moved to slower loop
|
||||
// --------------------
|
||||
update_throttle_mode();
|
||||
|
|
@ -1162,6 +1160,11 @@ static void slow_loop()
|
|||
slow_loopCounter++;
|
||||
superslow_loopCounter++;
|
||||
|
||||
// update throttle hold every 20 seconds
|
||||
if(superslow_loopCounter > 60){
|
||||
update_throttle_cruise();
|
||||
}
|
||||
|
||||
if(superslow_loopCounter > 1200){
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
if(g.rc_3.control_in == 0 && control_mode == STABILIZE && g.compass_enabled){
|
||||
|
|
@ -1219,13 +1222,14 @@ static void slow_loop()
|
|||
// 1Hz loop
|
||||
static void super_slow_loop()
|
||||
{
|
||||
if (g.log_bitmask & MASK_LOG_CUR)
|
||||
if (g.log_bitmask & MASK_LOG_CUR && motor_armed)
|
||||
Log_Write_Current();
|
||||
|
||||
// this function disarms the copter if it has been sitting on the ground for any moment of time greater than 30s
|
||||
// but only of the control mode is manual
|
||||
if((control_mode <= ACRO) && (g.rc_3.control_in == 0)){
|
||||
auto_disarming_counter++;
|
||||
|
||||
if(auto_disarming_counter == AUTO_ARMING_DELAY){
|
||||
init_disarm_motors();
|
||||
}else if (auto_disarming_counter > AUTO_ARMING_DELAY){
|
||||
|
|
@ -1234,8 +1238,10 @@ static void super_slow_loop()
|
|||
}else{
|
||||
auto_disarming_counter = 0;
|
||||
}
|
||||
|
||||
gcs_send_message(MSG_HEARTBEAT);
|
||||
gcs_data_stream_send(1,3);
|
||||
|
||||
// agmatthews - USERHOOKS
|
||||
#ifdef USERHOOK_SUPERSLOWLOOP
|
||||
USERHOOK_SUPERSLOWLOOP
|
||||
|
|
@ -1359,12 +1365,13 @@ static void update_GPS(void)
|
|||
current_loc.lng = g_gps->longitude; // Lon * 10 * *7
|
||||
current_loc.lat = g_gps->latitude; // Lat * 10 * *7
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_GPS){
|
||||
if (g.log_bitmask & MASK_LOG_GPS && motor_armed){
|
||||
Log_Write_GPS();
|
||||
}
|
||||
|
||||
#if HIL_MODE == HIL_MODE_ATTITUDE // only execute in HIL mode
|
||||
update_altitude();
|
||||
//update_altitude();
|
||||
alt_sensor_flag = true;
|
||||
#endif
|
||||
|
||||
}
|
||||
|
|
@ -1635,10 +1642,10 @@ void update_throttle_mode(void)
|
|||
if(reset_throttle_flag) {
|
||||
set_new_altitude(max(current_loc.alt, 100));
|
||||
reset_throttle_flag = false;
|
||||
update_throttle_cruise();
|
||||
}
|
||||
|
||||
// 10hz, don't run up i term
|
||||
if(invalid_throttle && motor_auto_armed == true){
|
||||
if(motor_auto_armed == true){
|
||||
|
||||
// how far off are we
|
||||
altitude_error = get_altitude_error();
|
||||
|
|
@ -1646,8 +1653,6 @@ void update_throttle_mode(void)
|
|||
// get the AP throttle
|
||||
nav_throttle = get_nav_throttle(altitude_error);
|
||||
|
||||
// clear the new data flag
|
||||
invalid_throttle = false;
|
||||
/*
|
||||
Serial.printf("tar_alt: %d, actual_alt: %d \talt_err: %d, \tnav_thr: %d, \talt Int: %d\n",
|
||||
next_WP.alt,
|
||||
|
|
@ -1656,6 +1661,7 @@ void update_throttle_mode(void)
|
|||
nav_throttle,
|
||||
(int16_t)g.pi_alt_hold.get_integrator());
|
||||
//*/
|
||||
|
||||
}
|
||||
|
||||
// hack to remove the influence of the ground effect
|
||||
|
|
@ -1864,7 +1870,6 @@ static void update_altitude()
|
|||
{
|
||||
static int16_t old_sonar_alt = 0;
|
||||
static int32_t old_baro_alt = 0;
|
||||
static int16_t old_climb_rate = 0;
|
||||
|
||||
#if HIL_MODE == HIL_MODE_ATTITUDE
|
||||
// we are in the SIM, fake out the baro and Sonar
|
||||
|
|
@ -1889,7 +1894,6 @@ static void update_altitude()
|
|||
// Note: sonar_alt is calculated in a faster loop and filtered with a mode filter
|
||||
#endif
|
||||
|
||||
|
||||
if(g.sonar_enabled){
|
||||
// filter out offset
|
||||
float scale;
|
||||
|
|
@ -1921,35 +1925,47 @@ static void update_altitude()
|
|||
current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt;
|
||||
|
||||
// solve for a blended climb_rate
|
||||
climb_rate = ((float)sonar_rate * (1.0 - scale)) + (float)baro_rate * scale;
|
||||
climb_rate_actual = ((float)sonar_rate * (1.0 - scale)) + (float)baro_rate * scale;
|
||||
|
||||
}else{
|
||||
// we must be higher than sonar (>800), don't get tricked by bad sonar reads
|
||||
current_loc.alt = baro_alt + home.alt; // home alt = 0
|
||||
// dont blend, go straight baro
|
||||
climb_rate = baro_rate;
|
||||
climb_rate_actual = baro_rate;
|
||||
}
|
||||
|
||||
}else{
|
||||
// NO Sonar case
|
||||
current_loc.alt = baro_alt + home.alt;
|
||||
climb_rate = baro_rate;
|
||||
climb_rate_actual = baro_rate;
|
||||
}
|
||||
|
||||
// simple smoothing
|
||||
climb_rate = (climb_rate + old_climb_rate)>>1;
|
||||
|
||||
// manage bad data
|
||||
climb_rate = constrain(climb_rate, -800, 800);
|
||||
|
||||
// save for filtering
|
||||
old_climb_rate = climb_rate;
|
||||
|
||||
// update the target altitude
|
||||
next_WP.alt = get_new_altitude();
|
||||
|
||||
// calc error
|
||||
climb_rate_error = (climb_rate_actual - climb_rate) / 5;
|
||||
}
|
||||
|
||||
#define THROTTLE_ADJUST 250
|
||||
static void update_altitude_est()
|
||||
{
|
||||
if(alt_sensor_flag){
|
||||
update_altitude();
|
||||
alt_sensor_flag = false;
|
||||
|
||||
if(g.log_bitmask & MASK_LOG_CTUN && motor_armed){
|
||||
Log_Write_Control_Tuning();
|
||||
}
|
||||
|
||||
}else{
|
||||
// simple dithering of climb rate
|
||||
climb_rate += climb_rate_error;
|
||||
current_loc.alt += (climb_rate / 50);
|
||||
}
|
||||
//Serial.printf(" %d, %d, %d, %d\n", climb_rate_actual, climb_rate_error, climb_rate, current_loc.alt);
|
||||
}
|
||||
|
||||
#define THROTTLE_ADJUST 200
|
||||
static void
|
||||
adjust_altitude()
|
||||
{
|
||||
|
|
|
|||
|
|
@ -186,7 +186,7 @@ get_rate_yaw(int32_t target_rate)
|
|||
static int16_t
|
||||
get_nav_throttle(int32_t z_error)
|
||||
{
|
||||
static int16_t old_output = 0;
|
||||
//static int16_t old_output = 0;
|
||||
int16_t rate_error = 0;
|
||||
int16_t output = 0;
|
||||
|
||||
|
|
@ -198,19 +198,23 @@ get_nav_throttle(int32_t z_error)
|
|||
z_error = constrain(z_error, -400, 400);
|
||||
|
||||
// compensates throttle setpoint error for hovering
|
||||
int16_t iterm = g.pi_alt_hold.get_i(z_error, .1);
|
||||
int16_t iterm = g.pi_alt_hold.get_i(z_error, .02);
|
||||
|
||||
// calculate rate error
|
||||
rate_error = rate_error - climb_rate;
|
||||
|
||||
// hack to see if we can smooth out oscillations
|
||||
if(rate_error < 0)
|
||||
rate_error = rate_error >> 1;
|
||||
|
||||
// limit the rate
|
||||
output = constrain(g.pid_throttle.get_pid(rate_error, .1), -160, 180);
|
||||
output = constrain(g.pid_throttle.get_pid(rate_error, .02), -80, 120);
|
||||
|
||||
// light filter of output
|
||||
output = (old_output + output) / 2;
|
||||
//output = (old_output + output) / 2;
|
||||
|
||||
// save our output
|
||||
old_output = output;
|
||||
//old_output = output;
|
||||
|
||||
// output control:
|
||||
return output + iterm;
|
||||
|
|
@ -219,8 +223,6 @@ get_nav_throttle(int32_t z_error)
|
|||
// Keeps old data out of our calculation / logs
|
||||
static void reset_nav_params(void)
|
||||
{
|
||||
// forces us to update nav throttle
|
||||
invalid_throttle = true;
|
||||
nav_throttle = 0;
|
||||
|
||||
// always start Circle mode at same angle
|
||||
|
|
|
|||
|
|
@ -741,7 +741,7 @@
|
|||
# define THROTTLE_P 0.45 //
|
||||
#endif
|
||||
#ifndef THROTTLE_I
|
||||
# define THROTTLE_I 0.0 //
|
||||
# define THROTTLE_I 0.0 // Don't edit
|
||||
#endif
|
||||
#ifndef THROTTLE_D
|
||||
# define THROTTLE_D 0.02 //
|
||||
|
|
|
|||
|
|
@ -186,9 +186,6 @@ static void auto_trim()
|
|||
|
||||
//Serial.println("Done");
|
||||
auto_level_counter = 0;
|
||||
|
||||
// set TC
|
||||
init_throttle_cruise();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -532,16 +532,8 @@ static void set_mode(byte mode)
|
|||
motor_auto_armed = true;
|
||||
}
|
||||
|
||||
if(throttle_mode == THROTTLE_MANUAL){
|
||||
// reset all of the throttle iterms
|
||||
update_throttle_cruise();
|
||||
|
||||
// reset auto_throttle
|
||||
nav_throttle = 0;
|
||||
}else {
|
||||
// an automatic throttle
|
||||
init_throttle_cruise();
|
||||
}
|
||||
// called to calculate gain for alt hold
|
||||
update_throttle_cruise();
|
||||
|
||||
if(roll_pitch_mode <= ROLL_PITCH_ACRO){
|
||||
// We are under manual attitude control
|
||||
|
|
@ -594,18 +586,10 @@ static void update_throttle_cruise()
|
|||
g.throttle_cruise += tmp;
|
||||
reset_throttle_I();
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
init_throttle_cruise()
|
||||
{
|
||||
#if AUTO_THROTTLE_HOLD == 0
|
||||
// are we moving from manual throttle to auto_throttle?
|
||||
if((old_control_mode <= STABILIZE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){
|
||||
reset_throttle_I();
|
||||
g.throttle_cruise.set_and_save(g.rc_3.control_in);
|
||||
}
|
||||
#endif
|
||||
// recalc kp
|
||||
g.pid_throttle.kP((float)g.throttle_cruise.get() / 981.0);
|
||||
//Serial.printf("kp:%1.4f\n",kp);
|
||||
}
|
||||
|
||||
#if CLI_SLIDER_ENABLED == ENABLED && CLI_ENABLED == ENABLED
|
||||
|
|
|
|||
Loading…
Reference in New Issue