diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index b42df8a712..58054c71b7 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -490,9 +490,9 @@ static byte gps_watchdog; static unsigned long fast_loopTimer; // Time in miliseconds of main control loop static uint8_t delta_ms_fast_loop; // Delta Time in miliseconds -static unsigned long medium_loopTimer; // Time in miliseconds of navigation control loop static byte medium_loopCounter; // Counters for branching from main control loop to slower loops -static uint8_t delta_ms_medium_loop; +static uint16_t throttle_timer; +static float delta_throttle; static unsigned long fiftyhz_loopTimer; static uint8_t delta_ms_fiftyhz; @@ -502,7 +502,7 @@ static int superslow_loopCounter; static byte simple_timer; // for limiting the execution of flight mode thingys -static unsigned long dTnav; // Delta Time in milliseconds for navigation computations +static float dTnav; // Delta Time in milliseconds for navigation computations static unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav //static float load; // % MCU cycles used @@ -526,9 +526,6 @@ void loop() // ---------------------------- if (millis() - fast_loopTimer >= 4) { //PORTK |= B00010000; - delta_ms_fast_loop = millis() - fast_loopTimer; - fast_loopTimer = millis(); - G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator // Execute the fast loop // --------------------- @@ -573,18 +570,6 @@ void loop() // Main loop static void fast_loop() { - // IMU DCM Algorithm - read_AHRS(); - - // This is the fast loop - we want it to execute at >= 100Hz - // --------------------------------------------------------- - if (delta_ms_fast_loop > G_Dt_max) - G_Dt_max = delta_ms_fast_loop; - - // Read radio - // ---------- - read_radio(); - // try to send any deferred messages if the serial port now has // some space available gcs.send_message(MSG_RETRY_DEFERRED); @@ -592,6 +577,22 @@ static void fast_loop() hil.send_message(MSG_RETRY_DEFERRED); #endif + // Read radio + // ---------- + read_radio(); + + // IMU DCM Algorithm + read_AHRS(); + + fast_loopTimer = millis(); + delta_ms_fast_loop = millis() - fast_loopTimer; + G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by PI Loops + + // Look for slow loop times + // ------------------------ + if (delta_ms_fast_loop > G_Dt_max) + G_Dt_max = delta_ms_fast_loop; + // custom code/exceptions for flight modes // --------------------------------------- update_yaw_mode(); @@ -663,7 +664,7 @@ static void medium_loop() g_gps->new_data = false; // we are not tracking I term on navigation, so this isn't needed - dTnav = millis() - nav_loopTimer; + dTnav = (float)(millis() - nav_loopTimer)/ 1000.0; nav_loopTimer = millis(); // calculate the copter's desired bearing and WP distance @@ -745,9 +746,6 @@ static void medium_loop() loop_step = 5; medium_loopCounter = 0; - delta_ms_medium_loop = millis() - medium_loopTimer; - medium_loopTimer = millis(); - if (g.battery_monitoring != 0){ read_battery(); } @@ -1190,7 +1188,6 @@ static void update_navigation() // calculates desired Yaw update_auto_yaw(); { - //circle_angle += dTnav; //1000 * (dTnav/1000); circle_angle = wrap_360(target_bearing + 3000 + 18000); target_WP.lng = next_WP.lng + (g.loiter_radius * cos(radians(90 - circle_angle/100))); diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index ee7b041b4b..35193f2f31 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -13,13 +13,13 @@ get_stabilize_roll(long target_angle) error = constrain(error, -2500, 2500); // desired Rate: - rate = g.pi_stabilize_roll.get_pi(error, delta_ms_fast_loop); + rate = g.pi_stabilize_roll.get_pi(error, G_Dt); //Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate); #if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters // Rate P: error = rate - (long)(degrees(omega.x) * 100.0); - rate = g.pi_rate_roll.get_pi(error, delta_ms_fast_loop); + rate = g.pi_rate_roll.get_pi(error, G_Dt); //Serial.printf("%d\t%d\n", (int)error, (int)rate); #endif @@ -40,13 +40,13 @@ get_stabilize_pitch(long target_angle) error = constrain(error, -2500, 2500); // desired Rate: - rate = g.pi_stabilize_pitch.get_pi(error, delta_ms_fast_loop); + rate = g.pi_stabilize_pitch.get_pi(error, G_Dt); //Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate); #if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters // Rate P: error = rate - (long)(degrees(omega.y) * 100.0); - rate = g.pi_rate_pitch.get_pi(error, delta_ms_fast_loop); + rate = g.pi_rate_pitch.get_pi(error, G_Dt); //Serial.printf("%d\t%d\n", (int)error, (int)rate); #endif @@ -66,19 +66,19 @@ get_stabilize_yaw(long target_angle) // limit the error we're feeding to the PID yaw_error = constrain(yaw_error, -YAW_ERROR_MAX, YAW_ERROR_MAX); - rate = g.pi_stabilize_yaw.get_pi(yaw_error, delta_ms_fast_loop); + rate = g.pi_stabilize_yaw.get_pi(yaw_error, G_Dt); //Serial.printf("%u\t%d\t%d\t", (int)target_angle, (int)error, (int)rate); #if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters if( ! g.heli_ext_gyro_enabled ) { // Rate P: error = rate - (long)(degrees(omega.z) * 100.0); - rate = g.pi_rate_yaw.get_pi(error, delta_ms_fast_loop); + rate = g.pi_rate_yaw.get_pi(error, G_Dt); } #else // Rate P: error = rate - (long)(degrees(omega.z) * 100.0); - rate = g.pi_rate_yaw.get_pi(error, delta_ms_fast_loop); + rate = g.pi_rate_yaw.get_pi(error, G_Dt); //Serial.printf("%d\t%d\n", (int)error, (int)rate); #endif @@ -100,7 +100,10 @@ get_nav_throttle(long z_error, int target_speed) rate_error = target_speed - altitude_rate; rate_error = constrain(rate_error, -110, 110); - return g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop); + delta_throttle = (float)(millis() - throttle_timer)/1000.0; + throttle_timer = millis(); + + return g.pi_throttle.get_pi(rate_error, delta_throttle); } static int @@ -110,7 +113,7 @@ get_rate_roll(long target_rate) target_rate = constrain(target_rate, -2500, 2500); error = (target_rate * 4.5) - (long)(degrees(omega.x) * 100.0); - target_rate = g.pi_rate_roll.get_pi(error, delta_ms_fast_loop); + target_rate = g.pi_rate_roll.get_pi(error, G_Dt); // output control: return (int)constrain(target_rate, -2500, 2500); @@ -123,7 +126,7 @@ get_rate_pitch(long target_rate) target_rate = constrain(target_rate, -2500, 2500); error = (target_rate * 4.5) - (long)(degrees(omega.y) * 100.0); - target_rate = g.pi_rate_pitch.get_pi(error, delta_ms_fast_loop); + target_rate = g.pi_rate_pitch.get_pi(error, G_Dt); // output control: return (int)constrain(target_rate, -2500, 2500); @@ -135,7 +138,7 @@ get_rate_yaw(long target_rate) long error; error = (target_rate * 4.5) - (long)(degrees(omega.z) * 100.0); - target_rate = g.pi_rate_yaw.get_pi(error, delta_ms_fast_loop); + target_rate = g.pi_rate_yaw.get_pi(error, G_Dt); // output control: return (int)constrain(target_rate, -2500, 2500); diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index e5da5aed28..a55a1b60ea 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -122,7 +122,7 @@ static void read_battery(void) if(g.battery_monitoring == 4) { current_amps = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps * .9; //reads power sensor current pin - current_total += current_amps * (float)delta_ms_medium_loop * 0.000278; + current_total += current_amps * 0.0278; // called at 100ms on average } #if BATTERY_EVENT == 1 diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 94144ff68c..018bbb5add 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -649,7 +649,7 @@ static int8_t test_current(uint8_t argc, const Menu::arg *argv) { print_hit_enter(); - delta_ms_medium_loop = 100; + //delta_ms_medium_loop = 100; while(1){ delay(100); diff --git a/libraries/APM_PI/APM_PI.cpp b/libraries/APM_PI/APM_PI.cpp index fbb8f01ed5..33954289a7 100644 --- a/libraries/APM_PI/APM_PI.cpp +++ b/libraries/APM_PI/APM_PI.cpp @@ -8,16 +8,16 @@ #include "APM_PI.h" long -APM_PI::get_pi(int32_t error, uint16_t dt) +APM_PI::get_pi(int32_t error, float dt) { float output = 0; - float delta_time = (float)dt / 1000.0; + //float delta_time = (float)dt / 1000.0; // Compute proportional component output += error * _kp; // Compute integral component if time has elapsed - _integrator += (error * _ki) * delta_time; + _integrator += (error * _ki) * dt; if (_integrator < -_imax) { _integrator = -_imax; @@ -26,7 +26,6 @@ APM_PI::get_pi(int32_t error, uint16_t dt) } output += _integrator; - return output; } diff --git a/libraries/APM_PI/APM_PI.h b/libraries/APM_PI/APM_PI.h index 6701c4781d..2e81aeeef5 100644 --- a/libraries/APM_PI/APM_PI.h +++ b/libraries/APM_PI/APM_PI.h @@ -77,7 +77,7 @@ public: /// /// @returns The updated control output. /// - long get_pi(int32_t error, uint16_t dt); + long get_pi(int32_t error, float dt); /// Reset the PI integrator ///