Tilt is the angle between the Phoebe-frame Z-axis, and Earth-frame ‘vertical’. It drops out of Euler calculations. It seems the ideal way to work out the conversion from the earth-frame vertical velocity target to the quad frame vertical velocity target thus:
qvz = evz / cos(tilt)
i.e. if the quad is tilted then the power applied to all four motors is increased to maintain the correct earth axis vertical speed. Seems obvious doesn’t it?
Yet up to now, I’ve been using the rotation matrix to take Earth-frame vertical velocity target to the quad-frame. Here’s what that looks like:
qvz = evx * (c_ra * s_pa * c_ya + s_ra * s_ya) + evy * (c_ra * s_pa * s_ya - s_ra * c_ya) + evz * c_pa * c_ra
- c_pa = cos(pitch angle)
- s_pa = sin(pitch angle)
- c_ra = cos(roll angle)
- s_ra = sin(roll angle)
- c_ya = cos(yaw angle)
- s_ya = sin(yaw angle)
I’m struggling to believe these two very different equations produce the same results. So I ran a test. Without the motors running I picked Phoebe up and rotated her around her X and Y axes producing a broad range of tilt:
- green is the vertical velocity target in the Earth-frame
- yellow is using the Euler tilt angle to transform the earth target to quad-frame target
- blue uses beard to do the same.
The flight plan is rise for 3s, hover for 5s and descend for 4.5s – the rest of the time is spent softening the transitions between these targets.
Anyway, back to the plot. While the target is 0, the tilt has no effect; that happens only for non-zero targets as you can see on the graph. I’m sure you’ve also noticed the Euler tilt and Beard matrix results don’t match. Similar in shape but inverted.
Euler looks right to me; as Phoebe tilts, Euler ups the quad-target to ensure the earth-target remains as set. Beard seems to be doing the exact opposite. I need to understand this discrepancy as I’m relying on Beard for horizontal velocity too where Euler can’t help.
Any thoughts? Probably an axis error in my implementation of Beard I guess?
I tried a test flight using Euler tilt angles rather than Beard’s matrix for conversion of the vertical velocity PID target in Phoene’s frame. Phoebe climbed higher, but seemed less stable. Then went back to Beard, and yet another near perfect flight. So Beard wins even though Euler’s tilt seems more logical to me!