Reducing MPU6000 jitter and lag (PX4)

[Steve McLaughlin]

1) Jitter caused by 400 Hz main loop:

Because the MPU6000 is sampled at 1000Hz and the main loop is 400Hz, every other sample will see a difference of 0.5ms of jitter.  That may not sound like a lot but that's equivalent to 25% error on the sample period @ 400Hz.

This can be eliminated by choosing a rate based on 1000/N (ie., 100, 200, 250, 333, 500, 1000Hz).  I'd probably go with 250Hz.  Where did 400Hz come from anyways?

2) Clock drift due to polling:

Polling can result in lags of up to the sample period that varies over time as the sampling clock and polling clock drift apart.  In our case, this results in potentially 1ms of lag on each sample.  Most flights are short enough that clock drift will not be visible, however there will be a random lag 0->1ms on each flight.

This can be eliminated by using the MPU6000 data ready interrupt to trigger reads.  Has anyone experimented with this?

[Paul Riseborough]

Steve,

Unless we run synchronously to the IMU,  ppdate rate is a trade-off between latency and jitter. If you are trying to minimise latency, then you don't want to run at 1000/N as you will have to assume the IMU data 'just misses the boat' and need to allow for the worst case inter-sampling delay in the control loop phase budget. If you run a non-integer multiple then the effect of  clock drift on inter-sampling delay is reduced, but we end up with jitter as you have pointed out.

The control systems engineer in me likes the idea of running control loop synchronously to the IMU to minimise phase loss, but given we are blending data from one IMU sampled at 800 and one sampled at 1000 Hz to try and reduce the effect of aliasing (the EKF changes the weighting on the two IMU's depending on their consistency with GPS  observations), this is not practical.

As for the the decision to run the main loops at 400Hz for copter, maybe Leonard or Randy can comment. It predates my involvement in the copter code-base.

Regards,

Paul

[Steve McLaughlin]

Yes, running synchronously to the MPU is what I am suggesting.  I was intending on accomplishing that by using the interrupt from the MPU to trigger its read and you might sync the main loop with it as well.  I understand your concern with multiple IMUs and it's definitely important for them to play nice together.

Regarding the interrupts, I'm new to this processor and px4/apm code base so there we be some learning curve for me.  I am just wanting to experiment with this and wondering if anyone has done anything similar before so I can save some work.  I saw some stuff in the code regarding automatic polling but I didn't track that down very far (mpu6000.cpp measure_trampoline: Called by the HRT in interrupt context at the specified rate if automatic polling is enabled).

[Steve McLaughlin]

I just reviewed OpenPilot Revolution code as a reference and they have the MPU (as well as other sensors) on interrupts.  I don't have one of these boards but from what I've read, they have a reputation of extremely smooth flight.  This is what I'm after.

Here's what they have:

MPU DLPF = 256Hz, Sample rate = 8000Hz w/12x oversampling yields 666Hz update rate.  All the subsequent tasks (EKF, stabilization, etc.) are triggered by the reception of this data.  This does 2 important things, minimizes data latency/jitter as described previously and frees up the CPU for other tasks.

Has this been considered in the past or is this something you'd consider for the future?

[Randy Mackay]

Steve,

 

     Re the hardware filtering for the MPU6k.  I checked with Leonard and it seems like there was a miscommunication between he and Tridge when the filtering was first implemented.  Here’s a patch that I think fixes the problem.  What do you think?

     https://github.com/rmackay9/PX4Firmware/commit/508ed12e2b603b7ff6c2893c88e34cdd8447971c

 

     Re hanging the main loop off the INS, this is how we do it on the APM2.  We don’t do it this way on the Pixhawk for two reasons (I think):

1.       The interrupt pin isn’t connected on the mpu6k so we have to poll to see if there is new data (I’m not totally sure about this)

2.       We have two IMUs and they update at different rates (1k for mpu6k, 800hz for lsm303d) so we would have to pick one or the other.  According to Leonard, because of the filtering it doesn’t matter too much if we have an extra sample or not.

 

      Re timing in general, I have at times spent time looking at the timing and especially on the APM2 trying to keep the main loops running reliably.  The APM2 is pretty smooth and even but the Pixhawk is actually much worse.  I don’t think the issue is the IMU so much as some other processes that are blocking the main CPU now and then.  I have not determined exactly what those processes are but I think there’s more than one culprit.

 

      Thanks for looking over all this.  It’s very detailed stuff.

 

-Randy

--
You received this message because you are subscribed to the Google Groups "drones-discuss" group.
To unsubscribe from this group and stop receiving emails from it, send an email to drones-discus...@googlegroups.com.
For more options, visit https://groups.google.com/d/optout.

[Chris Anderson]

Any reason we can't connect the interrupt pin on Pixhawk 2?

-Chris

--
Chris Anderson

CEO, 3D Robotics

ch...@3drobotics.com

[Philip Rowse]

@ Chris on the main board yes we can, but on the flex, no.  we ran out of pins on the FMU...

Philip Rowse

Electronics Engineering Dept

3DRobotics

Ballarat

Australia

[Chris Anderson]

Is there a pin we're using in the original FMU design that is actually not needed by current software and could be repurposed for this?

[Steve McLaughlin]

Randy,

I looked at the patch and I'd say it's 90% better.  The issue I have is you are now effectively at a 3rd order filter that is a blend of butterworth and whatever is in the MPU.  This is good in that you'll have a steeper rolloff above the cutoff frequency but you lose some the benefits of the butterworth and with 3rd order you're adding more filter lag.

The main advantage I see with the HW filter is that it is applied at the sample rate of the MPU.  In the last day or so I've discovered that, for PX4 running ardupilot, the sampling and software filtering is done at 400Hz (when I originally reviewed the code I thought the mpu6000 driver was operating at 1000Hz independently and the ardupilot code was subsampling that at 400Hz).  Anyways, if this is true, there's actually another error in the software filtering in that it is mistakenly applying the SW filter assuming 1000Hz sampling (see below).  All of these issues (filtering, jitter, lag) are very much related and very important to a real-time control system.  I like the way OpenPilot is doing it the best.  The key features being:

1) Use interrupts to read to minimize jitter/lag.

