Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ArduCopter: modified mainloop timing to be in sync with the arrival of new data from the IMU. 

This reduces the maximum delay between when new sensor data arrives and when it is actually used by about 5ms.
This commit is contained in:
rmackay9 2012-08-30 16:53:04 +09:00
parent 42406c827a
commit fd110a2723
1 changed files with 58 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
ArduCopter/ArduCopter.pde
View File

@ -899,7 +899,7 @@ static byte gps_watchdog;
// Time in microseconds of main control loop // Time in microseconds of main control loop
static uint32_t fast_loopTimer; static uint32_t fast_loopTimer;
// Time in microseconds of 50hz control loop // Time in microseconds of 50hz control loop
static uint32_t fiftyhz_loopTimer; static uint32_t fiftyhz_loopTimer = 0;
// Counters for branching from 10 hz control loop // Counters for branching from 10 hz control loop
static byte medium_loopCounter; static byte medium_loopCounter;
// Counters for branching from 3 1/3hz control loop // Counters for branching from 3 1/3hz control loop
@ -981,11 +981,12 @@ void setup() {
void loop() void loop()
{ {
uint32_t timer = micros(); uint32_t timer = micros();
bool spare_time = true; static bool run_50hz_loop = false;
// We want this to execute fast // We want this to execute fast
// ---------------------------- // ----------------------------
if ((timer - fast_loopTimer) >= 10000 && imu.new_data_available()) { if( imu.num_samples_available() >= NUM_IMU_SAMPLES_FOR_100HZ ) {
#if DEBUG_FAST_LOOP == ENABLED #if DEBUG_FAST_LOOP == ENABLED
Log_Write_Data(50, (int32_t)(timer - fast_loopTimer)); Log_Write_Data(50, (int32_t)(timer - fast_loopTimer));
#endif #endif
@ -996,18 +997,12 @@ void loop()
// Execute the fast loop // Execute the fast loop
// --------------------- // ---------------------
fast_loop(); fast_loop();////
spare_time = false;
} else {
#ifdef DESKTOP_BUILD
usleep(1000);
#endif
}
// port manipulation for external timing of main loops // run the 50hz loop 1/2 the time
//PORTK &= B11101111; run_50hz_loop = !run_50hz_loop;
if ((timer - fiftyhz_loopTimer) >= 20000) { if( run_50hz_loop ) {
#if DEBUG_MED_LOOP == ENABLED #if DEBUG_MED_LOOP == ENABLED
Log_Write_Data(51, (int32_t)(timer - fiftyhz_loopTimer)); Log_Write_Data(51, (int32_t)(timer - fiftyhz_loopTimer));
@ -1055,12 +1050,16 @@ void loop()
perf_mon_counter = 0; perf_mon_counter = 0;
} }
//PORTK &= B10111111; //PORTK &= B10111111;
spare_time = false; }
} else {
#ifdef DESKTOP_BUILD
usleep(1000);
#endif
} }
if (spare_time && g.compass_enabled) { // port manipulation for external timing of main loops
compass.accumulate(); //PORTK &= B11101111;
}
} }
// PORTK |= B01000000; // PORTK |= B01000000;
// PORTK &= B10111111; // PORTK &= B10111111;