Feiyu tuning

Found minimizing D & maximizing I/P gave better results for the Feiyu. P is the feedback which stabilizes the video. D fights the feedback. It oscillates more when it gets knocked off target, but it still recovers on its own. Pending another test timelapse, that leaves the motor mixing. There is no motor mixing, for all intents & purposes.

It works perfectly vertical or horizontal, but the ages old mixing algorithm always oscillates in between. Made a test program to manually control roll/yaw by mixing the motors. This revealed for changing heading when the handle was 45', the yaw/roll motors had to move equal & opposite distances. To change roll when the handle was 45', the yaw/roll motors had to move equal & equal distances. There were slight errors, but the feedback should eventually settle them.

It was exactly the mixing algorithm it did before, but it also revealed the pitch sensor wasn't enough to determine the handle angle. Both pitch & roll sensors need to be applied to get the handle position. The yaw sensor isn't needed.

It would definitely be best done with a matrix. It's also a recursive problem. In order to move the servos, you need the handle angle, but knowing the handle angle requires knowing how much the servos are going to move. It might work in small steps. Since the stock firmware didn't do a good job either, a 2nd IMU in the handle might be the best solution.

Motor mixing must be solved for hard coded handle angles, first.