relocated reset_I term functions to central location

This commit is contained in:
Jason Short 2012-01-20 10:11:18 -08:00
parent f504c79fb0
commit 0475dbf8ae
3 changed files with 18 additions and 40 deletions

View File

@ -212,8 +212,7 @@ A_off: 201.95, -24.00, -88.56
static void trim_accel() static void trim_accel()
{ {
g.pi_stabilize_roll.reset_I(); reset_stability_I();
g.pi_stabilize_pitch.reset_I();
float trim_roll = (float)g.rc_1.control_in / 30000.0; float trim_roll = (float)g.rc_1.control_in / 30000.0;
float trim_pitch = (float)g.rc_2.control_in / 30000.0; float trim_pitch = (float)g.rc_2.control_in / 30000.0;

View File

@ -143,7 +143,7 @@ static void calc_location_error(struct Location *next_loc)
*/ */
#define NAV_ERR_MAX 800 #define NAV_ERR_MAX 800
static void calc_loiter(int x_error, int y_error) static void calc_loiter1(int x_error, int y_error)
{ {
int16_t lon_PI = g.pi_loiter_lon.get_pi(x_error, dTnav); int16_t lon_PI = g.pi_loiter_lon.get_pi(x_error, dTnav);
int16_t lon_D = 3 * x_actual_speed ; // this controls the small bumps int16_t lon_D = 3 * x_actual_speed ; // this controls the small bumps
@ -160,7 +160,7 @@ static void calc_loiter(int x_error, int y_error)
} }
static void calc_loiter1(int x_error, int y_error) static void calc_loiter(int x_error, int y_error)
{ {
// East/West // East/West
x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX); //800 x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX); //800
@ -389,15 +389,15 @@ static void set_new_altitude(int32_t _new_alt)
if(target_altitude > original_altitude){ if(target_altitude > original_altitude){
// we are below, going up // we are below, going up
alt_change_flag = ASCENDING; alt_change_flag = ASCENDING;
Serial.printf("go up\n"); //Serial.printf("go up\n");
}else if(target_altitude < original_altitude){ }else if(target_altitude < original_altitude){
// we are above, going down // we are above, going down
alt_change_flag = DESCENDING; alt_change_flag = DESCENDING;
Serial.printf("go down\n"); //Serial.printf("go down\n");
}else{ }else{
// No Change // No Change
alt_change_flag = REACHED_ALT; alt_change_flag = REACHED_ALT;
Serial.printf("reached alt\n"); //Serial.printf("reached alt\n");
} }
//Serial.printf("new alt: %d Org alt: %d\n", target_altitude, original_altitude); //Serial.printf("new alt: %d Org alt: %d\n", target_altitude, original_altitude);

View File

@ -361,29 +361,6 @@ static void init_ardupilot()
SendDebug("\nReady to FLY "); SendDebug("\nReady to FLY ");
} }
/*
reset all I integrators
*/
static void reset_I_all(void)
{
g.pi_rate_roll.reset_I();
g.pi_rate_pitch.reset_I();
g.pi_rate_yaw.reset_I();
g.pi_stabilize_roll.reset_I();
g.pi_stabilize_pitch.reset_I();
g.pi_stabilize_yaw.reset_I();
g.pi_loiter_lat.reset_I();
g.pi_loiter_lon.reset_I();
g.pi_nav_lat.reset_I();
g.pi_nav_lon.reset_I();
g.pi_alt_hold.reset_I();
g.pi_throttle.reset_I();
g.pi_acro_roll.reset_I();
g.pi_acro_pitch.reset_I();
g.pi_optflow_roll.reset_I();
g.pi_optflow_pitch.reset_I();
}
//******************************************************************************** //********************************************************************************
//This function does all the calibrations, etc. that we need during a ground start //This function does all the calibrations, etc. that we need during a ground start
@ -405,8 +382,8 @@ static void startup_ground(void)
// --------------------------- // ---------------------------
clear_leds(); clear_leds();
// when we re-calibrate the gyros, all previous I values now // when we re-calibrate the gyros,
// make no sense // all previous I values are invalid
reset_I_all(); reset_I_all();
} }
@ -473,7 +450,6 @@ static void set_mode(byte mode)
yaw_mode = YAW_ACRO; yaw_mode = YAW_ACRO;
roll_pitch_mode = ROLL_PITCH_ACRO; roll_pitch_mode = ROLL_PITCH_ACRO;
throttle_mode = THROTTLE_MANUAL; throttle_mode = THROTTLE_MANUAL;
reset_rate_I();
break; break;
case STABILIZE: case STABILIZE:
@ -561,14 +537,19 @@ static void set_mode(byte mode)
update_throttle_cruise(); update_throttle_cruise();
}else { }else {
// an automatic throttle // an automatic throttle
// todo: replace with a throttle cruise estimator
init_throttle_cruise(); init_throttle_cruise();
} }
if(roll_pitch_mode <= ROLL_PITCH_ACRO){ if(roll_pitch_mode <= ROLL_PITCH_ACRO){
// We are under manual attitude control // We are under manual attitude control
// removes the navigation from roll and pitch commands, but leaves the wind compensation // remove the navigation from roll and pitch command
reset_nav(); reset_nav_params();
// remove the wind compenstaion
reset_wind_I();
// Clears the WP navigation speed compensation
reset_nav_I();
// Clears the alt hold compensation
reset_throttle_I();
} }
Log_Write_Mode(control_mode); Log_Write_Mode(control_mode);
@ -608,8 +589,7 @@ static void update_throttle_cruise()
int16_t tmp = g.pi_alt_hold.get_integrator(); int16_t tmp = g.pi_alt_hold.get_integrator();
if(tmp != 0){ if(tmp != 0){
g.throttle_cruise += tmp; g.throttle_cruise += tmp;
g.pi_alt_hold.reset_I(); reset_throttle_I();
g.pi_throttle.reset_I();
} }
} }
@ -619,8 +599,7 @@ init_throttle_cruise()
#if AUTO_THROTTLE_HOLD == 0 #if AUTO_THROTTLE_HOLD == 0
// are we moving from manual throttle to auto_throttle? // are we moving from manual throttle to auto_throttle?
if((old_control_mode <= STABILIZE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){ if((old_control_mode <= STABILIZE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){
g.pi_throttle.reset_I(); reset_throttle_I();
g.pi_alt_hold.reset_I();
g.throttle_cruise.set_and_save(g.rc_3.control_in); g.throttle_cruise.set_and_save(g.rc_3.control_in);
} }
#endif #endif