- lack of drift control by my drift control code
- mismatched scales between horizontal and vertical axes
Let’s deal with drift control first:
pa_target = -math.atan2(evx_out, 1.0 + eaz - eaz_offset)
This essentially says the required pitch angle is the inverse tangent of the desired horizontal acceleration (the corrective output of the horizontal velocity PID) divided by the actual vertical acceleration.
The math(s) is right, but both top and bottom of the fraction are plagued with noise; the top due to PID d-gain, and the bottom because it’s a direct feed from the accelerometer. The two noise sources aren’t in phase with each other, so this is what you get as the target pitch angle:
Removing the noise from the horizontal velocity PID output (evx_out) is just a case of setting the D gain to zero. Handling the vertical accelerometer noise is a diferent matter – I think only reducing the dlpf will help here.
Regarding the horizontal / vertical scale difference, I’d been tinkering to ensure angles are calculated consistently throughout the code – in particular the Euler angles, conversion matrix, and the equation above. The aim was to eliminate yaw in all calculations by eliminating any cross-pollination between X and Y axes between Phoebe and the earth (or putting another way, making sure that if Phoebe yaws, so does the earth, so that viewed from above, the horizontal axes align. If I had a way to correct yaw, then I’d need to add it back into all the math(s) but I don’t!
As a result the scaling seemed to improve significantly: all axes now are out by a factor of about 2.5 – but are self consistent.
The flight ascended to ≈2.5m and drifted by ≈4m.