Circles

Another day, another type of test to track down where these strange readings are from.  This time, Phoebe sits on a flat platform which sits on a set of sponge balls on the floor.

I then roll her round in circles on the floor, which should produce sine waves of acceleration from the X and Y sensors 90° out of sync.  And ignoring the noise (I’d turned down dlpf to 40Hz, that’s what’s shown:

Circular acceleration

Circular acceleration

Scale seems plausible at up to 0.4g acceleration. qax has a slight negative offset suggesting she’s leaning slightly – she’s picking up -0.025g or as a first order estimate of the angle -0.025 radians or -1.5° – again plausible.

So the deadzone theory is dead!  But there is something interesting here: above is the complete acceleration including gravity; once gravity is subtracted, then this is what you get:

net acceleration

net acceleration

And this is where it gets interesting – the -0.025g offset has gone, which is good, but so has the bulk of the sine wave shape leaving only spikes – compare the scales of the two graphs.

The measurement for gravity in the earth reference frame is fixed throughout the flight – it’s measured as part of the pre-flight checks.  But critically, the measured acceleration is used to calculate the angles used to redistribute fixed gravity to the new quadcopter reference frame – and I think it’s these angles and their reorientation of gravity between reference frames that’s also removing the sine waves. And sure enough, the angles graph below shows Phoebe pitching and rolling by ±20°, whereas I know full well she is not because she’s sitting on a flat platform that can’t pitch and roll.

Pitch and Roll

Pitch and Roll

So what I need is a way to calculate these rotation matrix angles ignoring the net acceleration; again we’re back to complementary or Kalman filters and using the gyro angles.  Except there’s another problem: the gyro angles are all in the quadcopter reference frame; what’s needed is a way to convert the gyro angles to the various intermediate Euler angles of a rotation matrix which lie in three different reference frame.  I vaguely remember something about this in a paper I read one – time to go on a hunt again.

An itch that needs scratching.

It’s bothered me for a long time that the two sources of angles fed into the complementary filter do not have the same meaning.  Since adding the rotation matrix code this has been increasingly evident.  Finally with the graphs from yesterday, and the consistent forward drift, understanding this has become a top priority.

The nub of the problem is the difference in the context of the angles:

Euler angles are cumulative: they represent the rotation of gravity between the inertial reference frame (earth axes aligned with north, west and up) and the quadcopter X, Y, and Z axes respectively.  To perform the rotation each angle is applied in sequence.

Integrated Gyro angles are independent – they are all in the quad frame, and represent rotation rate around each of the quads axes all within the quad reference frame.  Gravity doesn’t enter the equation.

So currently the complementary filters is trying to merge two different types of angles and can only produce garbage as a result.

I may to convert gyro angles into Euler angles, and for a change I found a site which explained these well, and in clear language and equations that don’t obfuscate the facts behind fancy symbols.

Euler vs Gyro Angles

There’s a few other links from that page that explain other parts of the quad code really clearly too.  Well worth reading.

Or I may need to use Kalman to smooth out the Euler angles, and dispose of the complementary filter altogether.

Or there may be a plan C that I’ve not had time to think of yet.

But I’m stuck on a bus for 2 hours tomorrow, so I’ll have plenty of time to muse and snooze!

Forward roley-poley

I’ve had at least one report of someone’s quadcopter based on Phoebe’s code just flipping onto it’s head at takeoff.  I couldn’t really help as I’d not seen such symptoms – until the other day that it.

I have no idea why the flip doesn’t happen each and every time on Phoebe, and does for others; my apologies to those whose quad suffered the problem and I was unable to help.

Anyway, the fix is to replace your version of calibrateGyros() with this one:

 def calibrateGyros(self):
   gx_offset = 0.0
   gy_offset = 0.0
   gz_offset = 0.0

   for iteration in range(0, self.__CALIBRATION_ITERATIONS):
     [ax, ay, az, gx, gy, gz] = self.readSensors()

     gx_offset += gx
     gy_offset += gy
     gz_offset += gz

   self.gx_offset = gx_offset / self.__CALIBRATION_ITERATIONS
   self.gy_offset = gy_offset / self.__CALIBRATION_ITERATIONS
   self.gz_offset = gz_offset / self.__CALIBRATION_ITERATIONS

Let me know via the comments if this solves your problem too.

Someone really doesn’t like Phoebe

I’ve found the bug.  It’s in the gyro calibration code;  it’s been in the code for weeks if not months; it affects the calibration of all 3 axes of the gyro.  Yet it only reared its ugly head at the point the thermostat code is done, and even then only in the y-axis calibration.

I have no idea why only now has it expressed itself, and why only in the Y-axis.  I simply have to assume god* has a sense of irony.


* Insert your favoured deity or bad luck demon here.

Gravity calibration, past and present…

Here’s what I’ve tried for gravity calibration so far:

Sit Phoebe on the flattest platform you can find.  Sensors should read (0, 0, 1)g.  Save the actual values to file and use each flight.  Take-off OK, but drift terrible suggesting angle errors – suggesting not just sensor offsets but gain errors too (hindsight applied liberally). This ‘solution’ was the default for many months until I really started trying to actively attack drift.

Next try the same, but with temperature compensation – convert multiple readings of “(0, 0, 1)” like above against temperature, and use excel to produce a trend-line linear equation for any temperature.  Good, but still no effect on drift, because I was ignoring the possibility of gain as well as offset.

Then the mathematical approach, but still ignoring gain: if the accelerometer was perfect, and not accelerating, then the combination of all 3 dimension sensors must read 1g regardless of surface tilt.

√(ax2 + ay2 + az2) = 1

where sensor values = real values + offset.  Trouble here is too few equations with too many variables to find (or perhaps not, read on…)

So I went back to the experimental side, and ran 4 tests, with Phoebe tilted towards each leg in turn, trying to guess the equation that might find a fixed offset for each dimension. No luck, but again hindsight reveals I was tilting in 3 axes not two so all my guessed equations could not be right.  But I hadn’t spotted that.

I had considered all this in the past, buying a perspex cube inside which I could strap the unchristened Phoebe, measure g in 6 directions, and work out both offset and gain.  Sadly even duct tape isn’t strong enough to strap her down well enough, and I had much bigger problems back then

But today is a new day and after a good night’s sleep with my thinking cap on, with it comes a new plan to allow for gain as well as offset.  Start with a test platform tilted at an angle.  Sit Phoebe on that platform, both nose down and nose up, and read those values.

Accelerometer calibration test rig

Accelerometer calibration test rig

Then

a = g * (s + o)

where a is expected value for gravity, g is the gain, s is the sensor reading and o is the offset.

Nose down ‘a’ and nose up ‘a’ should be the same value with opposite signs leading to

g * (s + o) = - g * (s' + o)
g * (s + s') = g * 2 * o

So

o = (s + s') / 2

Then knowing the angle of tilt from my spirit level combined with the fact that the perfect sensor reading for the angle would be sin θ (with units of g) leads to

a = sin θ
g * (s + o) = sin θ
g = sin θ / (s + o)

I suspect that just an approximate value for θ will suffice, as the equation right at the top could be used to fine tune all the gains equally to ensure 1g at any angle of tilt.

Time to stop talking and get on with it. Here’s the raw data I got:

 Raw     Temp      X accel   Y accel   Z accel    X gyro    Y gyro     Z gyro

FD
-4011, 24.732941, -0.142926, 0.012745, 1.050931, -0.053121, 0.025203, -0.000613
-4003, 24.756471, -0.142944, 0.012847, 1.050797, -0.053175, 0.025116, -0.000586
-4007, 24.744706, -0.143027, 0.012854, 1.050942, -0.053439, 0.025573, -0.000224
AD
-4269, 23.974118, 0.158685, 0.007537, 1.050188, -0.051737, 0.025540, -0.000832
-4280, 23.941765, 0.158802, 0.007494, 1.050043, -0.051684, 0.025404, -0.000848
-4269, 23.974118, 0.158779, 0.007461, 1.050281, -0.052030, 0.025653, -0.000663
LD
-4119, 24.415294, 0.005269, -0.140297, 1.050463, -0.052647, 0.025136, -0.000694
-4105, 24.456471, 0.005559, -0.139865, 1.050735, -0.052635, 0.025112, -0.000635
-4117, 24.421176, 0.005273, -0.140439, 1.050992, -0.052608, 0.025244, -0.000740
RD
-4057, 24.597647, 0.010099, 0.160093, 1.050826, -0.052801, 0.025332, -0.000706
-4046, 24.630000, 0.009812, 0.160029, 1.050554, -0.053320, 0.025580, -0.000406
-4055, 24.603529, 0.009934, 0.160293, 1.051040, -0.053330, 0.025560, -0.000511

where FD = fore down, AD = aft down, LD = left down and RD = right down by 10°.

That results in the following

x_offset = 0.007894833
x_gain = 1.28560825
y_offset = 0.009969
y_gain = 1.333382476

Same thing can be done with the z axis using the same data slightly differently:

oz = (sz - s'z) / 2
az = cos θ
gz * (sz + oz) = cos θ
gz = cos θ / (sz + oz)

Resulting in the following – you get two sets of possible sets of offsets / gains depending on whether the tilt is fore / aft or port / starboard.

zfa_offset = 0.000359667
zfa_gain = 0.936797208
zps_offset = 0.0000383333
zps_gain = 0.937294721

Although the offsets are quite different between both samples, they are an order of magnitude smaller than the X and Y axis offsets so I’m not bothered – the gains match nicely which is reassuring. Because I didn’t turn Phoebe on her head, it’s not possible to find the offsets, so essentially, the offsets are 0, and the gain is covering the discrepancy.

And finally, some sanity checking (not with the wife this time) that everything I’ve done here matches the equation I showed at the start based upon the 12 samples I took.

√(ax2 + ay2 + az2) = 1

There’s actually 24 sanity checks as the z axis gain and offsets could be calculated from both the fore / aft (pitch) tilt values or the left / right (roll) values:

1.006204657	1.007968538
1.005967585	1.007731109
1.006276408	1.008040318
1.004816622	1.006578528
1.004608175	1.006369695
1.005035531	1.006797684
1.007888677	1.009651313
1.008157198	1.009920557
1.008939806	1.010703849
1.008478641	1.010242242
1.007941093	1.009703971
1.008978921	1.010743092

And actually, I’m really pleased with that – errors are primarily going to be due to the rather course measurement of 10° of tilt.  Next steps:

  • test the values above to see what happens live
  • refine the test rig for accurate angles
  • re-run the test at multiple temperatures and throw the gains / offsets into excel for adding temperature related trend lines.

On a roll again!

Is 0.1% accelerometer error good enough?

I spent a fair proportion of today and yesterday tweaking and tuning the offsets out of the accelerometer and gyro. Gyro was a doddle as expected, and I’ve now got a process to reduce the accelerometer tune to a hour rather than a day, though it does start early in the morning before the sun rises. I suspect it won’t work well in a mid-summer heatwave where the temperature doesn’t drop much at night.

All I do it move Phoebe outdoors to the coldest spot I can find (the garage) and leave her for a while to cool down. While that’s going on, I move indoors and spend the time making the test platform as horizontal as possible – I even bought a super-spirit level a while back just for this job. When all’s ready, I whisk her indoors, power her up, and run her calibration code which reads the accelerometer, gyro and temperature sensors, writing the output to file. How often I do this depends on how rapidly the temperature rises – every few seconds at first, increasing to perhaps every 1/2 hour at the end, aiming for 0.5°C rise per sample. I’m aiming for an even spread as this will give me the most accurate trendline to use in the code. Phoebe is powered from the micro-USB port rather than the battery as this leaves the ESCs / motors unpowered and so noise is minimized.

Net result 0.001g accuracy in all 3 accelerometer axes, i.e. instead of reading (0, 0, 1)g, they read (0.001, 0.001, 1.001)g or roughly (0.01, 0.01, 10.01)ms-2. Previously the raw sensor output was more like (0.04, 0.036, 1.06)g so a 40 – 60-fold improvement.  Vertically, that’s probably OK, but horizontally, I’m less confident. Over 10s that 0.001g equates to 0.5m horizontal drift, which is not good enough for my ultimate goal of a 1 minute flight with drift < 10cm – i.e. demo’s in schools and Jams.

Still, I’ll give it a shot tomorrow to see if my unpowered tuning holds up in powered testing. Only time will tell.

Dodo!

Or in fact “D’oh! D’oh!”.  As you might guess, two bugs / enhancements found while snoozing on the sofa, musing over some of this morning’s flights:

  1. if you have nested PIDs, try to only include the integral gain in the outermost PID to ensure best performance.  Therefore tune from inner to outer (i.e. next outer gains set to 0 when tuning inner), and once tuned, increase the tuned inner P gain by up to 50%, set the I gain to 0, and reenable the next PID gains – i.e. tune angular rate, absolute angle and horizontal speed in that order.
  2. If you’re going to change how you calculate the Euler angles to match the coordinate system of the gyro angles, you really ought to change the Matrix used so that X, Y and Z angles are wholly independent (i.e. no sines).

Not finished yet with the inside-out re-tuning, but the progress so far looks promising.

Trends

Gyro and accelerometer trends

Gyro and accelerometer trends

Just a quicky: I spent yesterday gathering these higher quality trends mapping gyro and accelerometer offsets to temperature.

The trend lines are a lot more closely tied to the data points, so hopefully that’ll mean more accuracy.  I’ll give it a try today or tomorrow – I’m hoping that the net result will be even more limited drift, plus longer flight (different sides of the same coin essentially).

Yet more park testing

Went to the park again today to test my new angles calculation; complete garbage – with Phoebe whizzing 25m forward before I killed the flight.  The change to the angles had left me worried about calibration, specifically for the accelerometer, and to be honest I still do, but luckily I can leave that for another day: after checking the stats, the cause is quite clearly the complementary filter.

Pitch angles

Pitch angle

As I mentioned, the flight had Phoebe whizzing forwards across the park – i.e. nose down, and that’s quite clear from the integrated gyro pitch angle (i-pitch); it’s not surprising that it’s not visible in e-pitch, the Euler angles, but the fact the complementary filter (c-pitch) had effectively ignored the i-pitch clearly shows the problem.  The tau was set to 0.25s but quite clearly from the graph the i-pitch contains critical, accurate information for at least a few seconds without obvious signs of drift.  I think I need to start playing with tau at nearer 2.5s!

Just for comparison, I’ve dug out some stats from when I first added the complementary filter; in this test, the blades were not running, I was just manually tilting Phoebe – look how the i-pitch and e-pitch track each other, while the filter output (c-pitch) cancels out the gyro integral drift (i-pitch) – all shown in shades of blue:

Sensor Angles

Sensor Angles

Hopefully with tau sorted, I can go back to checking my new angles and putting my mind at rest about whether gravity calibration is correct / useful – more on that once I’ve made a decision.

Just an afterthought: it’s clear from the tests that the accelerometer Euler angles are varying wildly, so perhaps it’s better, at the same time as (say) doubling up tau to 0.5, I also increase the dlpf to 5Hz.  Only testing will tell.

P.S. The big downward spike at the end was Phoebe doing a forward roly-poly at the end of the flight as her front legs clipped the ground while she was still travelling at several mph.  Funny to watch and zero damage.

Caution – infectious terminal boredom alert!

I’m getting quite bored of writing this stuff about PID tuning, so it must be worse for you reading my wittering, so please forgive me, but it’s a necessity for me for recording my plans, thoughts, ideas and discoveries.

I’ve rerun my pitch and roll rate PID testing much as I did before to rebuild my confidence I hadn’t got my left and right completely wrong.

Anyway, all was fine which gave me the chance to throw some d-gain into the mix.  Bear in mind we are tuning PIDs related to the gyro sensors outputs, so the PIDs are passed a target of the desired roll / pitch speed and the values from the sensors, and the PID munges them together to come up with updated corrective power to the blades.

So the (P)roportial entry provides extra power directly proportional to the error (the difference between target and current sensor readings).  Set the gain too small and the pitch / roll speed may not achieve the target zero speed, just slow it down; get it too high and the pitch / rolls speed overshoots and then is backed off leading to ever increasing oscillations; get it somewhere near, and assuming the target is 0, the pitch / roll speed drops to zero – but that says nothing about whether it’s back at the same angle it started from (say horizontal); it’s stable at some arbitrary angle and not moving (angular speed is 0, but absolute angle is not known) – the return to the starting angle requires the pitch /roll PID I gain which can now be tuned.

Over time, the PID sums up all the errors – the (I)ntergral – and applies power in accordance with the total error scaled by the I gain.  This means that if the pitch / roll started horizontal, the I PID should get it back there eventually.  The gain is simply a matter setting it high enough to get back to where it was without overshooting – this is a much slower oscillation that’s quite different from the P gain oscillations.

So together P & I gains get you a stable horizontal hover, but neither are quick doing so — and that’s where (D)ifferential gain steps in; this checks the change in error since last reading.  If it’s grown, it adds some oomph to the power already added by the P gain.  If it’s shrunk, then it reduces it output having the net effect the P gain can’t overshoot.  It’s tuning needs to have the same net result as the P gain so they work together.

Here’s yet another pointless graph to show what I mean.  The D gain is keeping the drone ‘buzzing’ nicely tightly stable until I disturb it by pushing it; everyone joins together to ensure a fast recovery to the same position with no overshooting.  Cool huh?!  Reply “yes” only if you’re a geek and proud!

Stable hover and stabilized nudging

Stable hover and stabilized nudging

Why am I really bothering to run through this boring PID turning stuff again?  A: because it’s necessary; B: because everywhere seems to talk about negative D gain as though it was religious law – I spent a lot of time proving that was just wrong in my case and C: because I think I should have done the equivalent long ago with the accelerometer to tune the I gain to ensure that if the vertical speed started at zero, but that requires very stable pitch / roll first. With that in place, then the vertical acceleration I gain would return it to zero speed even if it is at a different height.  Until that level of inner-PID stabilization is set right, I’m wasting my time trying to do orderly take-offs.

And to set the I gain correctly, I’ve got to start with the vertical acceleration P gain first, as currently it’s too high, causing pogo-sticking at takeoff.

I’m not sure whether D gain will be needed or not?  Probably yes due to gravity to make sure vertical speed is the same in both up and down directions.

And the point?  That I suspect the only way for me to test the inner vertical acceleration PID settings is for me to hold it above my head each test run until I get a stable hover / vertical speed of 0).

Think I need to get my cycle helmet, safety glasses and gardening gloves out.  I’m gonna be a sight to behold!