The logic behind the protective fix

It first glance, the protective fix I put in looks as though it only deals with the exact error I showed in the graph two posts ago just looking for -1 in az, temp, gx, gy, and gz:

if gz == -1 and gy == -1 and gx == -1 and temp == -1 and az == -1:
    self.num_data_errs += 1
    continue
break

But actually, it’s a lots more subtle than that.

The MPU data registers are as follows:

  • registers 59 – 64 represent 2 bytes each of ax, ay, and az
  • registers 65 and 66 represent 2 bytes of temperature
  • registers 67 – 72 represents 2 bytes each of gx, gy and gz.

The duff data showed that ax and ay had been filled in, but the rest had not at the point of reading the data over I2C.  So I speculate that when the registers are being filled with new values, it starts at register 59 and finishes at 72.

The value for az is critical; if the value is -1 instead of somewhere around 8000 (1g), it permanently affects the velocity values controlling the rest of the flight, leading to permanent fixed speed drift.  If az is incorrectly -1, then whether the values of ax and ay or correct or -1 doesn’t matter; the flight is doomed to drift regardless.

In contrast, gyro values are used only short term, and the influence of a duff value gets overridden by the complementary filter in a fraction of a second.  The error lives for just a few milliseconds until it’s lost forever; the flight just has a few millisecond glitch, but the next motion processing loop will correct that glitch.

So actually, the test needs only check if az == -1.  az of -1 maps to freefall with no air resistance; it simply won’t happen unless you fly the quad upside down!  But there’s a very slight chance you might see freefall values (quadcopter in ISS?), but that does mean the test can still be refined.

In ‘C’, an “if” like the above does not necessarily process all of the tests. As soon as one fails the test (for example if gz is not -1), then code code stops testing, and drops out to the “break” statement.

So the tests can be ordered so that the most important fact is checked first: az.  If az is duff, then the values for ax and ay are irrelevant, so there’s no point in checking whether they are -1 whatsoever.  If az is not -1, then even though temperature and gyro data may still be duff, if doesn’t matter, and it’s worth taking the accelerometer data.  Only if az is -1, and the temperature and gyro values are all -1 does it mean the data cannot be reused, and so must be re-acquired.

The difference is subtle, but this is python code running at 1kHz, and every little bit counts (pun intended).

The revised “if” test at the top could actually look like this:

if az == -1 and temp == -1 and gx == -1 and qy == -1 and gz == -1:
    self.num_data_errs += 1
    continue
break

To be honest, this really doesn’t matter, but I did a few test flights today which were a lot better, but there was a blustery wind which meant the flights weren’t visually pretty so no video, so I had time to kill, and my mind wandered.

Leave a Reply

Your email address will not be published. Required fields are marked *

This site uses Akismet to reduce spam. Learn how your comment data is processed.