# 04 Sensor Calibration

\# 传感器校准方案

## 一、陀螺仪校准方案

延续 Crazyflie2 陀螺仪校准方案,在初次上电时,计算陀螺仪三个轴的方差与平均值.

1. 使用一个最大长度为 1024 的环形缓冲区,存储最新的 1024 组陀螺仪测量值
2. 通过计算陀螺仪输出值方差,确认飞机已经放置平稳并且陀螺仪工作正常.
3. 确认第 2 步正常后,计算静止时 1024 组陀螺仪输出值的平均值,作为陀螺仪的校准值

**陀螺仪偏差计算源代码:**

```
/**
 * Adds a new value to the variance buffer and if it is full
 * replaces the oldest one. Thus a circular buffer.
 */
static void sensorsAddBiasValue(BiasObj* bias, int16_t x, int16_t y, int16_t z)
{
  bias->bufHead->x = x;
  bias->bufHead->y = y;
  bias->bufHead->z = z;
  bias->bufHead++;

  if (bias->bufHead >= &bias->buffer[SENSORS_NBR_OF_BIAS_SAMPLES])
  {
    bias->bufHead = bias->buffer;
    bias->isBufferFilled = true;
  }
}

/**
 * Checks if the variances is below the predefined thresholds.
 * The bias value should have been added before calling this.
 * @param bias  The bias object
 */
static bool sensorsFindBiasValue(BiasObj* bias)
{
  static int32_t varianceSampleTime;
  bool foundBias = false;

  if (bias->isBufferFilled)
  {
    sensorsCalculateVarianceAndMean(bias, &bias->variance, &bias->mean);

    if (bias->variance.x < GYRO_VARIANCE_THRESHOLD_X &&
        bias->variance.y < GYRO_VARIANCE_THRESHOLD_Y &&
        bias->variance.z < GYRO_VARIANCE_THRESHOLD_Z &&
        (varianceSampleTime + GYRO_MIN_BIAS_TIMEOUT_MS < xTaskGetTickCount()))
    {
      varianceSampleTime = xTaskGetTickCount();
      bias->bias.x = bias->mean.x;
      bias->bias.y = bias->mean.y;
      bias->bias.z = bias->mean.z;
      foundBias = true;
      bias->isBiasValueFound = true;
    }
  }

  return foundBias;
}
```

**校准陀螺仪输出值:**

```
    sensorData.gyro.x = (gyroRaw.x - gyroBias.x) * SENSORS_DEG_PER_LSB_CFG;
    sensorData.gyro.y = (gyroRaw.y - gyroBias.y) * SENSORS_DEG_PER_LSB_CFG;
    sensorData.gyro.z = (gyroRaw.z - gyroBias.z) * SENSORS_DEG_PER_LSB_CFG;
    applyAxis3fLpf((lpf2pData *)(&gyroLpf), &sensorData.gyro); //低通滤波器,去除高频干扰
```

## 二、加速度计校准方案

### 1. 重力加速度校准

由于在不同地球纬度和海拔下,重力加速度 g 值不同,需要使用加速度计对 g 进行实际测试,用于校准传感器.参考 Crazyflie2 加速度计校准方案:

1. 陀螺仪校准完成后,立刻进行加速度计校准.
2. 使用 buffer 保存 200 组加速度计测量值&#x20;
3. 通过合成重力加速度在三个轴的分量,计算重力加速度在静止状态下的值.

