Betaflight PID 相关源代码解析

前言

文章会省略一些不相关的代码，用//...表示，还有很多东西没有写，慢慢更新吧，如果有什么错误的地方欢迎留言指出

main函数（main.c）

//main.c
int main(int argc, char * argv[])
{
#ifdef SIMULATOR_BUILD
    targetParseArgs(argc, argv);
#else
    UNUSED(argc);
    UNUSED(argv);
#endif
    init();

    run();

    return 0;
}

init函数(init.c)

开始运行时，通过init函数进行初始化，这其中包括tasks的初始化以及pid配置的载入

//init.c
void init(void)
{
    //....
    tasksInitData();
    //....
    pidInit(currentPidProfile);
    //....
    tasksInit();
    //....

}

 pidInit函数(pid_inti.c)

//pid_init.c
void pidInit(const pidProfile_t *pidProfile)
{
    pidSetTargetLooptime(gyro.targetLooptime); // Initialize pid looptime
    pidInitFilters(pidProfile);
    pidInitConfig(pidProfile);
#ifdef USE_RPM_FILTER
    rpmFilterInit(rpmFilterConfig(), gyro.targetLooptime);
#endif
}

taskInitData 函数和taskInit 函数(task.c)

//task.c
#define DEFINE_TASK(taskNameParam, subTaskNameParam, checkFuncParam, taskFuncParam, desiredPeriodParam, staticPriorityParam) {  \
    .taskName = taskNameParam, \
    .subTaskName = subTaskNameParam, \
    .checkFunc = checkFuncParam, \
    .taskFunc = taskFuncParam, \
    .desiredPeriodUs = desiredPeriodParam, \
    .staticPriority = staticPriorityParam \
}

task_t tasks[TASK_COUNT];

task_attribute_t task_attributes[TASK_COUNT] = {
    //...
    [TASK_PID] = DEFINE_TASK("PID", NULL, NULL, taskMainPidLoop, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
    //...
}

void tasksInitData(void)
{
    for (int i = 0; i < TASK_COUNT; i++) {
        tasks[i].attribute = &task_attributes[i];
    }
}

void tasksInit(void)
{
    //...
    setTaskEnabled(TASK_PID, true);
    //...
}

 

run函数(main.c)

//main.c
void FAST_CODE run(void)
{
    while (true) {
        scheduler();
        //....
    }
}

初始化完成后，开始进入程序主循环

scheduler函数（scheduler.c）

//scheduler.c
FAST_CODE void scheduler(void)
{
    //...
    if (pidLoopReady()) {
        taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_PID), currentTimeUs);
    }
    //...

}

 在scheduler函数中，会调用schedulerExecuteTask函数,getTask返回索引为TASK_PID枚举的task_t*指针

schedulerExecuteTask函数(scheduler.c)

//scheduler.c
FAST_CODE timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTimeUs)
{
    //......
    selectedTask->attribute->taskFunc(currentTimeBeforeTaskCallUs);
    //......
}

 schedulerExecuteTask函数中selectedTask->attribute->taskFunc函数指针调用的函数名为taskMainPidLoop

 

 getTask函数(task.c)

//task.c
task_t *getTask(unsigned taskId)
{
    return &tasks[taskId];
}

 taskMainPidLoop函数(core.c)

//core.c
FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs)
{
    //....
    subTaskPidController(currentTimeUs);
    //....
}

 subTaskPidController函数(core.c)

//core.c
static FAST_CODE_NOINLINE void subTaskPidController(timeUs_t currentTimeUs)
{
    //.....
    pidController(currentPidProfile, currentTimeUs);
    //.....
}

 subTaskPidController函数中会调用pidController函数，其中包含了betaflight PID的算法逻辑

pidController函数(pid.c)

