目录
摘要
本节主要记录IMU初始化和数据更新的过程，欢迎批评指正！！！
1.IMU 原理图
从原理图可以看出ICM42688-P主要接到SPI1上面。
本节主要用到的IMU是ICM42688和ICM42605可以完全兼容。
2.IMU 板层配置
1.在target文件下找到自己板层名称，在target.h定义使用ICM相关配置，设置SPI1总线特性。
在target.c中设置
3.IMU 程序初始化
#if defined(SITL_BUILD)
int main(int argc, char *argv[])
{
parseArguments(argc, argv);
#else
int main(void)
{
#endif
//初始化
init();
//循环初始化
loopbackInit();
//开始任务调度
while (true)
{
#if defined(SITL_BUILD)
serialProxyProcess();
#endif
//开始任务
scheduler();
//进程循环初始化
processLoopback();
}
}
在init函数中执行
void init(void)
{
......
//传感器自动识别
if (!sensorsAutodetect())
{
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}
......
}
bool sensorsAutodetect(void)
{
//eeprom 更新待定
bool eepromUpdatePending = false;
//陀螺仪初始化
if (!gyroInit())
{
return false;
}
//加速度初始化
accInit(getLooptime());
......
}
下面我们分别看加速度和陀螺仪的初始化。
1.陀螺仪初始化
bool gyroInit(void)
{
//清空数据
memset(&gyro, 0, sizeof(gyro));
//设置惯性传感器标签（用于双陀螺仪选择）----- Set inertial sensor tag (for dual-gyro selection)
#ifdef USE_DUAL_GYRO
gyroDev[0].imuSensorToUse = gyroConfig()->gyro_to_use;
#else
gyroDev[0].imuSensorToUse = 0;
#endif
//识别陀螺仪---- Detecting gyro0
gyroSensor_e gyroHardware = gyroDetect(&gyroDev[0], GYRO_AUTODETECT);
//没有陀螺仪数据
if (gyroHardware == GYRO_NONE)
{
gyro.initialized = false;
detectedSensors[SENSOR_INDEX_GYRO] = GYRO_NONE;
return true;
}
// 陀螺仪初始化----Gyro is initialized
gyro.initialized = true;
detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
sensorsSet(SENSOR_GYRO);
//驱动初始化---- Driver initialisation
gyroDev[0].lpf = GYRO_LPF_256HZ;
gyroDev[0].requestedSampleIntervalUs = TASK_GYRO_LOOPTIME;
gyroDev[0].sampleRateIntervalUs = TASK_GYRO_LOOPTIME;
//初始化函数
gyroDev[0].initFn(&gyroDev[0]);
// initFn will initialize sampleRateIntervalUs to actual gyro sampling rate (if driver supports it). Calculate target looptime using that value
//initFn将把sampleRateIntervalUs初始化为实际陀螺仪采样率（如果驱动程序支持的话）。使用该值计算目标循环时间
gyro.targetLooptime = gyroDev[0].sampleRateIntervalUs;
//初始化滤波
gyroInitFilters();
//是否使用动态滤波
#ifdef USE_DYNAMIC_FILTERS
//以PID频率运行的动态陷波--- Dynamic notch running at PID frequency
dynamicGyroNotchFiltersInit(&dynamicGyroNotchState);
secondaryDynamicGyroNotchFiltersInit(&secondaryDynamicGyroNotchState);
//陀螺仪数据状态分析
gyroDataAnalyseStateInit(
&gyroAnalyseState,
gyroConfig()->dynamicGyroNotchMinHz,
getLooptime()
);
#endif
//返回成功
return true;
}
我们需要重点关注： gyroSensor_e gyroHardware = gyroDetect(&gyroDev[0], GYRO_AUTODETECT);
可以看出下面就是循环识别查找我们需要的传感器。
STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
{
//默认对齐方式
dev->gyroAlign = ALIGN_DEFAULT;
//识别
switch (gyroHardware)
{
case GYRO_AUTODETECT:
FALLTHROUGH;
#ifdef USE_IMU_MPU6000 //判断是否使用mpu6000
case GYRO_MPU6000:
if (mpu6000GyroDetect(dev))
{
gyroHardware = GYRO_MPU6000;
break;
}
FALLTHROUGH;
#endif
#if defined(USE_IMU_MPU6500) //MPU6500
case GYRO_MPU6500:
if (mpu6500GyroDetect(dev)) {
gyroHardware = GYRO_MPU6500;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_IMU_MPU9250//MPU9250
case GYRO_MPU9250:
if (mpu9250GyroDetect(dev)) {
gyroHardware = GYRO_MPU9250;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_IMU_BMI160//BMI160
case GYRO_BMI160:
if (bmi160GyroDetect(dev)) {
gyroHardware = GYRO_BMI160;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_IMU_BMI088//BMI088
case GYRO_BMI088:
if (bmi088GyroDetect(dev)) {
gyroHardware = GYRO_BMI088;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_IMU_ICM20689 //ICM20689
case GYRO_ICM20689:
if (icm20689GyroDetect(dev)) {
gyroHardware = GYRO_ICM20689;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_IMU_ICM42605 //ICM42605
case GYRO_ICM42605:
if (icm42605GyroDetect(dev)) {
gyroHardware = GYRO_ICM42605;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_IMU_BMI270 //BMI270
case GYRO_BMI270:
if (bmi270GyroDetect(dev)) {
gyroHardware = GYRO_BMI270;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_IMU_LSM6DXX //LSM6DXX
case GYRO_LSM6DXX:
if (lsm6dGyroDetect(dev)) {
gyroHardware = GYRO_LSM6DXX;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_IMU_FAKE
case GYRO_FAKE:
if (fakeGyroDetect(dev)) {
gyroHardware = GYRO_FAKE;
break;
}
FALLTHROUGH;
#endif
default:
case GYRO_NONE:
gyroHardware = GYRO_NONE;
}
return gyroHardware;
}
这里可以看出要想使用icm42688必须定义：#define USE_IMU_ICM42605
bool icm42605GyroDetect(gyroDev_t *gyro)
{
gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_ICM42605, gyro->imuSensorToUse, OWNER_MPU);
if (gyro->busDev == NULL) {
return false;
}
//重点关注的识别
if (!icm42605DeviceDetect(gyro->busDev)) {
busDeviceDeInit(gyro->busDev);
return false;
}
// Magic number for ACC detection to indicate that we have detected icm42605 gyro
mpuContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev);
ctx->chipMagicNumber = 0x4265;
gyro->initFn = icm42605AccAndGyroInit;
gyro->readFn = icm42605GyroRead;
gyro->intStatusFn = gyroCheckDataReady;
gyro->temperatureFn = icm42605ReadTemperature;
gyro->scale = 1.0f / 16.4f; // 16.4 dps/lsb scalefactor
gyro->gyroAlign = gyro->busDev->param;
return true;
}
static bool icm42605DeviceDetect(busDevice_t * dev)
{
uint8_t tmp;
uint8_t attemptsRemaining = 5;
busSetSpeed(dev, BUS_SPEED_INITIALIZATION);
busWrite(dev, ICM42605_RA_PWR_MGMT0, 0x00);
do {
delay(150);
//读取who am i
busRead(dev, MPU_RA_WHO_AM_I, &tmp);
switch (tmp) {
/* ICM42605 and ICM42688P share the register structure*/
case ICM42605_WHO_AM_I_CONST: //0x42
is42688P = false;
return true;
case ICM42688P_WHO_AM_I_CONST: //0x47
is42688P = true;
return true;
default:
// Retry detection
break;
}
} while (attemptsRemaining--);
return false;
}
从这里可以看出具体就做了区分。对于其他的设置暂时不在讲解。
2.加速度初始化
bool accInit(uint32_t targetLooptime)
{
memset(&acc, 0, sizeof(acc));
// Set inertial sensor tag (for dual-gyro selection)
#ifdef USE_DUAL_GYRO
acc.dev.imuSensorToUse = gyroConfig()->gyro_to_use; // Use the same selection from gyroConfig()
#else
acc.dev.imuSensorToUse = 0;
#endif
//识别
if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware))
{
return false;
}
acc.dev.acc_1G = 256; // set default
acc.dev.initFn(&acc.dev);
acc.accTargetLooptime = targetLooptime;
acc.accClipCount = 0;
accInitFilters();
updateAccCoefficients();
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++)
{
acc.extremes[axis].min = 100;
acc.extremes[axis].max = -100;
}
return true;
}
static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
{
accelerationSensor_e accHardware = ACC_NONE;
dev->accAlign = ALIGN_DEFAULT;
requestedSensors[SENSOR_INDEX_ACC] = accHardwareToUse;
switch (accHardwareToUse) {
case ACC_AUTODETECT:
FALLTHROUGH;
#ifdef USE_IMU_MPU6000
case ACC_MPU6000:
if (mpu6000AccDetect(dev)) {
accHardware = ACC_MPU6000;
break;
}
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
if (accHardwareToUse != ACC_AUTODETECT) {
break;
}
FALLTHROUGH;
#endif
#if defined(USE_IMU_MPU6500)
case ACC_MPU6500:
if (mpu6500AccDetect(dev)) {
accHardware = ACC_MPU6500;
break;
}
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
if (accHardwareToUse != ACC_AUTODETECT) {
break;
}
FALLTHROUGH;
#endif
#if defined(USE_IMU_MPU9250)
case ACC_MPU9250:
if (mpu9250AccDetect(dev)) {
accHardware = ACC_MPU9250;
break;
}
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
if (accHardwareToUse != ACC_AUTODETECT) {
break;
}
FALLTHROUGH;
#endif
#if defined(USE_IMU_BMI160)
case ACC_BMI160:
if (bmi160AccDetect(dev)) {
accHardware = ACC_BMI160;
break;
}
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
if (accHardwareToUse != ACC_AUTODETECT) {
break;
}
FALLTHROUGH;
#endif
#if defined(USE_IMU_BMI088)
case ACC_BMI088:
if (bmi088AccDetect(dev)) {
accHardware = ACC_BMI088;
break;
}
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
if (accHardwareToUse != ACC_AUTODETECT) {
break;
}
FALLTHROUGH;
#endif
#ifdef USE_IMU_ICM20689
case ACC_ICM20689:
if (icm20689AccDetect(dev)) {
accHardware = ACC_ICM20689;
break;
}
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
if (accHardwareToUse != ACC_AUTODETECT) {
break;
}
FALLTHROUGH;
#endif
#ifdef USE_IMU_ICM42605
case ACC_ICM42605:
if (icm42605AccDetect(dev)) {
accHardware = ACC_ICM42605;
break;
}
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
if (accHardwareToUse != ACC_AUTODETECT) {
break;
}
FALLTHROUGH;
#endif
#ifdef USE_IMU_BMI270
case ACC_BMI270:
if (bmi270AccDetect(dev)) {
accHardware = ACC_BMI270;
break;
}
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
if (accHardwareToUse != ACC_AUTODETECT) {
break;
}
FALLTHROUGH;
#endif
#ifdef USE_IMU_LSM6DXX
case ACC_LSM6DXX:
if (lsm6dAccDetect(dev)) {
accHardware = ACC_LSM6DXX;
break;
}
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
if (accHardwareToUse != ACC_AUTODETECT) {
break;
}
FALLTHROUGH;
#endif
#ifdef USE_IMU_FAKE
case ACC_FAKE:
if (fakeAccDetect(dev)) {
accHardware = ACC_FAKE;
break;
}
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
if (accHardwareToUse != ACC_AUTODETECT) {
break;
}
FALLTHROUGH;
#endif
default:
case ACC_NONE: // disable ACC
accHardware = ACC_NONE;
break;
}
if (accHardware == ACC_NONE) {
return false;
}
detectedSensors[SENSOR_INDEX_ACC] = accHardware;
sensorsSet(SENSOR_ACC);
return true;
}
重点关注：
#ifdef USE_IMU_ICM42605
case ACC_ICM42605:
if (icm42605AccDetect(dev)) {
accHardware = ACC_ICM42605;
break;
}
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
if (accHardwareToUse != ACC_AUTODETECT) {
break;
}
FALLTHROUGH;
#endif
bool icm42605AccDetect(accDev_t *acc)
{
acc->busDev = busDeviceOpen(BUSTYPE_ANY, DEVHW_ICM42605, acc->imuSensorToUse);
if (acc->busDev == NULL) {
return false;
}
mpuContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
if (ctx->chipMagicNumber != 0x4265) {
return false;
}
acc->initFn = icm42605AccInit;
acc->readFn = icm42605AccRead;
acc->accAlign = acc->busDev->param;
return true;
}
对于其他的配置后续在深入讲解
4.IMU 程序数据更新
4.1 陀螺仪数据更新
void FAST_CODE NOINLINE gyroUpdate(void)
{
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE_HITL)) {
//output: gyro.gyroADCf[axis]
//unused: dev->gyroADCRaw[], dev->gyroZero[];
return;
}
#endif
if (!gyro.initialized) {
return;
}
if (!gyroUpdateAndCalibrate(&gyroDev[0], &gyroCalibration[0], gyro.gyroADCf)) {
return;
}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// At this point gyro.gyroADCf contains unfiltered gyro value [deg/s]
float gyroADCf = gyro.gyroADCf[axis];
// Set raw gyro for blackbox purposes
gyro.gyroRaw[axis] = gyroADCf;
/*
* First gyro LPF is the only filter applied with the full gyro sampling speed
*/
gyroADCf = gyroLpfApplyFn((filter_t *) &gyroLpfState[axis], gyroADCf);
gyro.gyroADCf[axis] = gyroADCf;
}
}
最终我们得到陀螺仪数据：
gyro.gyroADCf[axis] = gyroADCf;
下面我们看下数据如何周期性执行的
// Function for loop trigger
void FAST_CODE taskGyro(timeUs_t currentTimeUs) {
UNUSED(currentTimeUs);
// getTaskDeltaTime() returns delta time frozen at the moment of entering the scheduler. currentTime is frozen at the very same point.
// To make busy-waiting timeout work we need to account for time spent within busy-waiting loop
const timeDelta_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
/* Update actual hardware readings */
//更新实际的数据读取
gyroUpdate();
#ifdef USE_OPFLOW
if (sensors(SENSOR_OPFLOW)) {
opflowGyroUpdateCallback(currentDeltaTime);
}
#endif
}
可以看出这里在一个任务列表中。我们需要看下任务是如何启动的：
void fcTasksInit(void)
{
//任务调度初始化
schedulerInit();
//PID任务
rescheduleTask(TASK_PID, getLooptime());
setTaskEnabled(TASK_PID, true);
//陀螺仪任务
rescheduleTask(TASK_GYRO, getGyroLooptime());
setTaskEnabled(TASK_GYRO, true);
//任务开关初始化使能
setTaskEnabled(TASK_AUX, true);
//串口使能
setTaskEnabled(TASK_SERIAL, true);
#if defined(BEEPER) || defined(USE_DSHOT)
//蜂鸣器使能
setTaskEnabled(TASK_BEEPER, true);
#endif
#ifdef USE_LIGHTS
setTaskEnabled(TASK_LIGHTS, true);
#endif
//电池使能
setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || isAmperageConfigured());
setTaskEnabled(TASK_TEMPERATURE, true);
setTaskEnabled(TASK_RX, true);
#ifdef USE_GPS
setTaskEnabled(TASK_GPS, feature(FEATURE_GPS));
#endif
#ifdef USE_MAG
setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
#if defined(USE_MAG_MPU9250)
// fixme temporary solution for AK6983 via slave I2C on MPU9250
rescheduleTask(TASK_COMPASS, TASK_PERIOD_HZ(40));
#endif
#endif
#ifdef USE_BARO
//气压计使能
setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
#endif
#ifdef USE_PITOT
//飞行传感器使能
setTaskEnabled(TASK_PITOT, sensors(SENSOR_PITOT));
#endif
#ifdef USE_ADSB
setTaskEnabled(TASK_ADSB, true);
#endif
#ifdef USE_RANGEFINDER
//测距仪使能
setTaskEnabled(TASK_RANGEFINDER, sensors(SENSOR_RANGEFINDER));
#endif
#ifdef USE_DASHBOARD
setTaskEnabled(TASK_DASHBOARD, feature(FEATURE_DASHBOARD));
#endif
#ifdef USE_TELEMETRY
setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY));
#endif
#ifdef USE_LED_STRIP
//LED 使能
setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
#endif
#ifdef STACK_CHECK
setTaskEnabled(TASK_STACK_CHECK, true);
#endif
#if defined(USE_SERVO_SBUS)
setTaskEnabled(TASK_PWMDRIVER, (servoConfig()->servo_protocol == SERVO_TYPE_SBUS) || (servoConfig()->servo_protocol == SERVO_TYPE_SBUS_PWM));
#endif
#ifdef USE_CMS
#ifdef USE_MSP_DISPLAYPORT
setTaskEnabled(TASK_CMS, true);
#else
setTaskEnabled(TASK_CMS, feature(FEATURE_OSD) || feature(FEATURE_DASHBOARD));
#endif
#endif
#ifdef USE_OPFLOW
setTaskEnabled(TASK_OPFLOW, sensors(SENSOR_OPFLOW));
#endif
#ifdef USE_VTX_CONTROL
#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP)
setTaskEnabled(TASK_VTXCTRL, true);
#endif
#endif
#ifdef USE_RCDEVICE
#ifdef USE_LED_STRIP
setTaskEnabled(TASK_RCDEVICE, rcdeviceIsEnabled() || osdJoystickEnabled());
#else
setTaskEnabled(TASK_RCDEVICE, rcdeviceIsEnabled());
#endif
#endif
#ifdef USE_PROGRAMMING_FRAMEWORK
setTaskEnabled(TASK_PROGRAMMING_FRAMEWORK, true);
#endif
#ifdef USE_IRLOCK
setTaskEnabled(TASK_IRLOCK, irlockHasBeenDetected());
#endif
#if defined(USE_SMARTPORT_MASTER)
setTaskEnabled(TASK_SMARTPORT_MASTER, true);
#endif
#ifdef USE_SERIAL_GIMBAL
setTaskEnabled(TASK_GIMBAL, true);
#endif
#ifdef USE_HEADTRACKER
setTaskEnabled(TASK_HEADTRACKER, true);
#endif
#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SBUS2)
setTaskEnabled(TASK_TELEMETRY_SBUS2,feature(FEATURE_TELEMETRY) && rxConfig()->receiverType == RX_TYPE_SERIAL && rxConfig()->serialrx_provider == SERIALRX_SBUS2);
#endif
#ifdef USE_ADAPTIVE_FILTER
setTaskEnabled(TASK_ADAPTIVE_FILTER, (
gyroConfig()->gyroFilterMode == GYRO_FILTER_MODE_ADAPTIVE &&
gyroConfig()->adaptiveFilterMinHz > 0 &&
gyroConfig()->adaptiveFilterMaxHz > 0
));
#endif
#if defined(SITL_BUILD)
serialProxyStart();
#endif
#ifdef USE_GEOZONE
setTaskEnabled(TASK_GEOZONE, feature(FEATURE_GEOZONE));
#endif
}
我们只需要重点关注：
//陀螺仪任务
rescheduleTask(TASK_GYRO, getGyroLooptime());
setTaskEnabled(TASK_GYRO, true);
加速度数据更新
void accUpdate(void)
{
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE_HITL)) {
//output: acc.accADCf
//unused: acc.dev.ADCRaw[], acc.accClipCount, acc.accVibeSq[]
return;
}
#endif
if (!acc.dev.readFn(&acc.dev)) {
return;
}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accADC[axis] = acc.dev.ADCRaw[axis];
DEBUG_SET(DEBUG_ACC, axis, accADC[axis]);
}
if (!ARMING_FLAG(SIMULATOR_MODE_SITL)) {
performAcclerationCalibration();
applyAccelerationZero();
}
applySensorAlignment(accADC, accADC, acc.dev.accAlign);
applyBoardAlignment(accADC);
// Calculate acceleration readings in G's
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
acc.accADCf[axis] = (float)accADC[axis] / acc.dev.acc_1G;
}
// Before filtering check for clipping and vibration levels
if (fabsf(acc.accADCf[X]) > ACC_CLIPPING_THRESHOLD_G || fabsf(acc.accADCf[Y]) > ACC_CLIPPING_THRESHOLD_G || fabsf(acc.accADCf[Z]) > ACC_CLIPPING_THRESHOLD_G) {
acc.isClipped = true;
acc.accClipCount++;
}
else {
acc.isClipped = false;
}
// Calculate vibration levels
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// filter accel at 5hz
const float accFloorFilt = pt1FilterApply(&accVibeFloorFilter[axis], acc.accADCf[axis]);
// calc difference from this sample and 5hz filtered value, square and filter at 2hz
const float accDiff = acc.accADCf[axis] - accFloorFilt;
acc.accVibeSq[axis] = pt1FilterApply(&accVibeFilter[axis], accDiff * accDiff);
acc.accVibe = fast_fsqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]);
}
// Filter acceleration
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
acc.accADCf[axis] = accSoftLpfFilterApplyFn(accSoftLpfFilter[axis], acc.accADCf[axis]);
}
if (accelerometerConfig()->acc_notch_hz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
acc.accADCf[axis] = accNotchFilterApplyFn(accNotchFilter[axis], acc.accADCf[axis]);
}
}
}
void setTaskEnabled(cfTaskId_e taskId, bool enabled)
{
if (taskId == TASK_SELF || taskId < TASK_COUNT) {
cfTask_t *task = taskId == TASK_SELF ? currentTask : &cfTasks[taskId];
if (enabled && task->taskFunc)
{
//添加到消息队列中去
queueAdd(task);
} else
{
queueRemove(task);
}
}
}
添加进去后，随着任务周期性执行。
void FAST_CODE NOINLINE scheduler(void)
{
// Cache currentTime
const timeUs_t currentTimeUs = micros();
// The task to be invoked
cfTask_t *selectedTask = NULL;
uint16_t selectedTaskDynamicPriority = 0;
bool forcedRealTimeTask = false;
// Update task dynamic priorities
uint16_t waitingTasks = 0;
for (cfTask_t *task = queueFirst(); task != NULL; task = queueNext())
{
// Task has checkFunc - event driven
if (task->checkFunc) {
const timeUs_t currentTimeBeforeCheckFuncCallUs = micros();
// Increase priority for event driven tasks
if (task->dynamicPriority > 0) {
task->taskAgeCycles = 1 + ((timeDelta_t)(currentTimeUs - task->lastSignaledAt)) / task->desiredPeriod;
task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
waitingTasks++;
} else if (task->checkFunc(currentTimeBeforeCheckFuncCallUs, currentTimeBeforeCheckFuncCallUs - task->lastExecutedAt)) {
const timeUs_t checkFuncExecutionTime = micros() - currentTimeBeforeCheckFuncCallUs;
checkFuncMovingSumExecutionTime -= checkFuncMovingSumExecutionTime / TASK_MOVING_SUM_COUNT;
checkFuncMovingSumExecutionTime += checkFuncExecutionTime;
checkFuncTotalExecutionTime += checkFuncExecutionTime; // time consumed by scheduler + task
checkFuncMaxExecutionTime = MAX(checkFuncMaxExecutionTime, checkFuncExecutionTime);
task->lastSignaledAt = currentTimeBeforeCheckFuncCallUs;
task->taskAgeCycles = 1;
task->dynamicPriority = 1 + task->staticPriority;
waitingTasks++;
} else {
task->taskAgeCycles = 0;
}
} else if (task->staticPriority == TASK_PRIORITY_REALTIME) {
//realtime tasks take absolute priority. Any RT tasks that is overdue, should be execute immediately
if (((timeDelta_t)(currentTimeUs - task->lastExecutedAt)) > task->desiredPeriod) {
selectedTaskDynamicPriority = task->dynamicPriority;
selectedTask = task;
waitingTasks++;
forcedRealTimeTask = true;
}
} else {
// Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods)
// Task age is calculated from last execution
task->taskAgeCycles = ((timeDelta_t)(currentTimeUs - task->lastExecutedAt)) / task->desiredPeriod;
if (task->taskAgeCycles > 0) {
task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
waitingTasks++;
}
}
if (!forcedRealTimeTask && task->dynamicPriority > selectedTaskDynamicPriority) {
selectedTaskDynamicPriority = task->dynamicPriority;
selectedTask = task;
}
}
totalWaitingTasksSamples++;
totalWaitingTasks += waitingTasks;
currentTask = selectedTask;
if (selectedTask) {
// Found a task that should be run
selectedTask->taskLatestDeltaTime = (timeDelta_t)(currentTimeUs - selectedTask->lastExecutedAt);
selectedTask->lastExecutedAt = currentTimeUs;
selectedTask->dynamicPriority = 0;
// Execute task
const timeUs_t currentTimeBeforeTaskCall = micros();
selectedTask->taskFunc(currentTimeBeforeTaskCall);
const timeUs_t taskExecutionTime = micros() - currentTimeBeforeTaskCall;
selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / TASK_MOVING_SUM_COUNT;
selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task
selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime);
}
if (!selectedTask || forcedRealTimeTask) {
// Execute system real-time callbacks and account for them to SYSTEM account
const timeUs_t currentTimeBeforeTaskCall = micros();
taskRunRealtimeCallbacks(currentTimeBeforeTaskCall);
selectedTask = &cfTasks[TASK_SYSTEM];
const timeUs_t taskExecutionTime = micros() - currentTimeBeforeTaskCall;
selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / TASK_MOVING_SUM_COUNT;
selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task
selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime);
}
}
整体思路：
4.2 加速度数据更新
void accUpdate(void)
{
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE_HITL)) {
//output: acc.accADCf
//unused: acc.dev.ADCRaw[], acc.accClipCount, acc.accVibeSq[]
return;
}
#endif
if (!acc.dev.readFn(&acc.dev)) {
return;
}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accADC[axis] = acc.dev.ADCRaw[axis];
DEBUG_SET(DEBUG_ACC, axis, accADC[axis]);
}
if (!ARMING_FLAG(SIMULATOR_MODE_SITL)) {
performAcclerationCalibration();
applyAccelerationZero();
}
applySensorAlignment(accADC, accADC, acc.dev.accAlign);
applyBoardAlignment(accADC);
// Calculate acceleration readings in G's
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
acc.accADCf[axis] = (float)accADC[axis] / acc.dev.acc_1G;
}
// Before filtering check for clipping and vibration levels
if (fabsf(acc.accADCf[X]) > ACC_CLIPPING_THRESHOLD_G || fabsf(acc.accADCf[Y]) > ACC_CLIPPING_THRESHOLD_G || fabsf(acc.accADCf[Z]) > ACC_CLIPPING_THRESHOLD_G) {
acc.isClipped = true;
acc.accClipCount++;
}
else {
acc.isClipped = false;
}
// Calculate vibration levels
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// filter accel at 5hz
const float accFloorFilt = pt1FilterApply(&accVibeFloorFilter[axis], acc.accADCf[axis]);
// calc difference from this sample and 5hz filtered value, square and filter at 2hz
const float accDiff = acc.accADCf[axis] - accFloorFilt;
acc.accVibeSq[axis] = pt1FilterApply(&accVibeFilter[axis], accDiff * accDiff);
acc.accVibe = fast_fsqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]);
}
// Filter acceleration
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
acc.accADCf[axis] = accSoftLpfFilterApplyFn(accSoftLpfFilter[axis], acc.accADCf[axis]);
}
if (accelerometerConfig()->acc_notch_hz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
acc.accADCf[axis] = accNotchFilterApplyFn(accNotchFilter[axis], acc.accADCf[axis]);
}
}
}
void imuUpdateAccelerometer(void)
{
if (sensors(SENSOR_ACC)) {
accUpdate();
isAccelUpdatedAtLeastOnce = true;
}
}
void taskMainPidLoop(timeUs_t currentTimeUs)
{
......
imuUpdateAccelerometer();
......
}
void fcTasksInit(void)
{
//任务调度初始化
schedulerInit();
//PID任务
rescheduleTask(TASK_PID, getLooptime());
setTaskEnabled(TASK_PID, true);
......
}
这里随着任务的添加而随着任务调度执行。
5.IMU 数据怎么被使用
IMU数据正常都是在姿态解算和控制中使用：
imuUpdateAttitude(currentTimeUs);
void imuUpdateAttitude(timeUs_t currentTimeUs)
{
/* Calculate dT */
static timeUs_t previousIMUUpdateTimeUs;
const float dT = (currentTimeUs - previousIMUUpdateTimeUs) * 1e-6;
previousIMUUpdateTimeUs = currentTimeUs;
if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {
gyroGetMeasuredRotationRate(&imuMeasuredRotationBF); // Calculate gyro rate in body frame in rad/s
accGetMeasuredAcceleration(&imuMeasuredAccelBF); // Calculate accel in body frame in cm/s/s
imuCheckVibrationLevels();
imuCalculateEstimatedAttitude(dT); // Update attitude estimate
} else {
acc.accADCf[X] = 0.0f;
acc.accADCf[Y] = 0.0f;
acc.accADCf[Z] = 0.0f;
}
}
重点关注：
gyroGetMeasuredRotationRate(&imuMeasuredRotationBF); // Calculate gyro rate in body frame in rad/s
accGetMeasuredAcceleration(&imuMeasuredAccelBF); // Calculate accel in body frame in cm/s/s
void gyroGetMeasuredRotationRate(fpVector3_t *measuredRotationRate)
{
for (int axis = 0; axis < 3; axis++) {
measuredRotationRate->v[axis] = DEGREES_TO_RADIANS(gyro.gyroADCf[axis]);
}
}
可以看出：
gyro.gyroADCf[axis]
FASTRAM gyro_t gyro; // gyro sensor object
void accGetMeasuredAcceleration(fpVector3_t *measuredAcc)
{
arm_scale_f32(acc.accADCf, GRAVITY_CMSS, measuredAcc->v, XYZ_AXIS_COUNT);
}
FASTRAM acc_t acc; // acc access functions
到这里我们可以看出重要的就是IMU输出的数据：
gyro.gyroADCf[axis]
acc.accADCf
6.总结
本节主要对IMU的整体数据流进行了分析，很多细节梳理的还不够完善，后续会在继续分析。