STM32无人机姿态内外环控制函数

姿态外环

在这里只是用P方法处理误差

    fc.RateError[0] = fc.pid[PIDROLL].get_p(rc.Command[ROLL]-imu.angle.x);
    fc.RateError[1] = fc.pid[PIDPITCH].get_p(rc.Command[PITCH]-imu.angle.y);
    fc.RateError[2] = fc.pid[PIDYAW].get_p(rc.Command[YAW]-imu.angle.z);

姿态内环

在这里使用的是PID方法处理误差，同时进行了油门的偏移校正以及积分清零的操作

    int32_t PIDTerm[3];
    PIDTerm[0] = fc.pid[PIDROLL].get_pid(fc.RateError[0]-imu.Gyro.x, PID_INNER_LOOP_TIME*(1e-6));
    PIDTerm[1] = fc.pid[PIDPITCH].get_pid(fc.RateError[1]-imu.Gyro.y, PID_INNER_LOOP_TIME*(1e-6));
    PIDTerm[2] = fc.pid[PIDYAW].get_pid(fc.RateError[2]-imu.Gyro.z, PID_INNER_LOOP_TIME*(1e-6));
    if(rc.Command[THROTTLE].pid[PIDROLL].reset_I();
        fc.pid[PIDPITCH].reset_I();
        fc.pid[PIDYAW].reset_I();
    }
    int32_t Throttle;
    if(cos(imu.angle.x)>cos(imu.angle.y))
    {
        Throttle = rc.Command[THROTTLE] / cos(imu.angle.x);
    }
    else
    {
        Throttle = rc.Command[THROTTLE] / cos(imu.angle.y);
    }
    motor.writeMotor(Throttle, PIDTerm[0], PIDTerm[1], PIDTerm[2]);