Tangerine Dream

A comment from Jeremy yesterday planted a seed, which has rapidly become a germinating coconut!

It started with using a RPi B2 to give me the extra CPU cores I wanted rather than wait for the fictional RPi A2.  I’d not considered a B2 as only A+ fit between the top and bottom plates of the frame, but in the past I’ve used Bs, As and B+s sat on the top-plate.  Immediately I thought of Chloe who’s currently just a Phoebe clone – but a RPi B2 could make her so much more – I started rebuilding Chloe to shuffle the bits around to make space for the RPi B2 on the top plate.

I’d need a case of course for protection in her new exposed position on the top plate, and the Tangerine PiBow appealed for some reason.  Perhaps it’s the seasonal stocking filler from Santa?

Then it dawned on me that since using Jessie for Zoe, I’d not seen any I2C errors which have plagued the accuracy of sensor readings for a year now, and that meant I could reconsider using some of the FIFO variants of the code I’d shelved.

By using a FIFO, control of time is no longer tied to the data ready interrupt: additional function does not have to be squeezed into the few milliseconds between sensor readings; the code can do what it wants (for example with additonal sensors) and periodically empty the FIFO and process the contents, with accurate elapsed time based on the number of samples retrieved from the FIFO.

For now, I’m going to focus on getting Zoe and her PiZero up to Phoebe’s standard, but I hope to then raise the Tangerine phoenix that’s Chloe from the flames.

 

Zoe the Zero – 3 – I2C

I thought I’d be unable to proceed further until the new PCB had arrived, but I’d forgotten I’d already started building Zoe prior to the release of the Pi Zero; So I had an HAT PCB ready and tested with an A+.  Since the GPIO pins for the A+ and Pi Zero are identical, I could test the I2C and the Quadcopter code itself.

First step is to run

i2cdetect -y 1

which if you are using the Drotek MPU9250 + MS5611 breakout should look like this:

 0 1 2 3 4 5 6 7 8 9 a b c d e f
00: -- -- -- -- -- -- -- -- -- -- -- -- -- 
10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
60: -- -- -- -- -- -- -- -- 68 -- -- -- -- -- -- -- 
70: -- -- -- -- -- -- -- 77

68 is the MPU-9250 I2C address and 77, the address of the MS5611.

Next, update the baudrate of the I2C interface to the maximum the MPU9250 can use since we want to get as much data as we can as frequently as we can.  Open up /boot/config.txt and set the following and reboot.

dtparam=i2c_arm_baudrate=400000

At this point, it’s possible to run the quadcopter code to test the I2C.

cd ~/Quadcopter
sudo ./qc.py -g
sudo ./qc.py -f fp.csv

Here I got 2 surprises.  First it just worked, and second, there were no signs of I2C errors.  So I upped the dri_frequency in the code

dri_frequency = 500
samples_per_motion = 5

And that worked too; with the A+ and the same PCB this didn’t work – data samples were being missed. Perhaps it’s the 1GHz overclocking? I dunno and I don’t care, it bodes well for when the Pi Zero size PCB arrives.

When the new PCB does, I’ll have to redo this to check the PCB itself, but it’s a reassuring test to have passed.

Oh Jessie, you’re so fine

Installment 2 of updating Chloe to the Jessie level of Raspian: I2C and RPIO.

    • sudo apt-get install i2c-tools python-smbus pypy-smbus-cffi python-codebug-i2c-tether
    • “i2cdetect -y 1” shows 68 and 77 for Drotek MPU9250 + MS5611
    • /boot/config.txt:
      dtparam=i2c_arm_baudrate=400000
    • Python development tools:
      sudo get install python-dev
    • Install and build RPIO
      git clone https://github.com/metachris/RPIO.git
      cd RPIO
      sudo python setup.py install
    • Install and build my custom GPIO – I tried using the standard RPi.GPIO again, and it’s a lot faster than it was, but not quite fast enough
      tar xvf GPIO-0.6.0.tgz
      cd GPIO
      sudo python setup install

Hand-in-glove

First the good bit so you can skip the boring explanation if you wish!

Low drift Phoebe

I did some experimentation to test which of the four different ways the IMU hardware interrupt and my GPIO code can work together to get the most efficient and accurate readings.

The IMU data ready interrupt can either generate a 50us pulse each time the data registers are updated, or it can produce a rising edge which latches high, only falling when the data is read over I2C.

