Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

Fixed Throttle integrator bug. Time was not being reset causing a big drop in throttle control.

Browse Source
This commit is contained in:
Jason Short 2011-09-22 12:28:46 -07:00
parent 6abb8a0796
commit 078f64e276
3 changed files with 14 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduCopter/ArduCopter.pde
View File

@ -341,7 +341,6 @@ static float cos_yaw_x = 1;
static float sin_pitch_y, sin_yaw_y, sin_roll_y; static float sin_pitch_y, sin_yaw_y, sin_roll_y;
static long initial_simple_bearing; // used for Simple mode static long initial_simple_bearing; // used for Simple mode
static float simple_sin_y, simple_cos_x; static float simple_sin_y, simple_cos_x;
static float boost; // used to give a little extra to maintain altitude
// Acro // Acro
#if CH7_OPTION == CH7_FLIP #if CH7_OPTION == CH7_FLIP
@ -420,7 +419,7 @@ static long nav_lat; // for error calcs
static long nav_lon; // for error calcs static long nav_lon; // for error calcs
static int nav_throttle; // 0-1000 for throttle control static int nav_throttle; // 0-1000 for throttle control
static long throttle_integrator; // used to integrate throttle output to predict battery life static unsigned long 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
//static bool set_throttle_cruise_flag = false; // used to track the throttle crouse value //static bool set_throttle_cruise_flag = false; // used to track the throttle crouse value
@ -693,10 +692,6 @@ static void medium_loop()
// ---------------------------------- // ----------------------------------
invalid_throttle = true; invalid_throttle = true;
// calc boost
// ----------
boost = get_angle_boost();
break; break;
// This case deals with sending high rate telemetry // This case deals with sending high rate telemetry
@ -965,7 +960,7 @@ static void update_GPS(void)
// so that the altitude is more accurate // so that the altitude is more accurate
// ------------------------------------- // -------------------------------------
if (current_loc.lat == 0) { if (current_loc.lat == 0) {
SendDebugln("!! bad loc"); //SendDebugln("!! bad loc");
ground_start_count = 5; ground_start_count = 5;
}else{ }else{
@ -1096,7 +1091,7 @@ void update_throttle_mode(void)
case THROTTLE_MANUAL: case THROTTLE_MANUAL:
if (g.rc_3.control_in > 0){ if (g.rc_3.control_in > 0){
g.rc_3.servo_out = g.rc_3.control_in + boost; g.rc_3.servo_out = g.rc_3.control_in + get_angle_boost();
}else{ }else{
g.rc_3.servo_out = 0; g.rc_3.servo_out = 0;
} }
@ -1116,13 +1111,14 @@ void update_throttle_mode(void)
// get the AP throttle // get the AP throttle
nav_throttle = get_nav_throttle(altitude_error, 200); //150 = target speed of 1.5m/s nav_throttle = get_nav_throttle(altitude_error, 200); //150 = target speed of 1.5m/s
//Serial.printf("in:%d, cr:%d, NT:%d, I:%1.4f\n", g.rc_3.control_in,altitude_error, nav_throttle, g.pi_throttle.get_integrator());
// clear the new data flag // clear the new data flag
invalid_throttle = false; invalid_throttle = false;
} }
// apply throttle control at 200 hz // apply throttle control at 200 hz
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + boost; g.rc_3.servo_out = g.throttle_cruise + nav_throttle + get_angle_boost();
break; break;
} }
} }

17
ArduCopter/Attitude.pde
View File

@ -104,7 +104,7 @@ get_nav_throttle(long z_error, int target_speed)
float delta_throttle = (float)(timer - throttle_timer)/1000.0; float delta_throttle = (float)(timer - throttle_timer)/1000.0;
throttle_timer = timer; throttle_timer = timer;
return g.pi_throttle.get_pi(rate_error, delta_throttle); return (int)g.pi_throttle.get_pi(rate_error, delta_throttle);
} }
static int static int
@ -162,6 +162,9 @@ static void reset_nav(void)
nav_throttle = 0; nav_throttle = 0;
invalid_throttle = true; invalid_throttle = true;
// clear the throttle timer
throttle_timer = millis();
g.pi_nav_lat.reset_I(); g.pi_nav_lat.reset_I();
g.pi_nav_lon.reset_I(); g.pi_nav_lon.reset_I();
@ -174,14 +177,6 @@ static void reset_nav(void)
throttle control throttle control
****************************************************************/ ****************************************************************/
// user input:
// -----------
//static int get_throttle(int throttle_input)
//{
// throttle_input = (float)throttle_input * angle_boost();
// return max(throttle_input, 0);
//}
static long static long
get_nav_yaw_offset(int yaw_input, int reset) get_nav_yaw_offset(int yaw_input, int reset)
{ {
@ -217,10 +212,10 @@ static int alt_hold_velocity()
} }
*/ */
static float get_angle_boost() static int get_angle_boost()
{ {
float temp = cos_pitch_x * cos_roll_x; float temp = cos_pitch_x * cos_roll_x;
temp = 2.0 - constrain(temp, .5, 1.0); temp = 2.0 - constrain(temp, .5, 1.0);
return temp; return (int)(temp * 50.0);
} }

6
ArduCopter/config.h
View File

@ -393,7 +393,7 @@
#endif #endif
#ifndef RTL_THR #ifndef RTL_THR
# define RTL_THR THROTTLE_AUTO # define RTL_THR THROTTLE_HOLD
#endif #endif
@ -513,7 +513,7 @@
# define THROTTLE_P 0.6 // # define THROTTLE_P 0.6 //
#endif #endif
#ifndef THROTTLE_I #ifndef THROTTLE_I
# define THROTTLE_I 0.10 // with 4m error, 12.5s windup # define THROTTLE_I 0.03 // with 4m error, 12.5s windup
#endif #endif
#ifndef THROTTLE_IMAX #ifndef THROTTLE_IMAX
# define THROTTLE_IMAX 300 # define THROTTLE_IMAX 300
@ -670,7 +670,7 @@
#endif #endif
#ifndef ALT_HOLD_HOME #ifndef ALT_HOLD_HOME
# define ALT_HOLD_HOME 8 # define ALT_HOLD_HOME -1
#endif #endif
#ifndef USE_CURRENT_ALT #ifndef USE_CURRENT_ALT