为方便说明,我们把电机进行编号, 右下为 9号,右上为 10号,左下为 11,左上为 3号 电机。
1. 飞行器保持悬停, 4个电机的转速保持一致,来使飞行器保持水平。
四个电机的转速 =悬停油门
2. 当我们希望飞行器向右飞的时候 ,我们设定在第一种情况的基础上,增加左边两个电机 (3,11)的转速,减小右边两个电机(9,10)的转速。
9号电机 =悬停油门 - 右倾的量
10号电机 = 悬停油门 - 右倾的量
11号电机 = 悬停油门 + 右倾的量
3号电机 = 悬停油门 + 右倾的量
3. 当我们希望飞行器向左飞的时候 ,上面的公式依然成立,只不过右倾的量是负数了。
4. 当我们希望飞行器向前飞的时候 ,那么我们要增加后面一组电机(11,9)的转速,减小前 面一组电机(3,10)的转速
9号电机 =悬停油门 + 前飞的量
10号电机 = 悬停油门 - 前飞的量
11号电机 = 悬停油门 + 前飞的量
3号电机 = 悬停油门 - 前飞的量
5. 飞行器向后飞的情况 ,上面公式依然成立,前飞的量为负数。
6. 当我们希望飞行器顺时针旋转 , 我们增加 10号, 11号 对角线两个电机的转速,减小 3号, 9号这条对角线电机的转速。
9号电机 =悬停油门 - 旋转的量
10号电机 = 悬停油门 + 旋转的量
11号电机 = 悬停油门 + 旋转的量
3号电机 = 悬停油门 - 旋转的量
等等♚ 2018/5/21 10:52:45
四旋翼的平衡及方向的控制原理
7. 当我们希望飞行器逆时针旋转 , 我们减小 10号, 11号对角线两个电机的转速,增加 3号, 9号这条对角线电机的转速。继续使用上面的公式。
8. 最后, 针对一个电机, 它同时要负责前后左右和旋转的情况, 那它就叠加了 4种情况下 的值:
9号电机 = 悬停油门 - 右倾的量 + 前飞的量 - 旋转的量
10号电机 = 悬停油门 - 右倾的量 - 前飞的量 + 旋转的量
11号电机 = 悬停油门 + 右倾的量 + 前飞的量 + 旋转的量
3号电机 = 悬停油门 + 右倾的量 - 前飞的量 - 旋转的量
所以实现代码如下:
#define PIDMIX(X,Y,Z) rcCommand[THROTTLE] + axisPID[ROLL]*X + axisPID[PITCH]*Y + YAW_DIRECTION * axisPID[YAW]*Z
#ifdef QUADX
motor[0] = PIDMIX(-1,+1,-1); //REAR_R
motor[1] = PIDMIX(-1,-1,+1); //FRONT_R
motor[2] = PIDMIX(+1,+1,+1); //REAR_L
motor[3] = PIDMIX(+1,-1,-1); //FRONT_L
#endif