My GPIO tweaks can either capture these interrupts continuously, or just capture the next one, and then capture no more until called again.

So I ran 4 tests to see which pairing worked together best based upon the number of I2C and data corruption errors I got and how accurate the timing was which is directly related to how many sensor readings are missed during motion processing.

Other than the latching, continuous option which just blocked (predictably), it was marginal between the other three in the lab testing, but with a slight advantage shown by 50us pulse, continuous reading.

And a few test flights with Phoebe confirmed this; much reduced drift and improved timing and at 500Hz sampling she missed only 2% of samples which is the best yet by a very long way – previously it’s been more like 20%

Two final tweaks: I loosened the tests for duff data and changed the butterworth filter values:

  • duff data can be identified when the sequence of ax, ay, az, temp, gx, gy, gz has some values set to -1 (0xFFFF) starting from the gz end.  Previously I’d been checking gy and gz.  Now I check just az, the reasoning being that errors in gyro of -1 have a very short term effect on the accuracy of angles, whereas -1 for az (which represents freefall in a vacuum) will plague a flight thereafter due to integration for velocity
  • The butterworth changes were experimental to see what settings might provide better flights by filtering more acceleration out of gravity; a 0.05Hz (20s) 6th order was used in these flights compared to the previous 0.1Hz (10s) 4th order resulting in much better drift control.

Wasted time.time()

I’ve been doing some fine tuning of the HoG code as a result of the FIFO trial I did.  The FIFO timing came from the IMU sampling rate – there’s a sample every 1ms and so there’s no need for using time.time() when the IMU hardware clock can do it.

I’ve now applied the same to the hardware interrupt driven code; motion processing now takes less time as it was the only call to time.time(), and as such that means there’s a smaller chance of missing samples too.  I’ve also added code so that when there are I2C / data corruption errors, that counts as another 1ms gap since the code then waits for the next hardware interrupt.

I’ve also removed the zero 0 calibration again, as I’m fairly convinced it’s pointless – I _think_ the deletion of (Butterworth filter extracted) gravity from acceleration means that calibration is pointless.  That also adds a very marginal performance increase to motion processing.

A few test flights showed no regression (nor noticeable improvement), so I’ve uploaded the code to GitHub.  You also need to grab the latest version of my GPIO library as in now imports as GPIO rather than as a clone of RPi.GPIO.

Probable source for I2C read errors

This is really just a note to myself that there is a known issue with the I2C kernel drivers / hardware for the Raspberry Pi covering all models.  Essentially the I2C clocks between master and slave can get stretched, the MPU-6050 can do this, but the Raspberry Pi ignores this, which would result in reading data before it’s ready.  Doesn’t seem like a solution is forthcoming, and it’s made worst at high-baudrates and large data reads i.e. exactly what my code is doing.  Feck.

 

I2C is error proof

Yesterday was proof of error, today is proof of the protective fix:

    def readSensors(self):
        #-------------------------------------------------------------------------------------------
        # For the sake of always getting good date wrap the interrupt and the register read in a try
        # except loop.
        #-------------------------------------------------------------------------------------------
        while True:
            try:
                #-----------------------------------------------------------------------------------
                # Wait for the data ready interrupt
                #-----------------------------------------------------------------------------------
                RPIO.edge_detect_wait(RPIO_DATA_READY_INTERRUPT)

                #-----------------------------------------------------------------------------------
                # For speed of reading, read all the sensors and parse to SHORTs after.  This also
                # ensures a self consistent set of sensor data compared to reading each individually
                # where the sensor data registers could be updated between reads.
                #-----------------------------------------------------------------------------------
                sensor_data = self.i2c.readList(self.__MPU6050_RA_ACCEL_XOUT_H, 14)
            except:
                self.num_i2c_errs += 1
                continue

            #---------------------------------------------------------------------------------------
            # Convert the array of 14 bytes to 7 shorts
            #---------------------------------------------------------------------------------------
            for index in range(0, 14, 2):
                if (sensor_data[index] > 127):
                    sensor_data[index] -= 256
                self.result_array[int(index / 2)] = (sensor_data[index] << 8) + sensor_data[index + 1]

            #-------------------------------------------------------------------------------------------
            # +/- 4g * 16 bit range for the accelerometer
            # +/- 250 degrees per second * 16 bit range for the gyroscope
            #-------------------------------------------------------------------------------------------
            [ax, ay, az, temp, gx, gy, gz] = self.result_array
            if gz == -1 and gy == -1 and gx == -1 and temp == -1 and az == -1:
               self.num_data_errs += 1
               continue 
            break

        return ax, ay, az, gx, gy, gz

