EEPW论坛

其实串级PID都是一样的（也有人用并级的），主要还是姿态要解的好，不然啥控制都没用的。

下面是代码：

/****************主函数运行*************************************/

                if(++ tmrBase1ms >= 4){
                    tmrBase1ms = 0;

                    commanderGetRPY(&eulerRollDesired, &eulerPitchDesired, &eulerYawDesired);
                    IMUupdate(&ANGL.X, &ANGL.Y, &ANGL.Z,
                                            GYRO.X, GYRO.Y, GYRO.Z, ACCEL.X, ACCEL.Y, ACCEL.Z);
                    
                    Kalman_Filter(ANGL.X, ANGL.Y, ANGL.Z,
                                                GYRO.X, GYRO.Y, GYRO.Z,
                                                &eulerRollActual, &eulerPitchActual, &eulerYawActual);
#if 0
                    controllerCorrectAttitudePID(eulerRollActual, eulerPitchActual, eulerYawActual,
                                                                            eulerRollDesired, eulerPitchDesired, eulerYawDesired,
                                                                            &ANGL_out.X, &ANGL_out.Y, &ANGL_out.Z);
#endif
//#if 0
                    controllerCorrectAttitudePID(eulerRollActual, eulerPitchActual, eulerYawActual,
                                                                            eulerRollDesired, eulerPitchDesired, eulerYawDesired,
                                                                            &rollRateDesired, &pitchRateDesired, &yawRateDesired);

                    controllerCorrectRatePID(GYRO.X, GYRO.Y, GYRO.Z,
                                                                        rollRateDesired, pitchRateDesired, yawRateDesired,
                                                                        &ANGL_out.X, &ANGL_out.Y, &ANGL_out.Z);
                    
//#endif
                    distributePower(thrust, ANGL_out.X, ANGL_out.Y, ANGL_out.Z,
                                                    &motorPowerLeft, &motorPowerRight, &motorPowerFront, &motorPowerRear);
                }



/*************子函数*********************/

void controllerCorrectRatePID(
    float rollRateActual, float pitchRateActual, float yawRateActual,
    float rollRateDesired, float pitchRateDesired, float yawRateDesired,
    float* rollOut, float* pitchOut, float* yawOut)
{
#ifdef PidRateAdd
    pidSetDesired(&pidRollRate, rollRateDesired);
    TRUNCATE_SINT16(rollOutput, pidUpdate(&pidRollRate, rollRateActual, TRUE));

    pidSetDesired(&pidPitchRate, pitchRateDesired);
    TRUNCATE_SINT16(pitchOutput, pidUpdate(&pidPitchRate, pitchRateActual, TRUE));

    pidSetDesired(&pidYawRate, yawRateDesired);
    TRUNCATE_SINT16(yawOutput, pidUpdate(&pidYawRate, yawRateActual, TRUE));
    *rollOut = rollOutput;
    *pitchOut = pitchOutput;
    *yawOut = yawOutput;
#else
    TRUNCATE_SINT16(rollOutput, rollRateDesired);

    TRUNCATE_SINT16(pitchOutput, pitchRateDesired);

    TRUNCATE_SINT16(yawOutput, yawRateDesired);

    *rollOut = rollOutput;
    *pitchOut = pitchOutput;
    *yawOut = yawOutput;
#endif

}

void controllerCorrectAttitudePID(
        float eulerRollActual, float eulerPitchActual, float eulerYawActual,
        float eulerRollDesired, float eulerPitchDesired, float eulerYawDesired,
        float* rollRateDesired, float* pitchRateDesired, float* yawRateDesired)
{
    float yawError;
    pidSetDesired(&pidRoll, eulerRollDesired);
    *rollRateDesired = pidUpdate(&pidRoll, eulerRollActual, TRUE);

    // Update PID for pitch axis
    pidSetDesired(&pidPitch, eulerPitchDesired);
    *pitchRateDesired = pidUpdate(&pidPitch, eulerPitchActual, TRUE);

    // Update PID for yaw axis
  yawError = eulerYawDesired - eulerYawActual;

  if (yawError > 180.0)
    yawError -= 360.0;
  else if (yawError < -180.0)
    yawError += 360.0;

  pidSetError(&pidYaw, yawError);
    *yawRateDesired = pidUpdate(&pidYaw, eulerYawActual, FALSE);
}



/***************pid函数***********************/

float pidUpdate(PidObject* pid, const float measured,
    const bool updateError)
{
    float output;
    if (updateError)
    {
        pid->err = pid->desired - measured;
    }

    pid->integ += pid->err * IMU_UPDATE_DT;

    if (pid->integ > pid->iLimit)
    {
        pid->integ = pid->iLimit;
    }
    else if (pid->integ < -pid->iLimit)
    {
        pid->integ = -pid->iLimit;
    }

    pid->deriv = (pid->err - pid->prevError) * IMU_UPDATE_FREQ;

    pid->outP = pid->kp * pid->err;
    pid->outI = pid->ki * pid->integ;
    pid->outD = pid->kd * pid->deriv;

    output = (pid->kp * pid->err) +
        (pid->ki * pid->integ) +
        (pid->kd * pid->deriv);
    
    pid->prevError = pid->err;

     return output;
}