MWC的姿态解算求解

forwinry

最近在学习一个德国的BruGi的开源两轴云台的代码，他在姿态解算上参考了MWC的旋转矩阵计算方法。以下是部分代码：

void initIMU() {

  // resolutionDevider=131, scale = 0.000133
  // 102us
  gyroScale =  1.0 / resolutionDevider / 180.0 * 3.14159265359 * DT_FLOAT;
  
  // initialize complementary filter timw constant
  setACCtc(config.accTimeConstant);

  accMag = ACC_1G*ACC_1G; // magnitude = 1G initially

  // initialize coordinate system in EstG
  EstG.V.X = 0;
  EstG.V.Y = 0;
  EstG.V.Z = ACC_1G;

}

// Rotate Estimated vector(s) with small angle approximation, according to the gyro data
// needs angle in radian units !
inline void rotateV(struct fp_vector *v,float* delta) {
  fp_vector v_tmp = *v;
  v->Z -= delta[ROLL]  * v_tmp.X + delta[PITCH] * v_tmp.Y;
  v->X += delta[ROLL]  * v_tmp.Z - delta[YAW]   * v_tmp.Y;
  v->Y += delta[PITCH] * v_tmp.Z + delta[YAW]   * v_tmp.X;
}

void readGyros() {
  int16_t axisRot[3];
  char idx;
  // 414 us

  // read gyros
  mpu.getRotation(&axisRot[0], &axisRot[1], &axisRot[2]);
  axisRot[0] -= config.gyrOffsetX;
  axisRot[1] -= config.gyrOffsetY;
  axisRot[2] -= config.gyrOffsetZ;
  
  idx = sensorDef.Gyro[0].idx;
  gyroADC[ROLL] = axisRot[idx];
  gyroADC[ROLL] *= sensorDef.Gyro[0].dir;

  idx = sensorDef.Gyro[1].idx;
  gyroADC[PITCH] = axisRot[idx];
  gyroADC[PITCH] *= sensorDef.Gyro[1].dir;

  idx = sensorDef.Gyro[2].idx;
  gyroADC[YAW] = axisRot[idx];  
  gyroADC[YAW] *= sensorDef.Gyro[2].dir;
  
}

// get acceleration for 3-axis
void readACCs()
{
  int16_t rawVal[3];
  int16_t devVal[3];
  
  mpu.getAcceleration(
    &rawVal[0],
    &rawVal[1],
    &rawVal[2]
    );
   
  devVal[sensorDef.Acc[ROLL].idx]  = rawVal[0] - config.accOffsetX;
  devVal[sensorDef.Acc[PITCH].idx] = rawVal[1] - config.accOffsetY;
  devVal[sensorDef.Acc[YAW].idx]   = rawVal[2] - config.accOffsetZ;
  
  for (int8_t axis = 0; axis < 3; axis++) {
    accADC[axis] = devVal[axis]*sensorDef.Acc[axis].dir;
  }
}


void updateGyroAttitude(){
  uint8_t axis;
  
  float deltaGyroAngle[3];

  // 43 us
  for (axis = 0; axis < 3; axis++) {
    deltaGyroAngle[axis] = gyroADC[axis]  * gyroScale;
  }
  
  // 111 us
  rotateV(&EstG.V,deltaGyroAngle);

}

void updateACC(){
  uint8_t axis;
  float accMagSum = 0;

  for (axis = 0; axis < 3; axis++) {
    accLPF[axis] = accADC[axis];
    accMagSum += accLPF[axis]*accLPF[axis];
  }

  // 24 us
  accMagSum = accMagSum*100.0/(ACC_1G*ACC_1G);
  utilLP_float(&accMag, accMagSum, (1.0f/ACC_LPF_FACTOR));
}


