I2C errors continued

I’m still getting I2C errors logged, and the quality of each flight seems to depend on how many of these I get and when.  They all occur during the warm-up code, and could be anywhere from zero to five or six.  If they occur in the first few seconds of warm up, the impact on the quality of flight is small – later in the sequence and it becomes larger.  That’s due to the fact the 20s warmup is primarily there for filling up the Butterworth filter history – earlier errors are less significant.

I do wonder whether it’s the altimeter also on the same I2C bus that may be causing the problem.  My code doesn’t bother initializing it, so next step is to add code just for that.  Fingers crossed.

I added the few lines of code to just initialize the barometer, but that made no difference whatsoever to the I2C errors so for the moment, I’m out of ideas.

Oh silly scope!

Here’s a capture from my ‘scope of the data ready interrupt pin.  I’ve configured it so that it goes high when there is data ready, and drops back low when the data is read by the code:

Data ready interrupt

Data ready interrupt

The display is 20ms wide, so there should be 20 pulses shown if everything was perfect.  The narrow pulses are perfect.  But there’s a couple interesting bits:

  1. There are several missing pulses most obviously between 14 and 18ms – that suggests the sensor is not consistently setting the interrupts pin high at the configured 1kHz sampling rate.
  2. There is one wide pulse between 6 and 9ms – that indicates an extended period between the sensor raising the interrupt pin high and the code reading the data – that could be because Linux / Raspian / Python is not real time, or because the motion processing thread was running, taking longer that 1ms – that prevents the sensors being read due to the Python GIL blocking threads.  That’s to be expected and there’s not a lot I can do without switching to another Python implementation – Cython or PyPy rather than CPython..

There’s nothing I can do about the gaps – that’s a sensor problem.  The code already attempts to compensate though by timing gaps between reads and uses that for integration,  which is then averaged on passing to the motion processing code periodically.  Still, it’s a worry gaps this large exist.

It’s reassuring that all the pulses shows are at 1ms intervals, so there’s no bogus spikes causing the I2C errors during flight.  The flight logs show only 1 I2C error which happened during warm up; to some extent that has lesser impact as the period is used to load up the Butterworth filters and at the end, extract take-off platform slope and resultant gravity distribution and calibration across the quad axes.

After all the tinkering I’ve been up to, I think it’s worth a flight just to see if anything is better as a result.

A Valentines Kiss

Keep it simple, stupid!

I have a nasty feeling that the months and months of hard work I put into calibration of the sensors against temperature have been a complete waste of time; I’ve always wondered how off-the-shelf quadcopters work without this level of calibration and now I think I know.

The MPU-9250 and MPU-6050 product specs defined that the drift against temperature in 0g conditions is ±1.5mg/ºC – that’s 1.5 cm/s2 drift as the temperature changes.  That means that as long as the temperature remains vaguely constant during a flight, then the value effectively doesn’t change.  Add to that the Butterworth filter to dynamically extract gravity from accelerometer readings, and rotation matrices so that Butterworth is applied in the earth frame (i.e. 0, 0, 1g), then the X, Y and Z accelerometer readings should be stable in hover regardless of ambient temperature as long as ambient varies more slowly than it takes for the Butterworth to track any sensor drift.  Any offset and gains from (lack of) calibration are neutralized because of the subtraction of gravity from acceleration, both of which share the same offset / gain errors

This realization was all triggered by the fact the MPU-9250 specs don’t give a way to calculated absolute temperature, and initial testing shows widely different values depending on the ambient temperature the chip was powered up in e.g. powering up the IMU in a beer fridge and read the temperature sensor gives a hugely different value to powering it up in the ‘engineering lab’ and then putting it into the beer fridge and reading the temperature.  This meant I couldn’t do sensor calibration at a fixed temperature (40ºC for Phoebe, Chloe and Zoe).

So all I need to do prior to a flight is allow enough time for the Butterworth to settle while on the ground.  Oh, and delete reams of code, and throw away all the calibration equipment.

But I’m not too disconsolate – I couldn’t have got to this stage without Butterworth filtering for gravity, and I couldn’t have got there without rotation matrices, and I couldn’t have go there without precalibrated gravity drifting within flight at constant temperature and I couldn’t have got there without stable temperature calibration and I couldn’t have got there without … – well you get the idea.

For anyone reading the BYOAQ-BAT thread, don’t worry, that hasn’t got to the calibration stage yet, so everything so far has still necessary.  But once more, BYOAQ-BAT is now stalled pending testing of the above.

Butterworth, 2nd order, 0.1Hz cutoff, 71Hz sampling

Zoë with 2nd order, 0.1Hz cut-off and 71Hz sampling butterworth IIR filter from Andy Baker on Vimeo.

