Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 01:18:29 -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 long initial_simple_bearing; // used for Simple mode
static float simple_sin_y, simple_cos_x;
static float boost; // used to give a little extra to maintain altitude
// Acro
#if CH7_OPTION == CH7_FLIP
@ -420,7 +419,7 @@ static long nav_lat; // for error calcs
static long nav_lon; // for error calcs
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 set_throttle_cruise_flag = false; // used to track the throttle crouse value
@ -693,10 +692,6 @@ static void medium_loop()
// ----------------------------------
invalid_throttle = true;
// calc boost
// ----------
boost = get_angle_boost();
break;
// This case deals with sending high rate telemetry
@ -965,7 +960,7 @@ static void update_GPS(void)
// so that the altitude is more accurate
// -------------------------------------
if (current_loc.lat == 0) {
SendDebugln("!! bad loc");
//SendDebugln("!! bad loc");
ground_start_count = 5;
}else{
@ -1096,7 +1091,7 @@ void update_throttle_mode(void)
case THROTTLE_MANUAL:
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{
g.rc_3.servo_out = 0;
}
@ -1116,13 +1111,14 @@ void update_throttle_mode(void)
// get the AP throttle
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
invalid_throttle = false;
}
// 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;
}
}

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;
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
@ -162,6 +162,9 @@ static void reset_nav(void)
nav_throttle = 0;
invalid_throttle = true;
// clear the throttle timer
throttle_timer = millis();
g.pi_nav_lat.reset_I();
g.pi_nav_lon.reset_I();
@ -174,14 +177,6 @@ static void reset_nav(void)
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
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;
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
#ifndef RTL_THR
# define RTL_THR THROTTLE_AUTO
# define RTL_THR THROTTLE_HOLD
#endif
@ -513,7 +513,7 @@
# define THROTTLE_P 0.6 //
#endif
#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
#ifndef THROTTLE_IMAX
# define THROTTLE_IMAX 300
@ -670,7 +670,7 @@
#endif
#ifndef ALT_HOLD_HOME
# define ALT_HOLD_HOME 8
# define ALT_HOLD_HOME -1
#endif
#ifndef USE_CURRENT_ALT