陀螺仪数据为什么看起来总在动:零偏、噪声和零漂

经典三问

Q: 陀螺仪零偏是什么?
A: 传感器静止时仍然存在的固定偏差。它不起眼,但积分之后会让角度一直朝一个方向走。

Q: 为什么静止时数据也不为零?
A: 噪声、温度带来的移动和器件本身的偏差都叠在真实的角速度上;静止只说明真实的那一项是零。

Q: 零偏校准用在哪?
A: 上电时的姿态初始化、长时间的角度积分、云台保持,以及需要分辨微小转动的测量。


模块放在桌上,什么都没碰,串口一行一行地往外吐:

gx=12  gy=-8   gz=19
gx=15  gy=-6   gz=23
gx=10  gy=-11  gz=17

我刚看到这个现象时,还以为是桌子在抖,甚至以为地震了。拿胶带把它固定住,数字还是在动。开机那会儿变化不大;放上十几分钟,屏幕上那个偏航角已经跑到没法看的地步。

屏幕上这堆变化里,是什么原因导致的?哪些是噪声,哪些是零偏,哪些是模块真的被碰了? 下面就是把这三件事拆开。

先把两个词分开

它们常被混着用,但说的不是一回事:

  • 零偏是“你明明没动,它报出来的那个数”。在某个温度、某个时刻,它是一个固定值。
  • 零漂是这个固定值本身在移动——温度变了、时间久了,刚才校好的那个数就不是原来的数了。

分开之后,很多现象立刻能对上号:零偏决定了“积分出来的角度会朝一个方向走”,零漂决定了“你校得再好,过一阵子又不对了”。两个都要处理,但处理办法不一样:前者靠减掉一个数,后者只能靠重新校准、控制温度,或者引入外部参考。

加速度计那一侧也有零偏。它和陀螺仪的区别在于:这两个角是直接由重力方向算出来的,不靠累加,所以零偏只表现为“平放时那个数不是 2048、放回去也不回到原来的值”,不会像角度那样一路跑掉。

校准之前:先确认它在说真话

五秒钟的静置能算出零偏,前提是这颗芯片本身是好的。焊虚、摔过、或者买到了坏件,读出来可能是一条纹丝不动的常数——校准会一本正经地把它当成零偏减掉,曲线干净得像刚擦过,其实全是假的。

手册里为此留了一个开关:每根轴都能单独打开自检。打开之后,芯片用电信号把敏感结构推到相当于一个预定义科里奥利力的位移,输出跟着变;关掉再变回来。两次读数相减,就是 自检响应 = 开自检时的输出 − 关自检时的输出。

陀螺的三位在 0x1B(GYRO_CONFIG)的高三位,加速度的三位在 0x1C(ACCEL_CONFIG)的高三位。把 0x1B 写成 0xE0,就是三轴陀螺全开自检;把 0x1C 写成 0xE0,是三轴加速度全开。手册的规格表给每根轴规定了自检响应的上下限:落在范围内算通过,超出算这颗芯片不合格。

那几张规格表上的数,抄下来有用:陀螺的 X、Z 两轴,自检响应落在 10 到 105 度每秒之间算通过;Y 轴是 −105 到 −10——手册专门注了一句,Y 轴自检的极性和另外两轴相反,幅度范围一样。加速度三轴是 300 到 950 毫克。所以自检响应不是“有没有数”的判断题,它带量纲、有上下限:偏小说明敏感结构推不动了,偏大说明那条轴的增益变了。

要注意 0xE0 顺手把量程改成了最小那一档(低三位写 0,FS_SEL、AFS_SEL 都成了 0),所以自检完记得把 0x1B、0x1C 都写回 0x18——那是讲这颗芯片那一回定的 ±2000 度每秒和 ±16g。

工程上的用法很简单:上电跑一次,超出范围就在屏幕上打一行 selftest failed,别急着往下校准。省下的不是几行代码,是“花一下午调参数,最后发现是模块坏了”那一段时间。

