BetaFlight模块设计之十三：Gyro过滤任务分析

Posted 2022-05-30 lida2003

BetaFlight模块设计之十三：Gyro过滤任务分析

• Gyro过滤任务
• taskFiltering任务分析
• gyroFiltering函数分析
• filterGyro函数分析
• dynamic notch filter
• downsampling概念

基于BetaFlight开源代码框架简介的框架设计，逐步分析内部模块功能设计。

Gyro过滤任务

描述：主要是对Gyro数据的过滤处理。

 ├──> 初始化
 │   ├──> [x]硬件初始化
 │   └──> [v]业务初始化gyroInitFilters
 ├──> 任务
 │   ├──> [v]实时任务[TASK_FILTER] = DEFINE_TASK("FILTER", NULL, NULL, taskFiltering, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
 │   ├──> [x]事件任务
 │   └──> [x]时间任务
 ├──> 驱动
 │   ├──> [x]查询
 │   └──> [x]中断
 └──> 接口
     └──> TBD

taskFiltering任务分析

没什么看头，外面是一个过滤任务，里面实质是gyro的过滤函数。
估计最初考虑很多数据平滑或者过滤都在这里处理。而实际上这个和飞控姿态相关的数据过滤函数调用频率非常高，而其他传感数据并非需要类似高的频率。

taskFiltering
 └──> gyroFiltering

gyroFiltering函数分析

gyroFiltering
 ├──> GYRO_FILTER_FUNCTION_NAME()
 ├──> <USE_DYN_NOTCH_FILTER><isDynNotchActive()>
 │   └──> dynNotchUpdate
 ├──> <gyro.useDualGyroDebugging>
 │   ├──> <GYRO_CONFIG_USE_GYRO_1>[debug数据更新]
 │   ├──> <GYRO_CONFIG_USE_GYRO_2>[debug数据更新]
 │   └──> <GYRO_CONFIG_USE_GYRO_BOTH>[debug数据更新]
 ├──> <USE_GYRO_OVERFLOW_CHECK><gyroConfig()->checkOverflow && !gyro.gyroHasOverflowProtection>
 │   └──> checkForOverflow
 ├──> <USE_YAW_SPIN_RECOVERY><yawSpinRecoveryEnabled>
 │   └──> checkForYawSpin
 └──> <!overflowDetected> // integrate using trapezium rule to avoid bias
     ├──> accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyro.gyroADCf[axis]) * gyro.targetLooptime;
     ├──> gyroPrevious[axis] = gyro.gyroADCf[axis];
     └──> accumulatedMeasurementCount++;

filterGyro函数分析

这个函数有两个变种：filterGyro和filterGyroDebug；主要问题是是否需要debug一些数据，filterGyroDebug函数以下宏定义来数据的记录。

#define DEBUG_SET(mode, index, value) do  if (debugMode == (mode))  debug[(index)] = (value);   while (0)
#define GYRO_FILTER_AXIS_DEBUG_SET(axis, mode, index, value) if (axis == (int)gyro.gyroDebugAxis) DEBUG_SET(mode, index, value)

详见：\\src\\main\\sensors\\gyro.c

#define GYRO_FILTER_FUNCTION_NAME filterGyro
#define GYRO_FILTER_DEBUG_SET(mode, index, value) do  UNUSED(mode); UNUSED(index); UNUSED(value);  while (0)
#define GYRO_FILTER_AXIS_DEBUG_SET(axis, mode, index, value) do  UNUSED(axis); UNUSED(mode); UNUSED(index); UNUSED(value);  while (0)
#include "gyro_filter_impl.c"
#undef GYRO_FILTER_FUNCTION_NAME
#undef GYRO_FILTER_DEBUG_SET
#undef GYRO_FILTER_AXIS_DEBUG_SET

#define GYRO_FILTER_FUNCTION_NAME filterGyroDebug
#define GYRO_FILTER_DEBUG_SET DEBUG_SET
#define GYRO_FILTER_AXIS_DEBUG_SET(axis, mode, index, value) if (axis == (int)gyro.gyroDebugAxis) DEBUG_SET(mode, index, value)
#include "gyro_filter_impl.c"
#undef GYRO_FILTER_FUNCTION_NAME

实际运行或者发布的代码还是使用filterGyro，这里分析的时候直接使用了GYRO_FILTER_FUNCTION_NAME。

GYRO_FILTER_FUNCTION_NAME
 ├──> <loop axis> // downsample the individual gyro samples
 │   ├──> <gyro.downsampleFilterEnabled> // using gyro lowpass 2 filter for downsampling
 │   │   └──> gyroADCf = gyro.sampleSum[axis];
 │   ├──> <!gyro.downsampleFilterEnabled> // using simple average for downsampling
 │   │   └──> gyroADCf = gyro.sampleSum[axis] / gyro.sampleCount;
 │   ├──> <USE_RPM_FILTER>
 │   │   └──> rpmFilterGyro
 │   ├──> // apply static notch filters and software lowpass filters
 │   │   ├──> gyroADCf = gyro.notchFilter1ApplyFn((filter_t *)&gyro.notchFilter1[axis], gyroADCf);
 │   │   ├──> gyroADCf = gyro.notchFilter2ApplyFn((filter_t *)&gyro.notchFilter2[axis], gyroADCf);
 │   │   └──> gyroADCf = gyro.lowpassFilterApplyFn((filter_t *)&gyro.lowpassFilter[axis], gyroADCf);
 │   ├──> <USE_DYN_NOTCH_FILTER><isDynNotchActive()>
 │   │   ├──> dynNotchPush(axis, gyroADCf);
 │   │   ├──> gyroADCf = dynNotchFilter(axis, gyroADCf);
 │   └──> gyro.gyroADCf[axis] = gyroADCf;
 └──> gyro.sampleCount = 0;

dynamic notch filter

关于dynamic notch filter我们有时间再专门针对滤波这个作为一个模块进行分析，详见：\\src\\main\\flight\\dyn_notch_filter.c。

downsampling概念

这里是关于数据采集以及downsampling/upsampling数据处理的算法问题，暂不做展开，有兴趣的，这里有个链接可以简单了解下。

开发者涨薪指南 48位大咖的思考法则、工作方式、逻辑体系