ho trovato un grosso bug che fa impazzire i motori, causa logica sbagliata sul calcolo della rotazione dello yaw.
poco dopo orizzonte.IMUupdate(blbablabal)
il codice corretto è:
//find out actual estimated yaw angular rotation. We can also use directly gyro value with a proportion :-D
float yawR = yaw-lastYaw;
lastYaw=yaw;
if (yawR<minYaw)
minYaw = yawR;
if (yawR>maxYaw)
maxYaw = yawR;
/* because a quadricopter, engine setup:
0
2 3
1
*/
float engineAngle[4];
engineAngle[0]=roll+yawR;
engineAngle[1]=-roll+yawR;
engineAngle[2]=-pitch-yawR;
engineAngle[3]=pitch-yawR;