无人机项目跟踪五十二----pwm控制模块详解

pwm控制模块的函数是 void CtrlMotor(void)，详解如下：

    float  cosTilt = imu.accb[2] / ONE_G;        //没有用

    if(altCtrlMode==MANUAL)                      //如果是手动操作
    {
        DIF_ACC.Z =  imu.accb[2] - ONE_G;               //没有用
        Thro = RC_DATA.THROTTLE;                 //油门值    
        cosTilt=imu.DCMgb[2][2];                 //俯仰和横滚的余弦乘积
        Thro=Thro/cosTilt;                       //倾角补偿
    } else {   //自动定高模式
        Thro=(-thrustZSp) * 1000;// /imu.DCMgb[2][2];  //倾角补偿后效果不错，有时过猛
        if(Thro>1000)
            Thro=1000;
    }

如上，其中，imu.DCMgb是地理系到机体系转换矩阵，imu.DCMgb[2][2]为最末一位是横滚角与俯仰角余弦的乘积。Thro=Thro/cosTilt的意思是：用油门值除以横滚和俯仰的余弦乘积得到最终的油门值，相当于是把因横滚和俯仰后倾斜的飞机油门值变换到水平的坐标系的油门值。

    //将输出值融合到四个电机
    Motor[2] = (int16_t)(Thro - Pitch - Roll - Yaw );    //M3    //对四个电机进行控制
    Motor[0] = (int16_t)(Thro + Pitch + Roll - Yaw );    //M1
    Motor[3] = (int16_t)(Thro - Pitch + Roll + Yaw );    //M4
    Motor[1] = (int16_t)(Thro + Pitch - Roll + Yaw );    //M2

    if((FLY_ENABLE!=0))
        MotorPwmFlash(Motor[0],Motor[1],Motor[2],Motor[3]);   //将参数值保存到flash中
    else
        MotorPwmFlash(0,0,0,0);  //清空flash

如上，是对电机pwm的控制，油门值与经过pid整定的横滚、俯仰、方位的加减来控制pwm的输出。M1、M2、M3、M4代表四个电机，实际安放位置从前向后从左到右是，M1、M2在前面，M4、M3在后面。当飞机前面向上俯仰时，M1、M2要加快只要增大，所以M1、M2的Pitch前面是加号，M3、M4转速要下降这样才能保证飞机头向上俯仰，所以M3、M4前面为负值。同理横滚方位角也这样理解。最后两行代码是当无人机使能时候保存此参数，否则清空flash。

标签：Thro,M3,横滚,----,M4,imu,Motor,五十二,pwm
来源： https://blog.csdn.net/wyssailing/article/details/117089612