先让它安静地说五秒钟

真正开始积分之前,有几段时间躲不掉。手册给的“启动时间”是从加电到寄存器可以稳定读写,典型 20 毫秒、最坏 100 毫秒;把 PWR_MGMT_1 从睡眠里叫醒、并选陀螺做 PLL 参考之后,PLL 还要 1 到 10 毫秒才稳;陀螺自己的零偏要收进 ±1 度每秒以内,0 档低通下也要 30 毫秒。所以 MPU6050_Init() 里那个 Delay_ms(50) 不是客气,是把这几段等过去——它盖住的是典型值,要按最坏情况算,得等满 100 毫秒。等不到就开始采,前几组数带着一个还在变的零偏,平均出来一样是错的。

上电以后先别拿模块,程序自己进入校准:每 10 毫秒取一组角速度,取满 500 组,也就是静置五秒,然后求平均。

calibrating   42%
keep still

这五秒不会让传感器“变得更准”,它只是用来测出:模块没转时,读数还会自己偏多少。把这句话的答案记下来,之后从每个读数里减掉。

这两个结构体后面几段代码都要用,字段和讲这颗芯片那一回里给的一模一样,这里先照抄一遍:

typedef struct
{
    int16_t accel_x;
    int16_t accel_y;
    int16_t accel_z;
    int16_t temp;
    int16_t gyro_x;
    int16_t gyro_y;
    int16_t gyro_z;
} MPU6050_Raw;

typedef struct
{
    float accel_x_g;
    float accel_y_g;
    float accel_z_g;
    float temperature_c;
    float gyro_x_dps;
    float gyro_y_dps;
    float gyro_z_dps;
} MPU6050_Data;

#define GYRO_CALIB_COUNT   500U        /* 10ms 一组,500 组就是 5 秒 */

typedef struct
{
    float x;
    float y;
    float z;
} Gyro_Bias;

static float     gyro_sum_x = 0.0f;
static float     gyro_sum_y = 0.0f;
static float     gyro_sum_z = 0.0f;
static uint16_t  gyro_count = 0U;
static uint8_t   gyro_calibrating = 0U;
static Gyro_Bias gyro_bias = { 0.0f, 0.0f, 0.0f };
static float     yaw_deg = 0.0f;

/* 开始校准:把累加器清零,进入校准状态 */
void Gyro_CalibStart(void)
{
    gyro_sum_x = 0.0f;
    gyro_sum_y = 0.0f;
    gyro_sum_z = 0.0f;
    gyro_count = 0U;
    gyro_calibrating = 1U;
}

/* 每 10 毫秒调一次,喂进刚读到的一组数据;返回 1 表示这一拍刚校准完 */
uint8_t Gyro_CalibTask(const MPU6050_Data *data)
{
    if (gyro_calibrating == 0U)
    {
        return 0U;                                      /* 不在校准状态,什么都不做 */
    }

    gyro_sum_x += data->gyro_x_dps;
    gyro_sum_y += data->gyro_y_dps;
    gyro_sum_z += data->gyro_z_dps;
    gyro_count++;

    if (gyro_count < GYRO_CALIB_COUNT)
    {
        return 0U;                                      /* 还没采够 */
    }

    gyro_bias.x = gyro_sum_x / (float)GYRO_CALIB_COUNT;
    gyro_bias.y = gyro_sum_y / (float)GYRO_CALIB_COUNT;
    gyro_bias.z = gyro_sum_z / (float)GYRO_CALIB_COUNT;

    yaw_deg = 0.0f;                                     /* 从校准完成这一刻开始算角度 */
    gyro_calibrating = 0U;

    return 1U;
}

/* 校准进度,0~100,给屏幕用 */
uint8_t Gyro_CalibProgress(void)
{
    return (uint8_t)(((uint32_t)gyro_count * 100U) / GYRO_CALIB_COUNT);
}