//pid.c
void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTimeUs)
{
    static float previousGyroRateDterm[XYZ_AXIS_COUNT];
    static float previousRawGyroRateDterm[XYZ_AXIS_COUNT];
    //...
    const float tpaFactorKp = (pidProfile->tpa_mode == TPA_MODE_PD) ? pidRuntime.tpaFactor : 1.0f;
    //...
    for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
        float currentPidSetpoint = getSetpointRate(axis);
        //...
        if (axis < FD_YAW) {
            if (levelMode == LEVEL_MODE_RP || (levelMode == LEVEL_MODE_R && axis == FD_ROLL)) 
            {
                pidRuntime.axisInAngleMode[axis] = true;
                currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint, horizonLevelStrength);
            }
        } 
        else 
        { 
            //...
        }
        //...
        // -----calculate error rate
        const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
        float errorRate = currentPidSetpoint - gyroRate; // r - y

        const float previousIterm = pidData[axis].I;
        float itermErrorRate = errorRate;
        //...

        // -----calculate P component
        pidData[axis].P = pidRuntime.pidCoefficient[axis].Kp * errorRate * tpaFactorKp;
        if (axis == FD_YAW) {
            pidData[axis].P = pidRuntime.ptermYawLowpassApplyFn((filter_t *)&pidRuntime.ptermYawLowpass, pidData[axis].P);
        }

        // -----calculate I component
        float Ki = pidRuntime.pidCoefficient[axis].Ki;
        const float iTermChange = (Ki + pidRuntime.itermAccelerator) * dynCi * pidRuntime.dT * itermErrorRate;
        pidData[axis].I = constrainf(previousIterm + iTermChange, -pidRuntime.itermLimit, pidRuntime.itermLimit);
        //....
        // -----calculate D component

        float pidSetpointDelta = 0;
        //...
        const float delta = - (gyroRateDterm[axis] - previousGyroRateDterm[axis]) * pidRuntime.pidFrequency;
        float preTpaD = pidRuntime.pidCoefficient[axis].Kd * delta;
        //...
        pidRuntime.previousPidSetpoint[axis] = currentPidSetpoint;
        //...
        pidData[axis].D = preTpaD * pidRuntime.tpaFactor;
        //...
        
        // -----calculate feedforward component

        // include abs control correction in feedforward
        pidSetpointDelta += setpointCorrection - pidRuntime.oldSetpointCorrection[axis];
        pidRuntime.oldSetpointCorrection[axis] = setpointCorrection;
        // no feedforward in launch control
        const float feedforwardGain = launchControlActive ? 0.0f : pidRuntime.pidCoefficient[axis].Kf;
        pidData[axis].F = feedforwardGain * pidSetpointDelta;
        //....

        // calculating the PID sum
        const float pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F;
        if (axis == FD_YAW && pidRuntime.useIntegratedYaw) {
            pidData[axis].Sum += pidSum * pidRuntime.dT * 100.0f;
            pidData[axis].Sum -= pidData[axis].Sum * pidRuntime.integratedYawRelax / 100000.0f * pidRuntime.dT / 0.000125f;
        } else
        {
            pidData[axis].Sum = pidSum;
        }
    }
}

Setpoint

currentPidSetpoint是遥控信号经过换算后的目标角速度（手动模式和自稳模式会有所不同）

//pid.c
float currentPidSetpoint = getSetpointRate(axis);

 getSetpointRate函数（rc.c）

//rc.c
float getSetpointRate(int axis)
{
#ifdef USE_RC_SMOOTHING_FILTER
    return setpointRate[axis];
#else
    return rawSetpoint[axis];
#endif
}

 自稳模式的setpoint会调用pidLevel函数进行换算：

if (axis < FD_YAW) {
    if (levelMode == LEVEL_MODE_RP || (levelMode == LEVEL_MODE_R && axis == FD_ROLL)) 
    {
        pidRuntime.axisInAngleMode[axis] = true;
        currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint, horizonLevelStrength);
    }
} 
else 
{ 
    //...
}

 pidLevel函数（pid.c）

