This commit is contained in:
Michael Oborne 2012-01-05 07:51:33 +08:00
commit 53e4880cc1
5 changed files with 31 additions and 3 deletions

View File

@ -1419,7 +1419,7 @@ void update_roll_pitch_mode(void)
// new radio frame is used to make sure we only call this at 50hz
void update_simple_mode(void)
{
float simple_sin_y, simple_cos_x;
float simple_sin_y=0, simple_cos_x=0;
// used to manage state machine
// which improves speed of function

View File

@ -109,7 +109,7 @@ dump_log(uint8_t argc, const Menu::arg *argv)
last_log_num = DataFlash.find_last_log();
if (dump_log == -2) {
for(int count=1; count<=DataFlash.df_NumPages; count++) {
for(uint16_t count=1; count<=DataFlash.df_NumPages; count++) {
DataFlash.StartRead(count);
Serial.printf_P(PSTR("DF page, log file #, log page: %d,\t"), count);
Serial.printf_P(PSTR("%d,\t"), DataFlash.GetFileNumber());

View File

@ -387,7 +387,7 @@ static bool verify_land()
if((current_loc.alt - home.alt) < 300 && velocity_land <= 100){
land_complete = true;
// reset old_alt
old_alt == 0;
old_alt = 0;
init_disarm_motors();
return true;
}

View File

@ -243,6 +243,7 @@ static void reduce_wind_compensation()
tmp *= .98;
g.pi_loiter_lat.set_integrator(tmp); // Y
#if 0
// debug
int16_t t1 = g.pi_loiter_lon.get_integrator();
int16_t t2 = g.pi_loiter_lon.get_integrator();
@ -250,6 +251,7 @@ static void reduce_wind_compensation()
//Serial.printf("reduce wind iterm X:%d Y:%d \n",
// t1,
// t2);
#endif
}
static int16_t calc_desired_speed(int16_t max_speed)

View File

@ -361,6 +361,28 @@ static void init_ardupilot()
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();
}
//********************************************************************************
//This function does all the calibrations, etc. that we need during a ground start
//********************************************************************************
@ -380,6 +402,10 @@ static void startup_ground(void)
// reset the leds
// ---------------------------
clear_leds();
// when we re-calibrate the gyros, all previous I values now
// make no sense
reset_I_all();
}
/*