From 9ee3431928232363fea0db604400690804ce4f70 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 5 Jan 2012 10:13:36 +1100 Subject: [PATCH 1/5] ACM-simple: don't use uninitialised simple trig values --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 6f14ac7d77..6c0bf27bf4 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -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 From 3c965ff1a25adfebb8d9ae67f11956d28908b488 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 5 Jan 2012 10:14:12 +1100 Subject: [PATCH 2/5] ACM: fixed a signed/unsigned warning --- ArduCopter/Log.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 57e0af9584..cfc71a7454 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -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()); From 734b0a02beb4a80903aee38e8c6f661d7e4c8b84 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 5 Jan 2012 10:14:30 +1100 Subject: [PATCH 3/5] ACM: fixed a alt hold bug == instead of = --- ArduCopter/commands_logic.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 2401e8e4a5..631f54d931 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -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; } From a6808162d62536c952125efc3cf3fec6389d0f3b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 5 Jan 2012 10:14:53 +1100 Subject: [PATCH 4/5] ACM: fixed a warning comment out some unused debug code --- ArduCopter/navigation.pde | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 6de094b29b..8cd1e99ee4 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -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) From 600a5680f86fc171c5aaade0daa9f32997a51813 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 5 Jan 2012 10:15:14 +1100 Subject: [PATCH 5/5] ACM: reset all I terms on gyro calibration --- ArduCopter/system.pde | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 3d1cb57cad..4c4656a611 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -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(); } /*