参考:[不同地球纬度和海拔下的不同重力加速度值 g](https://baike.baidu.com/item/%E9%87%8D%E5%8A%9B%E5%8A%A0%E9%80%9F%E5%BA%A6/23553)

**计算静止状态下重力加速度值:**

```
/**
 * Calculates accelerometer scale out of SENSORS_ACC_SCALE_SAMPLES samples. Should be called when
 * platform is stable.
 */
static bool processAccScale(int16_t ax, int16_t ay, int16_t az)
{
    static bool accBiasFound = false;
    static uint32_t accScaleSumCount = 0;

    if (!accBiasFound)
    {
        accScaleSum += sqrtf(powf(ax * SENSORS_G_PER_LSB_CFG, 2) + powf(ay * SENSORS_G_PER_LSB_CFG, 2) + powf(az * SENSORS_G_PER_LSB_CFG, 2));
        accScaleSumCount++;

        if (accScaleSumCount == SENSORS_ACC_SCALE_SAMPLES)
        {
            accScale = accScaleSum / SENSORS_ACC_SCALE_SAMPLES;
            accBiasFound = true;
        }
    }

    return accBiasFound;
}
```

**通过重力加速度值,校准加速度计测量值:**

```
    accScaled.x = (accelRaw.x) * SENSORS_G_PER_LSB_CFG / accScale;
    accScaled.y = (accelRaw.y) * SENSORS_G_PER_LSB_CFG / accScale;
    accScaled.z = (accelRaw.z) * SENSORS_G_PER_LSB_CFG / accScale;
```

### 2. 水平校准

理想状态下,加速度传感器在飞机上完全水平的进行安装,进而可以使用 0 位置作为飞机的水平面,但是由于加速度计在安装时不可避免的存在一定的倾角,导致飞控错误的估计水平位置,导致飞机向某个方向偏飞.因此需要设置一定的校准策略来平衡这种误差.

1. 将飞机放置在一个水平面上,计算飞机 `cosRoll` `sinRoll`  `cosPitch` `sinPitch` .理想状态下 `cosRoll`  `cosPitch` 为 1 ,`sinPitch` `sinRoll` 为 0 .如果不是水平安装`sinPitch` `sinRoll` 不为 0,`cosRoll`  `cosPitch` 不为 1 .
2. 将步骤 1 的 `cosRoll` `sinRoll`  `cosPitch` `sinPitch` 或对应的 `Roll` `Pitch` 角度值保存到飞机,用于校准.

**加速度计水平校准:**

```
/**
 * Compensate for a miss-aligned accelerometer. It uses the trim
 * data gathered from the UI and written in the config-block to
 * rotate the accelerometer to be aligned with gravity.
 */
static void sensorsAccAlignToGravity(Axis3f *in, Axis3f *out)
{
    //TODO: need cosPitch calculate firstly
    Axis3f rx;
    Axis3f ry;

    // Rotate around x-axis
    rx.x = in->x;
    rx.y = in->y * cosRoll - in->z * sinRoll;
    rx.z = in->y * sinRoll + in->z * cosRoll;

    // Rotate around y-axis
    ry.x = rx.x * cosPitch - rx.z * sinPitch;
    ry.y = rx.y;
    ry.z = -rx.x * sinPitch + rx.z * cosPitch;

    out->x = ry.x;
    out->y = ry.y;
    out->z = ry.z;
}
```

**以上代码目的是计算合力,以下以存在俯仰角度 15 度偏差为例,进行分析**

飞机方向图:

![飞机方向图](https://img-blog.csdnimg.cn/20191223175644388.jpg?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzIwNTE1NDYx,size_16,color_FFFFFF,t_70)

![mpu6050方向定义](https://img-blog.csdnimg.cn/20191223181325287.jpg)

分析图:

![在这里插入图片描述](https://img-blog.csdnimg.cn/20191223175759643.jpg?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzIwNTE1NDYx,size_16,color_FFFFFF,t_70)

## 三、激光传感器校准方案

请参考: [07-激光传感器VL53L1x移植与调试-附源码](/esplane/developer-guide/08esp32+-ji-guang-chuan-gan-qi-vl53l1x-yi-zhi-yu-tiao-shi-fu-yuan-ma.md)


---

# Agent Instructions: Querying This Documentation

If you need additional information that is not directly available in this page, you can query the documentation dynamically by asking a question.

Perform an HTTP GET request on the current page URL with the `ask` query parameter:

```
GET https://libooogo.gitbook.io/esplane/developer-guide/23-chuan-gan-qi-xiao-zhun-fang-an.md?ask=<question>
```

The question should be specific, self-contained, and written in natural language.
The response will contain a direct answer to the question and relevant excerpts and sources from the documentation.

Use this mechanism when the answer is not explicitly present in the current page, you need clarification or additional context, or you want to retrieve related documentation sections.