//pid.c
STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim,
                                                        float currentPidSetpoint, float horizonLevelStrength)
{
    // Applies only to axes that are in Angle mode
    // We now use Acro Rates, transformed into the range +/- 1, to provide setpoints
    const float angleLimit = pidProfile->angle_limit;
    float angleFeedforward = 0.0f;

#ifdef USE_FEEDFORWARD
    angleFeedforward = angleLimit * getFeedforward(axis) * pidRuntime.angleFeedforwardGain * pidRuntime.maxRcRateInv[axis];
    //  angle feedforward must be heavily filtered, at the PID loop rate, with limited user control over time constant
    // it MUST be very delayed to avoid early overshoot and being too aggressive
    angleFeedforward = pt3FilterApply(&pidRuntime.angleFeedforwardPt3[axis], angleFeedforward);
#endif

    float angleTarget = angleLimit * currentPidSetpoint * pidRuntime.maxRcRateInv[axis];
    // use acro rates for the angle target in both horizon and angle modes, converted to -1 to +1 range using maxRate

#ifdef USE_GPS_RESCUE
    angleTarget += gpsRescueAngle[axis] / 100.0f; // Angle is in centidegrees, stepped on roll at 10Hz but not on pitch
#endif
    const float currentAngle = (attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f; // stepped at 500hz with some 4ms flat spots
    const float errorAngle = angleTarget - currentAngle;
    float angleRate = errorAngle * pidRuntime.angleGain + angleFeedforward;

    // minimise cross-axis wobble due to faster yaw responses than roll or pitch, and make co-ordinated yaw turns
    // by compensating for the effect of yaw on roll while pitched, and on pitch while rolled
    // earthRef code here takes about 76 cycles, if conditional on angleEarthRef it takes about 100.  sin_approx costs most of those cycles.
    float sinAngle = sin_approx(DEGREES_TO_RADIANS(pidRuntime.angleTarget[axis == FD_ROLL ? FD_PITCH : FD_ROLL]));
    sinAngle *= (axis == FD_ROLL) ? -1.0f : 1.0f; // must be negative for Roll
    const float earthRefGain = FLIGHT_MODE(GPS_RESCUE_MODE) ? 1.0f : pidRuntime.angleEarthRef;
    angleRate += pidRuntime.angleYawSetpoint * sinAngle * earthRefGain;
    pidRuntime.angleTarget[axis] = angleTarget;  // set target for alternate axis to current axis, for use in preceding calculation

    // smooth final angle rate output to clean up attitude signal steps (500hz), GPS steps (10 or 100hz), RC steps etc
    // this filter runs at ATTITUDE_CUTOFF_HZ, currently 50hz, so GPS roll may be a bit steppy
    angleRate = pt3FilterApply(&pidRuntime.attitudeFilter[axis], angleRate);

    if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)) {
        currentPidSetpoint = angleRate;
    } else {
        // can only be HORIZON mode - crossfade Angle rate and Acro rate
        currentPidSetpoint = currentPidSetpoint * (1.0f - horizonLevelStrength) + angleRate * horizonLevelStrength;
    }

    //logging
    if (axis == FD_ROLL) {
        DEBUG_SET(DEBUG_ANGLE_MODE, 0, lrintf(angleTarget * 10.0f)); // target angle
        DEBUG_SET(DEBUG_ANGLE_MODE, 1, lrintf(errorAngle * pidRuntime.angleGain * 10.0f)); // un-smoothed error correction in degrees
        DEBUG_SET(DEBUG_ANGLE_MODE, 2, lrintf(angleFeedforward * 10.0f)); // feedforward amount in degrees
        DEBUG_SET(DEBUG_ANGLE_MODE, 3, lrintf(currentAngle * 10.0f)); // angle returned

        DEBUG_SET(DEBUG_ANGLE_TARGET, 0, lrintf(angleTarget * 10.0f));
        DEBUG_SET(DEBUG_ANGLE_TARGET, 1, lrintf(sinAngle * 10.0f)); // modification factor from earthRef
        // debug ANGLE_TARGET 2 is yaw attenuation
        DEBUG_SET(DEBUG_ANGLE_TARGET, 3, lrintf(currentAngle * 10.0f)); // angle returned
    }

    DEBUG_SET(DEBUG_CURRENT_ANGLE, axis, lrintf(currentAngle * 10.0f)); // current angle
    return currentPidSetpoint;
}

ErrorRate 

 

//pid.c
const float gyroRate = gyro.gyroADCf[axis];
float errorRate = currentPidSetpoint - gyroRate; 

 gyroRate是滤波后的陀螺仪角速度，errorRate则是陀螺仪角速度与setpoint（目标角速度）的差值

 

P值

施工中。。。。未完待续 

 

 文章出现的数据类型定义等

 

taskId_e 枚举

//scheduler.h
typedef enum {
    //.....
    TASK_PID,
    //.....
} taskId_e;

task_t 结构体

//scheduler.h
typedef struct {
     //......
     task_attribute_t *attribute;
     //......
} task_t;

task_attribute_t 结构体

//scheduler.h
typedef struct {
    //......
    void (*taskFunc)(timeUs_t currentTimeUs);
    //......
} task_attribute_t;

 DEFINE_TASK宏定义

//task.c
#define DEFINE_TASK(taskNameParam, subTaskNameParam, checkFuncParam, taskFuncParam, desiredPeriodParam, staticPriorityParam) {  \
    .taskName = taskNameParam, \
    .subTaskName = subTaskNameParam, \
    .checkFunc = checkFuncParam, \
    .taskFunc = taskFuncParam, \
    .desiredPeriodUs = desiredPeriodParam, \
    .staticPriority = staticPriorityParam \
}

附录

看点好玩的

基于物理和PID控制的FPV飞行模拟