2) Select DLPF=256 (fixed), to maximize HW sample rate and apply high bandwidth low pass filter at full rate (anti aliasing).

3) [ardupilot] Oversample (or maybe just downsample) to get down to 400Hz.

4) [ardupilot] SW filter down to user selectable software filter.

That said I'd probably say just implement the patch you have until we can determine if this interrupt thing is going to work.  As far as I can tell, the MPU_DRDY (the interrupt line) is connected in the current PixHawk PCB on PD15 (pin 62).  In fact, it looks like every sensor has an interrupt pin connected to a unique pin.  I'm currently sorting out the best way to do interrupts with NuttX and the PX4Firmware.  It seems there's enough code already in place for it and it shouldn't be too bad.

Here's some more info on the SW filter error:

from mpu6000.cpp:

The default sample rates are defined as 1000Hz:

#define MPU6000_ACCEL_DEFAULT_RATE                      1000
#define MPU6000_GYRO_DEFAULT_RATE                       1000

Then the filters are defined with 1000Hz sample rate:

   _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
   _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
   _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
   _gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
   _gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
   _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),

Then measure() applies the filtering without previously adjusting the sample rate (ardupilot didn't use SENSORIOCSPOLLRATE):

   arb.x = _accel_filter_x.apply(x_in_new);
   arb.y = _accel_filter_y.apply(y_in_new);
   arb.z = _accel_filter_z.apply(z_in_new);

   grb.x = _gyro_filter_x.apply(x_gyro_in_new);
   grb.y = _gyro_filter_y.apply(y_gyro_in_new);
   grb.z = _gyro_filter_z.apply(z_gyro_in_new);

Steve

[Andrew Tridgell]

> The main advantage I see with the HW filter is that it is applied at the
> sample rate of the MPU. In the last day or so I've discovered that, for
> PX4 running ardupilot, the sampling and software filtering is done at
> 400Hz

no, it's not. For the MPU6000 the software filtering is done at
1kHz. For the LSM303D it is done at 800Hz.

See PX4Firmware/src/drivers/mpu6000/mpu6000.cpp

Cheers, Tridge

[Steve McLaughlin]

I've been reviewing mpu6000.cpp for some time now but it's a little confusing.  As a newcomer, I feel there is not enough documentation on how exactly ardupilot software sits on top of the PX4 software.  It's pretty clear the native PX4 firmware uses a Linux like startup script.  Does ardupilot use that as well?  My guess is no, because I see a lot of initialization code in ArduCopter.pde and HAL_PX4_Class.cpp.  I'm struggling to find how the mpu6000.cpp driver is started on ardupilot.  Ok, nevermind, I just found a separate startup script for ardupilot in mk\PX4\ROMFS\init.d.  Ah, this is where all the drivers get started...and yes now I see that the mpu6000 start is called which starts a 1000Hz measurement loop in the driver.  I apologize as there is no filter error - thanks Tridge!

Yesterday, I did a couple of interesting tests:

1) In the main loop of ArduCopter.pde, I pulsed a GPIO (AUX OUT 5) at the start of the loop and hooked it up to a scope.  Sure enough, I saw a 400Hz average but the timing was very loose and dancing all over the place.  I didn't spend too much time analyzing it but if I had to guess it was +/- 50 Hz.

2) I did a similar thing to verify the MPU_DRDY pin was connected and working.  I'll skip the details, but in the end I could see 50us pulses at 1000Hz which correlate to the MPU6000 datasheet.

My point here is that there is an opportunity for improvement here and it is purely a software change. 

[Jared Shamwell]

Steve, 

I've been noticing this problem as well as trying to figure out the best way to handle it. Have you been able to make any progress?

Jared

[Steve McLaughlin]

No, I haven't spent any time on this.  Although I am curious what sort of set up you used to observe this.