mirror of https://github.com/ArduPilot/ardupilot
Rover: switch to IMU driven timing, same as ArduPlane
This commit is contained in:
parent
4f868e04cc
commit
cd5ad49417
|
@ -605,9 +605,9 @@ void setup() {
|
||||||
|
|
||||||
void loop()
|
void loop()
|
||||||
{
|
{
|
||||||
// We want this to execute at 50Hz if possible
|
// We want this to execute at 50Hz, but synchronised with the gyro/accel
|
||||||
// -------------------------------------------
|
uint16_t num_samples = ins.num_samples_available();
|
||||||
if (millis()-fast_loopTimer > 19) {
|
if (num_samples >= 1) {
|
||||||
delta_ms_fast_loop = millis() - fast_loopTimer;
|
delta_ms_fast_loop = millis() - fast_loopTimer;
|
||||||
load = (float)(fast_loopTimeStamp - fast_loopTimer)/delta_ms_fast_loop;
|
load = (float)(fast_loopTimeStamp - fast_loopTimer)/delta_ms_fast_loop;
|
||||||
G_Dt = (float)delta_ms_fast_loop / 1000.f;
|
G_Dt = (float)delta_ms_fast_loop / 1000.f;
|
||||||
|
@ -642,7 +642,16 @@ void loop()
|
||||||
}
|
}
|
||||||
|
|
||||||
fast_loopTimeStamp = millis();
|
fast_loopTimeStamp = millis();
|
||||||
}
|
} else if (millis() - fast_loopTimeStamp < 19) {
|
||||||
|
// less than 19ms has passed. We have at least one millisecond
|
||||||
|
// of free time. The most useful thing to do with that time is
|
||||||
|
// to accumulate some sensor readings, specifically the
|
||||||
|
// compass, which is often very noisy but is not interrupt
|
||||||
|
// driven, so it can't accumulate readings by itself
|
||||||
|
if (g.compass_enabled) {
|
||||||
|
compass.accumulate();
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Main loop 50Hz
|
// Main loop 50Hz
|
||||||
|
|
Loading…
Reference in New Issue