Skip to content
Snippets Groups Projects
Commit 77926a69 authored by Andrew Tridgell's avatar Andrew Tridgell
Browse files

APM: switch to Randys new IMU rate control method

this drives the speed of the 50Hz loop by the number of samples
accumulated in the IMU. This should give much more consistent timing
in DCM.

Thanks to Randy for introducing this scheme in ArduCopter!
parent b56dc44e
No related branches found
No related tags found
No related merge requests found
......@@ -709,9 +709,9 @@ void setup() {
void loop()
{
// We want this to execute at 50Hz if possible
// -------------------------------------------
if (millis() - fast_loopTimer_ms > 19) {
// We want this to execute at 50Hz, but synchronised with the gyro/accel
uint16_t num_samples = imu.num_samples_available();
if (num_samples >= NUM_IMU_SAMPLES_FOR_50HZ) {
delta_ms_fast_loop = millis() - fast_loopTimer_ms;
load = (float)(fast_loopTimeStamp_ms - fast_loopTimer_ms)/delta_ms_fast_loop;
G_Dt = (float)delta_ms_fast_loop / 1000.f;
......@@ -745,7 +745,7 @@ void loop()
}
fast_loopTimeStamp_ms = millis();
} else {
} else if (num_samples < NUM_IMU_SAMPLES_FOR_50HZ-1) {
// less than 20ms 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
......
......@@ -853,3 +853,15 @@
#ifndef SERIAL_BUFSIZE
# define SERIAL_BUFSIZE 256
#endif
#if CONFIG_IMU_TYPE == CONFIG_IMU_OILPAN
# define NUM_IMU_SAMPLES_FOR_200HZ 5
# define NUM_IMU_SAMPLES_FOR_100HZ 10
# define NUM_IMU_SAMPLES_FOR_50HZ 20
#endif
#if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000
# define NUM_IMU_SAMPLES_FOR_200HZ 1
# define NUM_IMU_SAMPLES_FOR_100HZ 2
# define NUM_IMU_SAMPLES_FOR_50HZ 4
#endif
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment