Question

我在我的四轴飞行器项目中使用lis3mdl磁力计来补偿陀螺仪的漂移。不幸的是，我可能在校准方面遇到问题。 Ive达到最大值和最小值（奇怪的是它们是14位而不是16位），并计算出这样的偏差：

biases[i] = (MAX_VALUES[i]+MIN_VALUES[i])/2;

（其中i代表3的每个轴）。

Ive从原始值x = (double)new_x-biases[0];（等）中减去了偏差，然后想要这样计算标题：

heading = atan2(x,y);
heading += declinationAngle;

计算偏角。结果是角度（从弧度heading*(180/M_PI)转换），当Iam在偏航轴中旋转四边形时，它的确会发生变化，而当我在横摇和俯仰轴中旋转值时，BUT也会发生变化。我想获得稳定的偏航值，当Iam在其他轴上旋转对象时，该值不会改变。也许用加速度计进行某种类型的融合？

我不确定我何时在计算中犯了错误...

整个课程：

class Magnetometer {
int x=0,y=0,z=0;
LIS3MDL mag;
int running_min[3] = {32767, 32767, 32767}, running_max[3] = {-32768, -32768, -32768};
double mag_norm = 0.0;
double declinationAngle = 0.0;
double heading=0.0;

const int MAX_VALUES[3] = {3014,3439,10246};
const int MIN_VALUES[3] = {-4746, -4110, 492};

double biases[3] = {0.0};
double scales[3] = {0.0};
double avg_scale = 0.0;

ButterworthDLPF xyz_filter[3];

double DLPF_ON = true;
const float sample_rate = MAG_SAMPLE_RATE;
const float cutoff_freq = 4.0;

public:
Magnetometer() {}
void Init() {
    declinationAngle = (6.0 + (8.0 / 60.0)) / (180.0 / M_PI);
    for(int i=0; i<3; i++) {
        biases[i] = (MAX_VALUES[i]+MIN_VALUES[i])/2;
        scales[i] = (MAX_VALUES[i]-MIN_VALUES[i])/2;
    }
    avg_scale = (scales[0]+scales[1]+scales[2])/3.0;
    for(int i=0; i<3; i++) scales[i] = avg_scale / scales[i];
    Serial.println("Turning on magnetometer. . .");
    if(!mag.init()) {
        Serial.println("Failed to detect magnetometer!");
        ESP.restart();
    }
    mag.enableDefault();
    //Calibrate();
    for(int i=0; i<3; i++) xyz_filter[i].Init(sample_rate, cutoff_freq);
    Serial.println("9DOF readdy!");
}
void Calibrate() {
    delay(100);

    while(true) {
        mag.read();
        if(running_max[0]<mag.m.x) running_max[0] = mag.m.x;
        if(running_max[1]<mag.m.y) running_max[1] = mag.m.y;
        if(running_max[2]<mag.m.z) running_max[2] = mag.m.z;
        if(running_min[0]>mag.m.x) running_min[0] = mag.m.x;
        if(running_min[1]>mag.m.y) running_min[1] = mag.m.y;
        if(running_min[2]>mag.m.z) running_min[2] = mag.m.z;
        Serial.println((String)running_max[0]+" "+(String)running_max[1]+" "+(String)running_max[2]+ "  "+(String)running_min[0] +" "+(String)running_min[1]+" "+(String)running_min[2]);
        delay(20);
    }

}
void Update(){ 
    mag.read();

    xyz_filter[0].Update(mag.m.x);
    xyz_filter[1].Update(mag.m.y);
    xyz_filter[2].Update(mag.m.z);

    //Serial.println(xyz_filter[0].getData());

    /*x = ((double)xyz_filter[0].getData()-biases[0])*scales[0];
    y = ((double)xyz_filter[1].getData()-biases[1])*scales[1];
    z = ((double)xyz_filter[2].getData()-biases[2])*scales[2];*/

    x = ((double)mag.m.x-biases[0])*scales[0];
    y = ((double)mag.m.y-biases[1])*scales[1];
    z = ((double)mag.m.z-biases[2])*scales[2];

    CalculateHeading();
}
void CalculateHeading() {
    heading = atan2(y,x);
    heading += declinationAngle;
    //if(heading<0) heading += 2*PI;
    //else if(heading>2*PI) heading -= 2*PI;
    heading=MOD(heading*(180/M_PI));
}
double GetHeading() {return heading;}
void ShowRawValues(bool names=false) {
    if(names) Serial.print("X: "+(String)x+"  Y: "+ (String)y+ "  Z: " + (String)z);
    else Serial.print((String)x+" "+ (String)y+ " " + (String)z);
}
};

磁力计偏差问题

0 个答案: