Assuming the next few test flights continue to show the stability of the other day, then it’s time to start including horizontal movement – an enormous elephant in the corner of the room I’ve been avoiding because I’ve convinced myself it’s hard.

At the moment, the code only considers horizontal movement due to the quad propellers; if it tilts from horizontal then the quad z-axis lift provided by the props will then be split between horizontal and vertical like this in the earth X, Y, and Z plane:

`eax = faz * sin(pitch)`

eay = faz * sin(roll)

eaz = faz * cos(pitch) * cos(roll)

This code is already in use to ensure climb at the right rate even if there is a minor tilt. It can also be used to add horizontal movement.

What is doesn’t, and can’t cover is drift. When Phoebe is hovering perfectly horizontal, but there’s a tail wind blowing her forwards, her Z-axis accelerometer cannot detect this horizontal movement as it’s pointing vertically; it’s down to the X & Y axis accelerometers – also fine as long as there’s no tilt.

But when you have tilt, then vertical acceleration is measured by all three axes, and this is the point I get lost in matrix + vector linear algebra.

The sensors together read values [X, Y, Z] in Phoebe’s coordinate system. Then a 3×3 matrix full of sin() and cos() and pitch angles and roll angles convert [X, Y, Z] (Phoebe’s point of view) to [X’, Y’, Z’] (the rest of the world’s horizontal + vertical coordinate system).

Any help on this would be hugely appreciated!

Let’s define the transformation matrix as M = [[a,b,c],[d,e,f],[g,h,I]] where V’ = M.V and V’=[x’,y’,z’] and V=[x,y,z]

Then x’=ax+by+cz, y’=dx+ey+fz and z’=gx+hy+iz

Hi David,

I thought (hoped) you might chip in!. I’d got that far myself by some miracle – the current implementation that only uses data from the quad Z axis, plus pitch and roll angles results in a matrix with c = sin(pitch), f = sin(roll), i = cos(pitch)cos(roll).

I need to fill in the other 6 empty squares of the matrix – i.e. incorporating the mappings for the X and Y accelerometer data too. I guess I’ll be drawing a lot of triangles today!

The main question has been answered though – the results are simply summed i.e. x’ = ax + by + cz.

Cheers,

Andy