uint8_t Gyro_IsCalibrating(void)
{
    return gyro_calibrating;
}

MPU6050_Data 里那三个 gyro_x_dps、gyro_y_dps、gyro_z_dps 是讲从寄存器到角度那一回换算出来的角速度(原始刻度除以灵敏度常数),这里直接用。

为什么校准不写成“等五秒”

上面这段是非阻塞的:进入校准状态以后,每 10 毫秒喂它一组数据,采够了就自己结束。主循环一点都不停——屏幕还在刷进度,按键还在响应,串口还在发。要是写成“采够 500 组再往下走”,这五秒里屏幕是死的、按键没反应,看起来像死机,而实际上它只是在等一个跟它无关的计数。

这也是前面那套形状的又一次应用:记起点、保持状态、短节拍反复来、到点动一小步。

但是话又说来了,陀螺仪很精巧啊,稍微动一下桌子,动一下小车,读数就有人为误差了,这种情况下,真的还要按按钮那些吗?屏幕和串口真的急于这五秒吗?真的需要非阻塞这么麻烦吗?我认为这需要打个问号,学会非阻塞之后,真的什么都要非阻塞吗?这里是否改成阻塞式更加好用易懂啊?

那阻塞版长什么样

说这么多,不如直接看。把上面那套状态机整个扔掉,去零偏这件事本身短得可以塞进一个函数:

/* 阻塞式去零偏:进来就采满 500 组再返回,中间什么都不干 */
void Gyro_CalibBlocking(void)
{
    MPU6050_Raw  raw;
    MPU6050_Data data;
    float        sum_x = 0.0f;
    float        sum_y = 0.0f;
    float        sum_z = 0.0f;
    uint16_t     count;

    for (count = 0U; count < GYRO_CALIB_COUNT; count++)
    {
        MPU6050_ReadRaw(&raw);
        MPU6050_Convert(&raw, &data);

        sum_x += data.gyro_x_dps;
        sum_y += data.gyro_y_dps;
        sum_z += data.gyro_z_dps;

        Delay_ms(10U);                  /* 隔 10 毫秒再采下一组 */
    }

    gyro_bias.x = sum_x / (float)GYRO_CALIB_COUNT;
    gyro_bias.y = sum_y / (float)GYRO_CALIB_COUNT;
    gyro_bias.z = sum_z / (float)GYRO_CALIB_COUNT;

    yaw_deg = 0.0f;                     /* 从校准完成这一刻重新积 */
}

调用它只要一行,插在 MPU6050_Init() 后面:

Gyro_CalibBlocking();                   /* 这五秒里程序停在这里 */

主循环里再没有 Gyro_CalibTask(),也没有 gyro_calibrating 那个标志——校准从头到尾就是一次普通的函数调用。后面那份完整程序给的还是非阻塞那版,两版只差这一处:想换,把 Gyro_CalibStart() 换成 Gyro_CalibBlocking(),再删掉主循环里那行任务调用就行。

这版确实更短、更好懂,而且这一回它卡住的东西也没什么要紧:屏幕上的进度条可以不要,串口那几行晚五秒发也不差这五秒,按键在这五秒里本来也不该被按,后面函数也不需要校准期间的错误设计了,因为校准期间就是阻塞死的,不会运行到那些函数里面去。不过这里代价说清楚——这五秒里按键不响应,屏幕停在上一帧,串口一个字节都不发;更麻烦的是它没有“中止”的口子,你要是采到一半发现手碰到了模块,只能等它采完,再按 B 重来一次。

这五秒,靠的是一个先验

校准期间真的别动

如果此时你正拿着它走来走去,程序会把那段真实旋转的平均值当成“零”,之后每一次真实转动都会被多扣掉一部分。看起来是所有角度都偏了一截,实际是校准时把运动混进去了。