Currently the code which calculates the Butterworth coefficients and implements the filter only covers 2nd order – I need to do some more reading / understanding to go higher.  I suspect 4th order at 0.2Hz might be better.  Cut-off and sampling frequencies are already configurable.

The accelerating drift to the right suggests calibration errors rather than a take-off problem which normally leads to a fixed velocity drift. Calibration cube out again, I think.

Butterworth coefficient ‘C’ code

I have a nasty feeling the server used to calculate the Butterworth filter coefficients may have been overloaded – if I enter values there and hit “Submit” I just get “500 – Internal Server Error” back.

So I went hunting and found a site with ‘C’ code to calculate the coefficients, and the code is simple enough to rewrite in Python allowing easy testing with different order, cut-off and sampling frequency of the filter.  It’s under GPL so I’ve posted a version here:

 *                            COPYRIGHT
 *  Copyright (C) 2014 Exstrom Laboratories LLC
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  GNU General Public License for more details.
 *  A copy of the GNU General Public License is available on the internet at:
 *  http://www.gnu.org/copyleft/gpl.html
 *  or you can write to:
 *  The Free Software Foundation, Inc.
 *  675 Mass Ave
 *  Cambridge, MA 02139, USA
 *  Exstrom Laboratories LLC contact:
 *  stefan(AT)exstrom.com
 *  Exstrom Laboratories LLC
 *  Longmont, CO 80503, USA

#include <stdlib.h> 
#include <stdio.h>
#include <string.h>
#include <math.h>

// Compile: gcc -lm -o bwlpf bwlpf.c

int main( int argc, char *argv[] )
  if(argc < 4)
      printf("Usage: %s n s f\n", argv[0]);
      printf("Butterworth lowpass filter.\n");
      printf("  n = filter order 2,4,6,...\n");
      printf("  s = sampling frequency\n");
      printf("  f = half power frequency\n");

  int i, n = (int)strtol(argv[1], NULL, 10);
  n = n/2;
  long double s = strtod(argv[2], NULL);
  long double f = strtod(argv[3], NULL);
  long double a = tan(M_PI*f/s);
  long double a2 = a*a;
  long double r;
  long double *A = (long double *)malloc(n*sizeof(long double));
  long double *d1 = (long double *)malloc(n*sizeof(long double));
  long double *d2 = (long double *)malloc(n*sizeof(long double));

  for(i=0; i<n; ++i){
    r = sin(M_PI*(2.0*i+1.0)/(4.0*n));
    s = a2 + 2.0*a*r + 1.0;
    A[i] = a2/s;
    d1[i] = 2.0*(1-a2)/s;
    d2[i] = -(a2 - 2.0*a*r + 1.0)/s;

    printf("%Lf\n", 1/A[i]);
    printf("%Lf\n", d1[i]);
    printf("%Lf\n", d2[i]);

It compiles on my Raspberry Pi using the instructions in the code, and produces the same constants the York code did. So next step is to pythonize it.

P.S. A test flight this morning showed promising signs, but I think I need my Butterworth cut-off to be lower (0.1Hz instead of the 0.5Hz I flew).

Infinite Impulse Response

In addition to BYOAQ-BAT, I’m still flying Zoë, trying to tackle the accelerometer drift, especially in the Z axis.  That problem is that with drifting Z axis accelerometer, it’s hard to get a decent vertical hover because

The basic code just calculated gravity prior to flight, and then subtracts that from acceleration readings during the flight to come up with net acceleration.  But if the sensors drift during flight, the subtraction of gravity from the sensor readings will also drift, leading to sensors thinking they are hovering while they are still climbing.  This morning’s test had the quad drifting to ground during the hover phase, and on the ground prematurely in the descent phase.

My current approach has been to use a complementary ‘filter’ (though it’s not really filtering) to slowly mix new readings of acceleration into the preset gravity readings.  It works to some extent, but not well enough.  It does suggest filtering gravity may be a suitable approach.  This morning’s test had the flight climbing too high, and continuing to drift slowly upwards during hover.

What I need is a proper digital low pass filter to separate net acceleration from gravity drift.  And thanks to Phil’s comment, I’ve been investigating IIR filters and found a couple of things which are both useful and critically, I understand.

The gravity drift is slow and always in the same direction where as the acceleration is neither, so I’m thinking a very low cut-off (0.5Hz or less) DLPF will work.  Currently I’m implementing a hard-coded second-order Butterworth filter with a sample rate of 71Hz (the frequency of my motion processing) and a cut off of 0.5Hz.  If this is successful, I’ll add the code to actually calculate the Butterworth parameters so I can easily experiment with other filter types, cut-off frquencies and order for the filter.