The problem: the camera point of view is in the quad frame; the garmin point of view is in the earth frame.  They need to both be in the same frame to produce a vector that’s meaningful.  A pretty radical rewrite of this area last night resulted.  A test flight this morning sadly was pretty much the same as yesterday: a very stable hover, but shooting off right when she should have gone left.  More stats:



The top pair of accelerometer vs camera show pretty good alignment, right up to the point of 0.4m to the right.  I believe this is correct, but I wouldn’t put money on it yet!

The middle pair are accelerometer vs LiDAR height over time, which is excellent.

The bottom pair are the flight plans in earth and quad frames (the quad one is simply the earth one rotated from my to her POV) – this is where there’s clearly a problem – they should be the same but they are wrong once the flight rotates.  I can’t see an obvious bug in the code, which makes me suspect there’s an obvious bug in my understanding instead.

Notes on “Fusion Thoughts”

Coupla things on the second half about video macro block fusion from the previous post. First, it’s

δx2 = h2 tan(θ)


δx2 = atan(δx2 / h2)

The latter is working out θ which is known anyway.  Just a bit of careless writing down thoughts by me.

More interestingly, the picture represents forward positive movement from left to right across the page.  However, the pitch angle is negative following the right hand rule which my code adheres to.  Therefore, the full conversion equation is:

δx = δx1 + h2 tan(θ)

The situation on the Y / roll axis is different. If you mentally flip the diagram left / right, following the right to left represents a positive move on the Y axis. The roll angle in this case is also positive according to the right hand rule so the equation looks like this:

δy = δy1 - h2 tan(φ)

There are other places in the code which already follow this ruling.

Better angles

Here’s the current code used for calculating pitch, role, and yaw angles, which in turn are used to extract gravity from acceleration, and integrate the results to get velocities:

# Get angles in radians for Euler and quad frame: rotate the accelerometer readings to earth
# frame, pass them through the butterworth filter, rotate the new gravity back to the quad
# frame, and get the revised angles.
eax, eay, eaz = RotateQ2E(qax, qay, qaz, pa, ra, ya)
egx = bfx.filter(eax)
egy = bfy.filter(eay)
egz = bfz.filter(eaz)
qgx, qgy, qgz = RotateE2Q(egx, egy, egz, pa, ra, ya)
uap, uar = GetRotationAngles(qgx, qgy, qgz)

# Convert the gyro quad-frame rotation rates into the Euler frames rotation rates using the
# revised angles from the Butterworth filter
urp, urr, ury = Body2EulerRates(qry, qrx, qrz, uap, uar)

# Merge rotation frames angles with a complementary filter and fill in the blanks
tau_fraction = tau / (tau + i_time)
pa = tau_fraction * (pa + urp * i_time) + (1 - tau_fraction) * uap
ra = tau_fraction * (ra + urr * i_time) + (1 - tau_fraction) * uar
ya += i_qrz

The flaw in this is that it is using the previous set of angles to rotate the latest set of quad-frame accelerometer readings into the earth frame, as shown in red.

The revised code looks like this:

# Update the previous pitch, roll and yaw angles with the latest gyro output
urp, urr, ury = Body2EulerRates(qry, qrx, qrz, pa, ra)
pa += urp * i_time
ra += urr * i_time
ya += ury * i_time

# Based upon the revised angles, rotate the latest accelerometer readings to earth frame.
# Next, run the earth frame acceleration through the Butterworth LPF to extract gravity.
# Next, rotate and revise gravity back to the quad frame.
eax, eay, eaz = RotateQ2E(qax, qay, qaz, pa, ra, ya)

egx = bfx.filter(eax)
egy = bfy.filter(eay)
egz = bfz.filter(eaz)

qgx, qgy, qgz = RotateE2Q(egx, egy, egz, pa, ra, ya)

In this case, the angles are updated first, based upon converting the gyro anglular rates into Euler anglular rates, and integrating these onto the existing Euler angles. Only then with the updated angles are the accelerometer readings reoriented to the earth-frame in order to extract gravity.

Although this definitely fixes one problem with the gravity extraction (and hence potential drift), I do have concerns about the revised version: primarily, you’ll notice that the pitch and role angles are never now defined by accelerometer readings, only increments to gyro readings. As a result, you’ll notice the complementary filter has vanished, as the gyros are not prone to noise. However, this may actually be a double edged sword: while there is now a reliance on zero drift in gyro accuracy, the fact the angles now don’t rely on the accelerometer might mean the results are less prone to accelerometer drift. Without testing, it’s not possible to say which side wins.

Worst case, I could re-add the complementary filter, but that introduces it’s own problems with lag. Not sure yet when I have the change to take Phoebe out to test this. The predicted weather for the coming week isn’t great.


PIDs model a linear system without having to know all the factors which may affect the system, nor what effect they may have, as long as the affect is linear.

But there’s a non-linearity in the code at the moment (and a bug actually that became cler as I gave the none linearity some thought:

The velocity PIDs’ output is essentially a number representing the desired corrective acceleration.  The bigger the velocity error, the bigger the acceleration required to fix it; they are proportional.

For constant acceleration:

v = a * t

Or if the acceleration is not constant:

v = ∫ a dt

Similarly the rotation PID’s input is essentially representing the desired change in angle, and the output is the desired rotation rate applied to the ESCs / motors to carry out that change; again they are proportional.

For a fixed rotation rate, R:

θ = R * t

Or if the rotation rate is not constant:

θ = ∫ R dt

But there’s a problem turning the corrective acceleration (the output from the velocity PID) into the desired angle (the input to the rotation rate PID).  Here’s the current code.

 pr_target = qvx_out
 rr_target = -qvy_out

There’s two problems with the code above.  It’s mapping the qv*_out accelerations to a rotation rate.  But the acceleration in the quad X and Y axes is not proportional to the angle (for examply 0º change gives 0 change in acceleration; 45° gives 1/√2 acceleration change and 90° gives full power acceleration change.  Notice the underlined above – they don’t match and that’s the bug.  To add to the complexity, it’s not actually known what the angle change actually is – it’s not an absolute angle wrt the earth gravity; it’s the change in angle since last time so an integration of the gyro output or the gyro change since the last set of motion processing. But currently that’s not what is fed into that PID – it’s a rotation rate.

So once more, I have a rewrite of the code shown above to allow for the non-linearity, and the result is fed into the inner PID as a desired angle change and compared to the integrated gyro angle change to come up with the rotation rate.

A good thing then that the weather has gone lousy again, so I have plenty of time to think it through properly and implement the changes.

P.S I’m going to apply these changes to the thread + signal code as although its use of threads provided no performance improvement in data collection rate, the code is a lot cleaner / tidier both in allocation / ownership and freeing of variables, and has a nice clean split between locally owned and global variables.