And here’s the proof duff data was caught triggering another sampling loop:

lps: 990.767934
mpu6050 2 misses, i2c 3 misses

Next step is to take her out and fly her!

I2C error proof

I added some test code for 10,000 hard loops reading the sensors:

 start_time = time.time()
 logger.warning("loop, ax, ay, az, gx, gy, gz")
 for loop in range(10000):
     ax, ay, az, gx, gy, gz = mpu6050.readSensors() 
     logger.warning("%d, %f, %f, %f, %f, %f, %f", loop, ax, ay, az, gx, gy, gz)
 logger.warning("time:, %f", time.time() - start_time)
 CleanShutdown()

The test takes about 10 seconds to run confirming the 1kHz sample rate. Phoebe is sitting on her foam base (virtually horizontal) with only her HoG powered up. This is what the accelerometer saw – scale is ±4g so 8000 is 1g:

Uncaught I2C error

Uncaught I2C error

Here’s the snippet of diagnostic data matching the spike:

loop, ax, ay, az, gx, gy, gz
...
3069, 54.000000, 8.000000, 7996.000000, -380.000000, 146.000000, -86.000000
3070, 42.000000, -65.000000, -1.000000, -1.000000, -1.000000, -1.000000
3071, 64.000000, -10.000000, 8046.000000, -403.000000, 144.000000, -55.000000
...

Those “-1″‘s are 0xFFFF’s in hex bytes from the data read from the sensor.

Here’s the end of the logs once the test had finished:

time: 10.075210
mpu6050 3 misses, i2c 0 misses

The code recognised and retried 3 I2C misses so they don’t show in the graph, but there is that duff read that did not cause an I2C error, and so was missed, and hence corrupted the Z accelerometer readings, and all the gyroscope readings.  Since these values are integrated to make velocity and angles, there are permanent errors in the flight thereafter.

At least though, the error has a recognisable form, and a sticky-plaster filter fix can be applied.

Fee FiFo Fum, Feck

After a lot of timing experimentation today*, I’m pretty certain my I2C errors (and duff data) are due to the MPU-9250 sensor registers being updated while I’m still reading them.  If so, I need to move to using the FIFO provided.

I opted to read the data registers directly originally for simplicity, and then (when I found a single I2C ready could return multiple bytes of data) for performance.

Swapping to the FIFO is going to be a right pain.  It’s essentially a pipeline of bytes with data from the sensor fed in at the bottom, and read at the top.  But the bytes can only be read byte-by-byte (still over an I2C interface), so care needs to be taken to build a full set of bytes before processing that next batch from the sensors.  It does have a latch to hold-back updates while reads are happening which is good.

However, I think it also makes the hardware interrupt meaningless – the count of data in the FIFO is just polled over I2C and if greater than a complete batch, read the next full batch of data.  That’s messy as it’s a CPU hog, and is going to rule out having ‘spare CPU’.

Or putting it another way, it’d be better left to an Arduino (penny in the swear box) to hard-loop reading the FIFO as new data is queued up.


*I tried

  • different model A+’s
  • different MPU-9250 (Chloe’s and Phoebe’s)
  • a kernel update (sudo rpi-update)
  • various I2C baudrates (100, 200 and 400kHz)
  • various overclocking from 750MHz to 1GHz
  • setting combined=1 in /etc/modprobe.d/i2c_bcm2708.conf
  • various SMPLRATE_DIV settings for the sensor sampling

‘Cause 3 out of 4 ain’t bad

So the new IMU + PCB + Raspberry Pi passed the first stage by having no I2C errors.  Next step was 0g calibration at a reasonable temperature which produced good results.  Final step then is to power up the motors and ensure I’ve got all 4 to spin the right direction (testcase 1 in the code).

And that’s the next problem: the front right prop didn’t spin and the ESC didn’t shut up (they whine annoying when they are missing a PWM signal), suggesting something was wrong with the PCB or model A+ itself.

Now, I had 2 spare model A+’s and a very vague memory one was duff in some way, but I couldn’t remember which or how.  So I swapped the model A+ over, and at the same time double checked the PCB and resoldered a couple of joints that looked slightly dodgy.

The result? All four blades now spin, but the I2C errors are back!  It’s enough to drive you nuts – luckily I was nuts anyway just to take on this project!