论坛首页 › 飞行器 › MWC的姿态解算求解

forwinry 发表于 2015-8-14 14:25:16

MWC的姿态解算求解

最近在学习一个德国的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* v_tmp.X + delta * v_tmp.Y;
v->X += delta* v_tmp.Z - delta   * v_tmp.Y;
v->Y += delta * v_tmp.Z + delta   * v_tmp.X;
}

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

// read gyros
mpu.getRotation(&axisRot, &axisRot, &axisRot);
axisRot -= config.gyrOffsetX;
axisRot -= config.gyrOffsetY;
axisRot -= config.gyrOffsetZ;

idx = sensorDef.Gyro.idx;
gyroADC = axisRot;
gyroADC *= sensorDef.Gyro.dir;

idx = sensorDef.Gyro.idx;
gyroADC = axisRot;
gyroADC *= sensorDef.Gyro.dir;

idx = sensorDef.Gyro.idx;
gyroADC = axisRot;
gyroADC *= sensorDef.Gyro.dir;

}

// get acceleration for 3-axis
void readACCs()
{
int16_t rawVal;
int16_t devVal;

mpu.getAcceleration(
    &rawVal,
    &rawVal,
    &rawVal
    );
   
devVal.idx]= rawVal - config.accOffsetX;
devVal.idx] = rawVal - config.accOffsetY;
devVal.idx]   = rawVal - config.accOffsetZ;

for (int8_t axis = 0; axis < 3; axis++) {
    accADC = devVal*sensorDef.Acc.dir;
}
}


void updateGyroAttitude(){
uint8_t axis;

float deltaGyroAngle;

// 43 us
for (axis = 0; axis < 3; axis++) {
    deltaGyroAngle = gyroADC* gyroScale;
}

// 111 us
rotateV(&EstG.V,deltaGyroAngle);

}

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

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

// 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, accLPF, AccComplFilterConst);
      EstG.A = EstG.A * (1.0 - AccComplFilterConst) + accLPF * AccComplFilterConst; // note: this is different from MultiWii (wrong brackets postion in MultiWii ??.
    }
}
}


void getAttiduteAngles() {

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

}

计算的原理可以理解，用陀螺仪积分和旋转矩阵求重力加速度分量，然后和加速度计测的加速度做互补滤波。但问题来了，按照代码中对EstG.V的赋值为（0,0,1g），如果用这个旋转矩阵算的话，不管IMU怎么翻转，算出来的Z轴的值都会是1g的值。因为 v->Z -= delta* v_tmp.X + delta * 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;               
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* v_tmp.X + delta * v_tmp.Y;
v->X += delta* v_tmp.Z - delta   * v_tmp.Y;
v->Y += delta * v_tmp.Z + delta   * v_tmp.X;
}

static t_fp_vector EstG;

void getEstimatedAttitude(){
uint8_t axis;
int32_t accMag = 0;
float scale, deltaGyroAngle;
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 = imu.gyroADC* scale;

    accLPF32    -= accLPF32>>ACC_LPF_FACTOR;
    accLPF32    += imu.accADC;
    imu.accSmooth    = accLPF32>>ACC_LPF_FACTOR;

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

rotateV(&EstG.V,deltaGyroAngle);

大风吹不倒 发表于 2015-8-15 13:15:33

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

forwinry 发表于 2015-8-17 08:43:30

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

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

大风吹不倒 发表于 2015-8-17 10:53:29

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

2663498351{:lol:}

大风吹不倒 发表于 2016-4-4 20:46:24

304193423 楼主 这个qq想加你请教问题{:smile:}

查看完整版本: MWC的姿态解算求解