void updateACCAttitude(){
  uint8_t axis;
  
  // 80 us
  // Apply complimentary filter (Gyro drift correction)
  // If accel magnitude >1.4G or <0.6G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
  // To do that, we just skip filter, as EstV already rotated by Gyro
  if (( 36 < accMag && accMag < 196 ) || disableAccGtest) {
    for (axis = 0; axis < 3; axis++) {
      //utilLP_float(&EstG.A[axis], accLPF[axis], AccComplFilterConst);
      EstG.A[axis] = EstG.A[axis] * (1.0 - AccComplFilterConst) + accLPF[axis] * AccComplFilterConst; // note: this is different from MultiWii (wrong brackets postion in MultiWii ??.
    }
  }
}


void getAttiduteAngles() {

  // attitude of the estimated vector  
  // 200us
  angle[ROLL]  = angleOffsetRoll +  Rajan_FastArcTan2_deg1000(EstG.V.X , sqrt(EstG.V.Z*EstG.V.Z+EstG.V.Y*EstG.V.Y));
  // 142 us
  angle[PITCH] = angleOffsetPitch + Rajan_FastArcTan2_deg1000(EstG.V.Y , EstG.V.Z);

}

计算的原理可以理解，用陀螺仪积分和旋转矩阵求重力加速度分量，然后和加速度计测的加速度做互补滤波。但问题来了，按照代码中对EstG.V的赋值为（0,0,1g），如果用这个旋转矩阵算的话，不管IMU怎么翻转，算出来的Z轴的值都会是1g的值。因为 v->Z -= delta[ROLL]  * v_tmp.X + delta[PITCH] * v_tmp.Y;而X和Y都是初始为0。
所以现在就很迷茫，是我对代码的理解有误，还是代码中对EstG.V的值赋错了，如果是值错了，那EstG.V应该是什么呢。

同时附上MWC的部分代码，里面也用上了EstG，但MWC里反而没有对EstG赋值，所以弄的我更糊涂了。

typedef struct fp_vector {               
  float X,Y,Z;               
} t_fp_vector_def;

typedef union {               
  float A[3];               
  t_fp_vector_def V;               
} t_fp_vector;

// Rotate Estimated vector(s) with small angle approximation, according to the gyro data
void rotateV(struct fp_vector *v,float* delta) {
  fp_vector v_tmp = *v;
  v->Z -= delta[ROLL]  * v_tmp.X + delta[PITCH] * v_tmp.Y;
  v->X += delta[ROLL]  * v_tmp.Z - delta[YAW]   * v_tmp.Y;
  v->Y += delta[PITCH] * v_tmp.Z + delta[YAW]   * v_tmp.X;
}

static t_fp_vector EstG;

void getEstimatedAttitude(){
  uint8_t axis;
  int32_t accMag = 0;
  float scale, deltaGyroAngle[3];
  uint8_t validAcc;
  static uint16_t previousT;
  uint16_t currentT = micros();

  scale = (currentT - previousT) * GYRO_SCALE;
  previousT = currentT;

  // Initialization
  for (axis = 0; axis < 3; axis++) {
    deltaGyroAngle[axis] = imu.gyroADC[axis]  * scale;

    accLPF32[axis]    -= accLPF32[axis]>>ACC_LPF_FACTOR;
    accLPF32[axis]    += imu.accADC[axis];
    imu.accSmooth[axis]    = accLPF32[axis]>>ACC_LPF_FACTOR;

    accMag += (int32_t)imu.accSmooth[axis]*imu.accSmooth[axis] ;
  }

  rotateV(&EstG.V,deltaGyroAngle);

大风吹不倒

求源码  最近在看 apm的飞控源码  有点像

forwinry

大风吹不倒 发表于 2015-8-15 13:15
求源码  最近在看 apm的飞控源码  有点像

你qq多少，我加你，发给你

大风吹不倒

forwinry 发表于 2015-8-17 08:43
你qq多少，我加你，发给你

2663498351

大风吹不倒

304193423 楼主 这个qq想加你请教问题