complementary_filter.py 文件源码

def ComplementaryFilter(gyrData, accData, pitch, roll):

    pitchAcc = 0.0
    rollAcc = 0.0

    # Integrate the gyroscope data -> int(angularSpeed) = angle
    pitch += (gyrData[0] / GYROSCOPE_SENSITIVITY) * \
        dt  # Angle around the X-axis
    roll -= (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
    forceMagnitudeApprox = abs(accData[0]) + abs(accData[1]) + abs(accData[2])
    if (forceMagnitudeApprox > 8192 and forceMagnitudeApprox < 32768):
            # Turning around the X axis results in a vector on the Y-axis
        pitchAcc = math.atan2(accData[1], 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 = math.atan2(accData[0], accData[2]) * 180 / M_PI
        roll = roll * 0.98 + rollAcc * 0.02
    return pitch, roll