From bf94ba551e9996588cb524b1eb2879a4da21d322 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Mon, 19 Sep 2011 14:02:42 -0700 Subject: [PATCH] changed PI Library to take Float for DeltaTime Changed timing to be more accurate in main loop. Still need to switch to micros(). Left that for Tridge. This gets rid of six floating point calcs *250. Nice! --- ArduCopter/ArduCopter.pde | 43 +++++++++++++++++-------------------- ArduCopter/Attitude.pde | 25 +++++++++++---------- ArduCopter/sensors.pde | 2 +- ArduCopter/test.pde | 2 +- libraries/APM_PI/APM_PI.cpp | 7 +++--- libraries/APM_PI/APM_PI.h | 2 +- 6 files changed, 40 insertions(+), 41 deletions(-) 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 ///