From 6f42442e57df9f853dea1697a3d4778b7657fa8b Mon Sep 17 00:00:00 2001 From: Jason Short Date: Thu, 29 Dec 2011 10:28:01 -0800 Subject: [PATCH] Added 5 sec Takeoff Timer to clear i terms when lifting Moved loiter relocation function to update_nav moved User hooks to correct place Added tuning for D term --- ArduCopter/ArduCopter.pde | 110 +++++++++++++++++++++++++++----------- 1 file changed, 80 insertions(+), 30 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index f570882ead..d7233c32eb 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -325,7 +325,6 @@ static int16_t rc_override[8] = {0,0,0,0,0,0,0,0}; static bool rc_override_active = false; static uint32_t rc_override_fs_timer = 0; - // Heli // ---- #if FRAME_CONFIG == HELI_FRAME @@ -450,6 +449,7 @@ static byte roll_pitch_mode; static byte throttle_mode; static boolean takeoff_complete; // Flag for using take-off controls +static int32_t takeoff_timer; // time since we throttled up static boolean land_complete; static int32_t old_alt; // used for managing altitude rates static int16_t velocity_land; @@ -464,7 +464,7 @@ static int32_t old_target_bearing; // used to track difference in angle static int16_t loiter_total; // deg : how many times to loiter * 360 static int16_t loiter_sum; // deg : how far we have turned around a waypoint -static uint32_t loiter_time; // millis : when we started LOITER mode +static uint32_t loiter_time; // millis : when we started LOITER mode static unsigned loiter_time_max; // millis : how long to stay in LOITER mode @@ -518,7 +518,6 @@ static int16_t event_undo_value; // the value used to undo commands // -------------- static int32_t condition_value; // used in condition commands (eg delay, change alt, etc.) static int32_t condition_start; -//static int16_t condition_rate; // land command // ------------ @@ -608,13 +607,19 @@ void loop() // --------------------- fast_loop(); } + + // port manipulation for external timing of main loops //PORTK &= B11101111; if ((timer - fiftyhz_loopTimer) >= 20000) { + // store the micros for the 50 hz timer fiftyhz_loopTimer = timer; + + // port manipulation for external timing of main loops //PORTK |= B01000000; // reads all of the necessary trig functions for cameras, throttle, etc. + // -------------------------------------------------------------------- update_trig(); // update our velocity estimate based on IMU at 50hz @@ -631,7 +636,7 @@ void loop() counter_one_herz++; - if(counter_one_herz == 50){ + if(counter_one_herz >= 50){ super_slow_loop(); counter_one_herz = 0; } @@ -675,10 +680,10 @@ static void fast_loop() //if(motor_armed) //Log_Write_Attitude(); -// agmatthews - USERHOOKS -#ifdef USERHOOK_FASTLOOP - USERHOOK_FASTLOOP -#endif + // agmatthews - USERHOOKS + #ifdef USERHOOK_FASTLOOP + USERHOOK_FASTLOOP + #endif } @@ -925,21 +930,9 @@ static void slow_loop() // ------------------------------- read_control_switch(); - // Read main battery voltage if hooked up - does not read the 5v from radio - // ------------------------------------------------------------------------ - //#if BATTERY_EVENT == 1 - // read_battery(); - //#endif - - #if AUTO_RESET_LOITER == 1 - if(control_mode == LOITER){ - if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 500){ - // reset LOITER to current position - next_WP = current_loc; - // clear Loiter Iterms - reset_nav(); - } - } + // agmatthews - USERHOOKS + #ifdef USERHOOK_SLOWLOOP + USERHOOK_SLOWLOOP #endif break; @@ -972,11 +965,6 @@ static void slow_loop() break; } - // agmatthews - USERHOOKS - #ifdef USERHOOK_SLOWLOOP - USERHOOK_SLOWLOOP - #endif - } // 1Hz loop @@ -1208,6 +1196,7 @@ void update_roll_pitch_mode(void) #define THROTTLE_FILTER_SIZE 4 // 50 hz update rate, not 250 +// controls all throttle behavior void update_throttle_mode(void) { int16_t throttle_out; @@ -1226,11 +1215,43 @@ void update_throttle_mode(void) //throttle_avg = throttle_avg * .98 + rc_3.control_in * .02; //g.throttle_cruise = throttle_avg; } + + // Code to manage the Copter state + if ((millis() - takeoff_timer) > 5000){ + // we must be in the air by now + // stop resetting rate_I() + takeoff_complete = true; + + }else{ + // reset these I terms to prevent flips on takeoff + g.pi_rate_roll.reset_I(); + g.pi_rate_pitch.reset_I(); + } }else{ + // we are on the ground + takeoff_complete = false; + + // reset baro data if we are near home + if(home_distance < 4 || GPS_enabled == false){ + // causes Baro to do a quick recalibration + // XXX commented until further testing + // reset_baro(); + } + + // reset out i terms and takeoff timer + // ----------------------------------- g.pi_stabilize_roll.reset_I(); g.pi_stabilize_pitch.reset_I(); g.pi_rate_roll.reset_I(); g.pi_rate_pitch.reset_I(); + + // remember our time since takeoff + // ------------------------------- + takeoff_timer = millis(); + + // make sure we also request 0 throttle out + // so the props stop ... properly + // ---------------------------------------- g.rc_3.servo_out = 0; } break; @@ -1238,6 +1259,7 @@ void update_throttle_mode(void) case THROTTLE_HOLD: // allow interactive changing of atitude adjust_altitude(); + // fall through case THROTTLE_AUTO: @@ -1255,6 +1277,14 @@ void update_throttle_mode(void) // reset next_WP.alt and don't go below 1 meter next_WP.alt = max(current_loc.alt, 100); + /* + Serial.printf("tar_alt: %d, actual_alt: %d \talt_err: %d, \t manb: %d\n", + next_WP.alt, + current_loc.alt, + altitude_error, + manual_boost); + //*/ + }else{ // 10hz, don't run up i term if(invalid_throttle && motor_auto_armed == true){ @@ -1346,7 +1376,21 @@ static void update_navigation() // switch passthrough to LOITER case LOITER: case POSITION: - wp_control = LOITER_MODE; + // This feature allows us to reposition the quad when the user lets + // go of the sticks + if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 500){ + // this sets the copter to not try and nav while we control it + wp_control = NO_NAV_MODE; + + // reset LOITER to current position + next_WP = current_loc; + + // clear Loiter Iterms + reset_nav(); + }else{ + // this is also set by GPS in update_nav + wp_control = LOITER_MODE; + } // calculates the desired Roll and Pitch update_nav_wp(); @@ -1461,7 +1505,7 @@ static void update_altitude() baro_rate = (temp + baro_rate) >> 1; old_baro_alt = baro_alt; - // sonar_alt is calculaed in a faster loop and filtered with a mode filter + // sonar_alt is calculated in a faster loop and filtered with a mode filter #endif @@ -1563,6 +1607,12 @@ static void tuning(){ thrust = tuning_value * .2; break;*/ + case CH6_DAMP: + g.rc_6.set_range(0,1500); // 0 to 1 + //thrust = tuning_value * .2; + g.stablize_d.set(tuning_value); + break; + case CH6_STABILIZE_KP: g.rc_6.set_range(0,8000); // 0 to 8 g.pi_stabilize_roll.kP(tuning_value);