装到车上再校,效果比拿在手里悬空校更贴近实际:连杆、电机、车架都在场的时候,那个平均值才是这台车静止时的真实样子。

额外的信息,额外的支撑

这套做法能成立,靠的是芯片之外的一个先验:开机那五秒,模块确实是静止的。 只要它真的没动,读出来却不是零,那就只能是它自己的问题,减掉就对了。

这个先验一破,校准不但不修,还帮倒忙:上电时车正被人搬、放在会晃的桌子上、或者还在装箱运输途中,那五秒里混进去的真实转动会被当成“零偏”记下来,之后每一拍都多扣这一份,误差比不校还大。

陀螺仪的尴尬在于,它只跟自己比:一个固定零偏和一个匀速转动,在读数上是同一个常数,它分不出来。没有外部参考——水平面、磁力计、视觉、GPS——它连“我现在到底动没动”都不能确定。所以零偏修不干净不是程序写得不好,是这个器件的硬伤。就像一条腿不利索的人,没有拐杖、没有墙可扶,再怎么自己使劲也站不成正常人那样。 磁力计给航向定一个北,加速度计给姿态定一个“下”,有了这些参照,陀螺才算有地方可扶。

后面那些“加磁力计”“做组合导航”的做法,本质上不是给陀螺锦上添花,是给它一副拐杖——回到开头那个问题:噪声、零偏、真实的转动,这三样分不分得开,靠的不只是代码写得对不对,还看手上有没有一个独立的参照。

积分:角度是怎么来的

角速度要变成角度,就一句话:这一拍的角速度乘上这一拍的时间,加到角度上。

void Gyro_Update(const MPU6050_Data *data, float dt_s)
{
    float gz;

    if (gyro_calibrating != 0U)
    {
        return;                                         /* 校准期间不积分 */
    }

    gz = data->gyro_z_dps - gyro_bias.z;                /* 减掉零偏 */
    yaw_deg += gz * dt_s;

    while (yaw_deg > 180.0f)                            /* 绕出一圈就折回来 */
    {
        yaw_deg -= 360.0f;
    }
    while (yaw_deg < -180.0f)
    {
        yaw_deg += 360.0f;
    }
}

dt_s 是这一拍与上一拍之间真实经过的秒数,从每毫秒加一的那条时间轴上算出来,不是写死的 0.01。这一点很容易被忽略,但它是积分能不能信的前提:

dt_s = (float)((uint32_t)(tick_ms - read_last_ms)) * 0.001f;
read_last_ms = tick_ms;

为什么不能写死? 因为读数周期并不总是 10 毫秒。屏幕刷新一次要几十毫秒,串口在忙,按键在处理——这些都会让某一拍变长。如果 dt_s 永远写 0.01,而实际过了 0.02 秒,这一拍就少算了一半的角度,误差不会自己纠正,而是整体缩放:转过 90 度,屏幕上显示 45 度。

用时间轴算出来的 dt_s 不怕这个:循环晚了,它自己就变大了,角度还是对的。

这套时间戳够准吗

tick_ms 的分辨率是 1 毫秒。10 毫秒一拍的时候,这个分辨率带来的误差不算小。

真实间隔落在 [diff, diff + 1) 毫秒之间,代码拿 diff 当它,小数部分被截掉了。判断条件是“到了 10 毫秒就走”,所以绝大多数时候 diff 就是 10,而真实间隔平均是 10.5 毫秒。用 10 去乘,每一拍少算 0.5 毫秒,也就是 4.8%。

这个误差不会互相抵消,它会稳定累积:转一整圈 360 度,屏幕上是 342 度左右;想“转 90 度就停”,会停在 85.7 度。

改起来不复杂:把时间戳换成微秒,误差就从 5% 降到 0.5%。用一路 1 MHz 的自由运行计数器,或者把定时器计数器的低位也读出来,都行。

手册给的答案是:别拿单片机的时间去量

