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);
        //...
        // -----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;
        }
    }
}

施工中。。。。未完待续 

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

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飞行模拟