1

当我在赛道上骑摩托车时，我想计算倾斜角度，并考虑使用我的 lumia 920。引起我兴趣的产品是http://leanometer.com。我发现只使用陀螺仪是不好的，因为它会随着时间的推移而漂移，所以使用带有加速度计的互补滤波器似乎是可行的方法。我从http://www.pieter-jan.com/node/11找到了执行此操作的代码：

#define ACCELEROMETER_SENSITIVITY 8192.0
#define GYROSCOPE_SENSITIVITY 65.536

#define M_PI 3.14159265359      

#define dt 0.01                         // 10 ms sample rate!    

void ComplementaryFilter(short accData[3], short gyrData[3], float *pitch, float *roll)
{
    float pitchAcc, rollAcc;               

    // Integrate the gyroscope data -> int(angularSpeed) = angle
    *pitch += ((float)gyrData[0] / GYROSCOPE_SENSITIVITY) * dt; // Angle around the X-axis
    *roll -= ((float)gyrData[1] / GYROSCOPE_SENSITIVITY) * dt;    // Angle around the Y-axis

    // Compensate for drift with accelerometer data if !bullshit
    // Sensitivity = -2 to 2 G at 16Bit -> 2G = 32768 && 0.5G = 8192
    int forceMagnitudeApprox = abs(accData[0]) + abs(accData[1]) + abs(accData[2]);
    if (forceMagnitudeApprox > 8192 && forceMagnitudeApprox < 32768)
    {
    // Turning around the X axis results in a vector on the Y-axis
        pitchAcc = atan2f((float)accData[1], (float)accData[2]) * 180 / M_PI;
        *pitch = *pitch * 0.98 + pitchAcc * 0.02;

    // Turning around the Y axis results in a vector on the X-axis
        rollAcc = atan2f((float)accData[0], (float)accData[2]) * 180 / M_PI;
        *roll = *roll * 0.98 + rollAcc * 0.02;
    }
}

我已将此代码转换为 c#，但我无法获得准确的信息。一件事是我不知道陀螺仪/加速度计的灵敏度或如何获得它。过了一会儿，我开始用谷歌搜索更多关于加速度计和角度的信息，发现：http: //www.hobbytronics.co.uk/accelerometer-info这与加速度计上面的角度有点不同，但它似乎有效。当使用来自 hobbytronics 的算法并将其放入上面的代码中时，我得到了一个奇怪的行为，因此它总是试图接近 -1.4 度角。

我在另一台计算机上得到了真正的代码，但这就是我的做法：

var lastReading = new DateTime();
var angleX = 0.0; 
var gyro = new Gyroscope();
gyro.TimeBetweenUpdates = 20ms;
gyro.CurrentValueChanged += ValueChanged;
var acc = new Accelerometer();
acc.TimeBetweenUpdates = 20ms;
gyro.Start(); acc.Start();

void ValueChanged(SensorReading reading)
{
    if(lastReading < 1 sec old)
    {
        var dt = lastReading - reading.Timestamp;
        if(reading.X > 0.05 || Reading.X < -0.05) // Got so much errors if I did not use this, maybe too high?
        {
           var x = reading.X * dt.TotalSeconds;
           var accData = acc.CurrentValue(); 
           var accX = atan(accData.X / Math.Sqrt(accData.Y*accData.Y+accData.Z*accData.Z));
           angleX = 0.98*(angleX + x *180 / Math.PI) + 0.02 * (accX *180 /Math.PI);
           TxtAngleX.Text = "x:" + angleX.ToString("F");
        }
    }
    lastReading = reading.Timestamp;
 }

我错过了什么？使其更好的一种方法可能是仅从加速度计设置角度，当 lastReading 大于一秒时，但我知道这不是问题

4

0 回答 0