上面那套还在用“单片机的时间轴”去数“传感器出了几个样本”,绕远了一步。

这颗芯片的采样率由它自己的时钟决定:SMPLRT_DIV 写 0x09,就是每 10 毫秒出一个样本,这个 10 毫秒是芯片内部的时基,比单片机那个 1 毫秒计数器准得多。手册 §7.17(FIFO)和 §7.18(中断)给了两条取数的路:

  • INT_ENABLE 的最低位 DATA_RDY_EN,有新样本时拉中断;
  • 1024 字节的 FIFO,带一个计数器,可以攒一批再一次读出来。

走这两条路,积分的 dt 就回到标称值 0.01——没有量化误差,也不会漏样本。因为“每来一个样本积一次”和“每 10 毫秒积一次”,在采集这一环本来就是一回事,前者还顺手把主循环的抖动挡在了外面。

按精度排一下

做法 精度 代价
FIFO 攒批 + 按样本数积分,dt 用标称 0.01 最准,也不会漏样本 要处理溢出、要算攒了几个
数据就绪中断,每中断积一次,dt 用标称 很准 中断里做 I2C 会拉长中断
微秒时间戳 + 实测 dt 量化误差降到 0.5% 多几行代码
毫秒时间戳 + 实测 dt,也就是上面这一套 约 5% 的系统性偏小 ——
写死 0.01f 主循环一慢就整体缩放 ——

还有一个不起眼的地方:dt_s 是在判断“到点了”那一刻算出来的,然后才去读 I2C。100 kHz 下读十四个字节约 1.4 毫秒,样本其实是在时间戳之后才采到的。慢速旋转可以忽略,转得快时会有一点相位偏差。

三个现场,把三件事分开

校准结束以后,程序每 200 毫秒往串口发一行四个数:

gz, corrected_gz, yaw, bias_z

一行纯数字、用逗号隔开、行尾回车换行,这正是 VOFA+ 的文本模式要的格式(前面把 printf 接到串口那一回里用过)。四个通道分别是原始角速度、减掉零偏之后的角速度、积分出来的偏航角、以及当前用的零偏。屏幕上同步显示这几行,不接电脑时也有个现场。

void Gyro_PrintCsv(const MPU6050_Data *data)
{
    printf("%d,%d,%d,%d\r\n",
           (int32_t)(data->gyro_z_dps * 100.0f),                        /* 原始 */
           (int32_t)((data->gyro_z_dps - gyro_bias.z) * 100.0f),        /* 去偏后 */
           (int32_t)(yaw_deg * 100.0f),                                 /* 积分出来的偏航 */
           (int32_t)(gyro_bias.z * 100.0f));                            /* 当前零偏 */
}

小数都乘 100 变成整数发出去,收的时候当百分之一读——这条办法在讲浮点打印那一回交代过,在任何工具链配任何库里都好使。

现场一:桌面固定十秒

让它跑完校准,然后十秒内谁都别碰。会看到两样东西:

  • 抖:原始 gz 在一小段范围里来回跳,去偏后的 corrected_gz 围绕 0 上下晃。这部分是噪声,减掉平均值不会让它消失。
  • 走:yaw 仍然会慢慢往一个方向挪。这正是零偏没被减干净的那一部分,也是这里后面要算的那件事。

屏幕上的 bias 那一行就是刚才校出来的零偏。把它记下来:模块不动,重新校准一次,这个数一般不会完全一样——它不是器件的常数,是一个会飘的数。

现场二:手指敲一下桌面

轻轻敲桌面,gz 会跳出一个尖峰,yaw 跟着往上抬一点,松手之后停在新的位置上。

这一条必须分清性质:尖峰是真实的机械运动,不是噪声。 桌子被敲了,振动真的传到了芯片里。判断办法很直接——用手拿着模块慢慢转,读数应该平滑变化;敲桌子出来的尖峰只有几十毫秒宽,和“慢慢转”完全不是一个时间尺度。

