03 Sensor Angle Fusion

版本	日期	更新记录
V0.1	发布于2019-09-21 23:54:16	初始版本
V0.2	更新于2019-12-18 21:54:16	附源码

一、姿态解算

姿态的数学表示方法 姿态解算过程

• 欧拉角:莱昂哈德·欧拉用欧拉角来描述刚体在三维欧几里得空间的取向.对于在三维空间里的一个参考系,任何坐标系的取向,都可以用三个欧拉角来表现.参考系又称为实验室参考系,是静止不动的.而坐标系则固定于刚体,随着刚体的旋转而旋转.

欧拉角

• 四元数:旋转角到一个四维向量的转换.

  需要用到三角函数运算(这里会涉及到用基本运算等价与三角函数运算)

二、姿态解算源码解析

void estimatorComplementary(state_t *state, sensorData_t *sensorData, control_t *control, const uint32_t tick)
{
  sensorsAcquire(sensorData, tick); // Read sensors at full rate (1000Hz)
  if (RATE_DO_EXECUTE(ATTITUDE_UPDATE_RATE, tick)) {        //250hz
    sensfusion6UpdateQ(sensorData->gyro.x, sensorData->gyro.y, sensorData->gyro.z,
                       sensorData->acc.x, sensorData->acc.y, sensorData->acc.z,
                       ATTITUDE_UPDATE_DT);
#ifdef DEBUG 
    ESP_LOGD(TAG,"ax = %f,  ay = %f,  az = %f,  gx = %f,  gy = %f,  gz = %f , hx = %f , hy = %f, hz =%f \n", sensorData->acc.x, sensorData->acc.y, sensorData->acc.z, sensorData->gyro.x, sensorData->gyro.y, sensorData->gyro.z, sensorData->mag.x, sensorData->mag.y, sensorData->mag.z);
#endif
    // Save attitude, adjusted for the legacy CF2 body coordinate system
    sensfusion6GetEulerRPY(&state->attitude.roll, &state->attitude.pitch, &state->attitude.yaw);

    // Save quaternion, hopefully one day this could be used in a better controller.
    // Note that this is not adjusted for the legacy coordinate system
    sensfusion6GetQuaternion(
      &state->attitudeQuaternion.x,
      &state->attitudeQuaternion.y,
      &state->attitudeQuaternion.z,
      &state->attitudeQuaternion.w);

    state->acc.z = sensfusion6GetAccZWithoutGravity(sensorData->acc.x,
                                                    sensorData->acc.y,
                                                    sensorData->acc.z);

    positionUpdateVelocity(state->acc.z, ATTITUDE_UPDATE_DT);
  }

  if (RATE_DO_EXECUTE(POS_UPDATE_RATE, tick)) {      //100hz
    // If position sensor data is preset, pass it throught
    // FIXME: The position sensor shall be used as an input of the estimator
    if (sensorData->position.timestamp) {
      state->position = sensorData->position;
    } else {
      positionEstimate(state, sensorData, POS_UPDATE_DT, tick);
    }
  }
}