another flaw fixed. After some more PhD / Masters paper reading yesterday, I realized the angles being calculated should not all be in comparison to the the initial inertial reference frame; instead as the rotation matrix is applied, each angle is calculated against the reference frame the previous rotation had achieved.
So my pitch angles were wrong, which lead to bad gravity calculation prior to take-off and that’s never going to be a good thing! With the code change, the values read of the sloping takeoff platform are as follows where qfrgv_* are gravity read in the quad reference frame, and efrgv* are those reorientated back to the earth frame. The X and Y values are perfect, but Z is out – that doesn’t surprise me as the Z-axis accelerometer is twice as inaccurate as the X and Y. Never the less, the ‘drift’ it shows is 7cms-2 which isn’t bad, though it is making it hard for phoebe getting as far off the ground as far as she should be.
pitch 8.722535, roll -0.448118 qfrgv_x: 0.152762 qfrgv_y: -0.007787 qfrgv_z: = 0.995656 efrgv_x: 0.000000 efrgv: 0.000000 efrgv_z: = 1.007337
There’s still something wrong though as this plot of the flight shows:
She thinks she’s moving backwards according to the sensors and her compensation for that actually has her moving forwards in the real world.
The accurate measure of gravity values suggests that calibration isn’t the problem. My next suspicion is the way the earth frame velocity PID targets are converted to quad frame. Fingers crossed.