把这两种东西混在一起处理,就会出现一个典型的错误:加一个很重的平均滤波,尖峰被削平了,看起来“干净多了”,可车压过一个坎的时候,滤波器也把那一下削掉了。削掉的是真信息。

现场三:绕 Z 轴转一圈,再转回原位

看着 yaw 慢慢转过去:往一个方向转,它跟着长;停下来,它停在新位置上;再反向转回原来的方向,它不会回到零,一般会差几度。

这一次不是噪声,也不是校准没做,而是积分本身的性质:yaw 是角速度一点点累加出来的,每一拍的小误差都被加进去了,加够久就成了几度、几十度。陀螺仪告诉你“转得多快”,它从来没有告诉你“现在朝哪”。

校准前后,同一条曲线

把 bias_z 那一栏和另外两栏放在一起看,就能把“校准解决了什么、没解决什么”分开:

看什么 校准前 校准后
gz 的平均值 明显偏离 0,比如 +19 corrected_gz 在 0 附近晃
gz 的抖动幅度 十几到几十个刻度 一样,噪声没变
十秒里 yaw 走的路 往一个方向稳定地走 走得慢多了,但仍在走
温度变了以后 —— 又走快了,因为零偏跟着温度移动了

最后一行是零漂在起作用。它解释了为什么“校一次就一劳永逸”是不成立的。

减不干净这件事可以算。 手册上这颗芯片在 100Hz 那一档的总有效值噪声是 0.05 度每秒。采 500 组求平均,平均值本身的残余不确定度大约是

0.05 ÷ √500 ≈ 0.0022 度每秒

按这个残余偏差积分一小时:0.0022 × 3600 ≈ 8 度。也就是说,校准做到这个程度,一小时后角度误差就到了八度左右——还在漂,只是慢了一个数量级。

顺带看一个规律:组数从 50 涨到 500(十倍),残余只降到 1/√10,大约三分之一。校准时长的收益是开根号的,这就是为什么没人靠“多采一会儿”把漂移解决掉。

零漂为什么最难缠

零偏能被减掉,是因为它有“固定”的那一面;零漂偏偏把这一面拆掉。手册里那颗芯片的初始零偏保证值是 ±20 度每秒,全温区(−40℃ 到 +85℃)的变化也是 ±20 度每秒——两者同一量级。也就是说,温度一动,刚校好的那个数就可能变成另一个数。

这颗芯片自己带一个温度传感器,在 0x41、0x42 两个字节里,测的是芯片自己的结温,不是室温;上面那个 MPU6050_Data 里的 temperature_c 就是从这儿换算来的。它给了两条路:一条是每换一个温度环境就重校一次,一条是把温度也读出来,做一张“温度—零偏”的补偿表——比如每十度记一个数,中间线性插值。两条都比“校一次管一辈子”强,第二条麻烦,但不用总去按校准键。

再补一句:自检和温度这两件事都不解决漂移。它们只回答“现在这个数可不可信”,不负责让这个数变准。

零漂还有一条不太显眼的来源:时基本身。PWR_MGMT_1 的低三位选时钟源,0 是内部 8 兆赫的 RC 振荡器,1 到 3 是拿某一轴陀螺做 PLL 参考。手册给的数是:内部 RC 常温偏差 ±5%,全温区会飘到 −15% 到 +10%;换成陀螺 PLL 之后,常温 ±1%,全温区也是 ±1%。留下内部 RC,等于让“一秒”本身随温度伸缩十几个百分点——零偏还没修,时间的刻度先变了。初始化时写 0x01 不只是“把陀螺叫醒”,它同时把时基换成了一个稳得多的东西。

噪声那一侧还有另一个量法。手册给的角速度噪声密度是 0.005 度每秒每根号赫兹,把它乘 60,就是角度随机游走约 0.3 度每根号小时。这个单位读起来别扭,意思很直白:积分时间越长,角度上的不确定度越大,一小时约 0.3 度,四小时约 0.6 度——噪声这一项在按时间的平方根长大。

除了零偏、零漂和噪声,手册里还有两行容易被跳过的误差。一行是线性加速度灵敏度 0.1 度每秒每 g:芯片在受加速度的时候,陀螺读数会跟着偏一点。小车一颠、电机一启停,这一项就出场了——它不是“真在转”,但绕 Z 轴积分照样会把它算进去。另一行是电源纹波灵敏度:电源上叠 100 毫伏峰峰值的正弦纹波,频率在 1 到 250 赫兹时输出多出 0.2 度每秒,频率更高(250 赫兹到 100 千赫兹)是 4 度每秒。电机 PWM 一开,电源上就全是这种纹波。这两项都不大,但它们说明一件事:读数里的偏差不止“零偏”一种,减掉一个常数只治得了其中一种。
两个机制叠在一起,就是惯性器件的宿命:短时间非常准,时间一长必须有人拉它一把。 陀螺仪自己拉不了自己。

软件滤波不是万能橡皮擦

有一类问题,滤波一点忙都帮不上,因为它们不在信号里:

现场 滤波能做什么
SDA、SCL 接反 一点都读不到,滤波无从下手
轴映射错了,右转被当成左转 曲线很平滑,方向是反的
量程配置和换算常数不一致 数值整体差 8 倍,平滑得很整齐
模块没固定,线在拉着它 抖动被当成运动滤掉,“变稳”了但数据是假的
校准的时候拿着模块走 零偏里混进了真实旋转,之后每次都多扣一点
dt 写死成 0.01 角度整体缩放:转 90 度显示 45 度

这张表里最该动手试一次的是最后一行:把 dt_s 写死成 0.01f,再把读数周期从 10 毫秒改成 20 毫秒(把那个 >= 10U 改成 >= 20U)。转过 90 度,屏幕上就是 45 度。曲线很光滑,一点噪声都没有,但它是错的。

还有一个更直白的反例:把 gyro_bias.z 在代码里写成一个明显错误的数,比如 5.0f。曲线看起来可能“更稳”或者整体偏向一边,但 yaw 照样错,甚至错得更有规律——滤波处理的是信号的形状,它不替你确认接线、坐标、安装和时间的算法。

几个能改一行复现的故障

屏幕和按键僵五秒。 把校准写成“采够 500 组再往下走”的阻塞循环,上电后按什么都没反应。改成上面的非阻塞任务,同样的五秒,屏幕上的进度条一直在走。

转过 90 度显示 45 度。 前面说过的 dt 写死。判断方法:慢慢转到一个看得出来的角度(对着桌边、对着墙),比较屏幕上的数;差成整数倍,就先查时间是怎么算的,不要先去动滤波参数。

每次上电的零偏都不一样。 把 GYRO_CALIB_COUNT 从 500 改成 20,多上电几次,看 bias 那一栏跳得多厉害;改回 500,跳动明显变小。这也是“组数收益开根号”的现场版。

校准完了还是往一边走。 先把手按住芯片或让它贴着桌面别动,重新校准一次,看 yaw 的漂移有没有变小。没变小,再去看是不是把模块放在电机边上、或者校准时手还在碰它。

按键长按没反应。 检查长按事件有没有被接上:按键那一回的接口是取事件式的,event.id 是哪一个键、event.type 是单击、长按还是别的;长按成立之后松手不会再补一次单击,所以只用长按触发重校不会一次触发两次。

一份完整的程序

把上面几段接起来。校准是非阻塞的,重新校准挂在长按 B 上,积分用的时间间隔从时间轴算。

#include "stm32f10x.h"
#include <stdio.h>

static MPU6050_Raw  g_raw;
static MPU6050_Data g_data;

/* 屏幕上四行:不校准时显示角度、原始角速度、零偏、去偏后的角速度 */
void Gyro_ShowOnOled(const MPU6050_Data *data)
{
    if (Gyro_IsCalibrating() != 0U)
    {
        OLED_ShowString(0, 0, "calibrating", OLED_8X16);
        OLED_ShowNum(0, 16, Gyro_CalibProgress(), 3, OLED_8X16);
        OLED_ShowString(32, 16, "%", OLED_8X16);
        OLED_ShowString(0, 32, "keep still", OLED_8X16);
        OLED_Update();
        return;
    }

    OLED_ShowString(0, 0, "yaw", OLED_8X16);
    OLED_ShowSignedNum(40, 0, (int32_t)yaw_deg, 4, OLED_8X16);

    OLED_ShowString(0, 16, "raw", OLED_8X16);
    OLED_ShowSignedNum(40, 16, (int32_t)(data->gyro_z_dps * 100.0f), 6, OLED_8X16);

    OLED_ShowString(0, 32, "bias", OLED_8X16);
    OLED_ShowSignedNum(40, 32, (int32_t)(gyro_bias.z * 100.0f), 5, OLED_8X16);

    OLED_ShowString(0, 48, "corr", OLED_8X16);
    OLED_ShowSignedNum(40, 48,
                       (int32_t)((data->gyro_z_dps - gyro_bias.z) * 100.0f),
                       6, OLED_8X16);

    OLED_Update();
}

int main(void)
{
    uint32_t read_last_ms = 0U;
    uint32_t show_last_ms = 0U;
    float    dt_s;
    KeyEvent event;

    OLED_Init();
    MPU6050_Init();                     /* 里面先读一次身份寄存器;没应答就说明这颗芯片不在 */
    Gyro_CalibStart();                  /* 上电就开始校准,屏幕上会显示进度 */

    while (1)
    {
        if ((uint32_t)(tick_ms - read_last_ms) >= 10U)
        {
            dt_s = (float)((uint32_t)(tick_ms - read_last_ms)) * 0.001f;
            read_last_ms = tick_ms;

            MPU6050_ReadRaw(&g_raw);
            MPU6050_Convert(&g_raw, &g_data);

            Gyro_CalibTask(&g_data);    /* 在不在校准状态,它自己判断 */
            Gyro_Update(&g_data, dt_s);
        }

        while (Key_GetEvent(&event) != 0U)
        {
            if ((event.id == KEY_B) && (event.type == KEY_EVENT_LONG_HOLD))
            {
                Gyro_CalibStart();      /* 长按 B:重新校准,屏幕回到进度页 */
            }
        }

        if ((uint32_t)(tick_ms - show_last_ms) >= 200U)
        {
            show_last_ms = tick_ms;
            Gyro_ShowOnOled(&g_data);
            Gyro_PrintCsv(&g_data);
        }
    }
}

几处白话交代:tick_ms 是那条每毫秒加一的时间轴;MPU6050_Init/ReadRaw/Convert 是读这颗芯片那三个函数(初始化并做身份检查、连读十四个字节、换算成 g 和度每秒);Key_GetEvent 是按键的取事件接口,KEY_B 是 B 键,KEY_EVENT_LONG_HOLD 表示长按成立;那几个 OLED 显示函数分别是写字符串、写无符号整数、写带符号整数。

最后橘猫说

零偏靠减掉一个数就消掉了,零漂消不掉——它自己会走,温度一变,刚校好的那个数就成了另一个数。噪声也压不到零:校准时多采几组求平均,残余只按组数的平方根往下走,50 组换 500 组也只降到三分之一,指望“多等一会儿”或者“加个滤波”把它治好,方向就错了。所以先分清屏幕上那是抖、是漂,还是模块真的被碰了——这三种的下一步完全不同,分不清,后面的参数调整就全靠猜。

posted @ 2026-11-16 12:48  Zw-awa  阅读(95)  评论(0)    收藏  举报