示例#1
0
void computeMPU6000RTData(void)
{
    uint8_t  axis;
    uint16_t samples;

    double accelSum[3]    = { 0.0f, 0.0f, 0.0f };
    double gyroSum[3]     = { 0.0f, 0.0f, 0.0f };
    double accelSumMXR[3] = { 0.0f, 0.0f, 0.0f };

    mpu6000Calibrating = true;

    for (samples = 0; samples < 5000; samples++)
    {
        readMPU6000();

        computeMPU6000TCBias();

        accelSum[XAXIS] += (float)rawAccel[XAXIS].value - accelTCBias[XAXIS];
        accelSum[YAXIS] += (float)rawAccel[YAXIS].value - accelTCBias[YAXIS];
        accelSum[ZAXIS] += (float)rawAccel[ZAXIS].value - accelTCBias[ZAXIS];

        gyroSum[ROLL ]  += (float)rawGyro[ROLL ].value  - gyroTCBias[ROLL ];
        gyroSum[PITCH]  += (float)rawGyro[PITCH].value  - gyroTCBias[PITCH];
        gyroSum[YAW  ]  += (float)rawGyro[YAW  ].value  - gyroTCBias[YAW  ];

        accelSumMXR[XAXIS] += mxr9150XAxis();
        accelSumMXR[YAXIS] += mxr9150YAxis();
        accelSumMXR[ZAXIS] += mxr9150ZAxis();

        delayMicroseconds(1000);
    }

    for (axis = 0; axis < 3; axis++)
    {
        accelSum[axis]   = accelSum[axis] / 5000.0f * ACCEL_SCALE_FACTOR;
        gyroRTBias[axis] = gyroSum[axis]  / 5000.0f;

        accelSumMXR[axis] = (accelSumMXR[axis] / 5000.0f - eepromConfig.accelBiasMXR[axis]) * eepromConfig.accelScaleFactorMXR[axis];
    }

    #if defined(MPU_ACCEL)
        accelOneG = sqrt(SQR(accelSum[XAXIS]) + SQR(accelSum[YAXIS]) + SQR(accelSum[ZAXIS]));
    #endif

    #if defined(MXR_ACCEL)
        accelOneG = sqrt(SQR(accelSumMXR[XAXIS]) + SQR(accelSumMXR[YAXIS]) + SQR(accelSumMXR[ZAXIS]));
    #endif

    mpu6000Calibrating = false;
}
示例#2
0
文件: mpu6000.c 项目: jihlein/FF32
void computeMPU6000RTData(void)
{
    uint8_t  axis;
    uint16_t samples;

    float accelSum[3] = { 0.0, 0.0, 0.0 };
    float gyroSum[3]  = { 0.0, 0.0, 0.0 };

    mpu6000Calibrating = true;

    for (samples = 0; samples < 5000; samples++)
    {
        readMPU6000();

        computeMPU6000TCBias();

        if (eepromConfig.useMXR9150 == true)
        {
        	accelSum[XAXIS] += (mxr9150XAxis() - eepromConfig.accelBiasMXR[XAXIS]) * eepromConfig.accelScaleFactorMXR[XAXIS];
   	        accelSum[YAXIS] += (mxr9150YAxis() - eepromConfig.accelBiasMXR[YAXIS]) * eepromConfig.accelScaleFactorMXR[YAXIS];
   	        accelSum[ZAXIS] += (mxr9150ZAxis() - eepromConfig.accelBiasMXR[ZAXIS]) * eepromConfig.accelScaleFactorMXR[ZAXIS];
        }
        else
        {
        	accelSum[XAXIS] += ((float)rawAccel[XAXIS].value - eepromConfig.accelBiasMPU[XAXIS] - accelTCBias[XAXIS]) * eepromConfig.accelScaleFactorMPU[XAXIS];
        	accelSum[YAXIS] += ((float)rawAccel[YAXIS].value - eepromConfig.accelBiasMPU[YAXIS] - accelTCBias[YAXIS]) * eepromConfig.accelScaleFactorMPU[YAXIS];
        	accelSum[ZAXIS] += ((float)rawAccel[ZAXIS].value - eepromConfig.accelBiasMPU[ZAXIS] - accelTCBias[ZAXIS]) * eepromConfig.accelScaleFactorMPU[ZAXIS];
        }

        gyroSum[ROLL ] += (float)rawGyro[ROLL ].value - gyroTCBias[ROLL ];
        gyroSum[PITCH] += (float)rawGyro[PITCH].value - gyroTCBias[PITCH];
        gyroSum[YAW  ] += (float)rawGyro[YAW  ].value - gyroTCBias[YAW  ];

        delayMicroseconds(1000);
    }

    for (axis = 0; axis < 3; axis++)
    {
        accelSum[axis]   = accelSum[axis] / 5000.0f;
        gyroRTBias[axis] = gyroSum[axis]  / 5000.0f;
    }

    accelOneG = (float)(sqrt(SQR(accelSum[XAXIS]) + SQR(accelSum[YAXIS]) + SQR(accelSum[ZAXIS])));

    mpu6000Calibrating = false;
}
示例#3
0
void accelCalibrationMXR(void)
{
    float noseUp        = 0.0f;
	float noseDown      = 0.0f;
	float leftWingDown  = 0.0f;
	float rightWingDown = 0.0f;
	float upSideDown    = 0.0f;
    float rightSideUp   = 0.0f;

    float xBias         = 0.0f;
    float yBias         = 0.0f;
    float zBias         = 0.0f;

    int16_t index;

    accelCalibrating = true;

    cliPrint("\nMXR9150 Accelerometer Calibration:\n\n");

    ///////////////////////////////////

    cliPrint("Place accelerometer right side up\n");
    cliPrint("  Send a character when ready to proceed\n\n");

    while (cliAvailable() == false);
    cliRead();

    cliPrint("  Gathering Data...\n\n");

    for (index = 0; index < 5000; index++)
    {
        rightSideUp += mxr9150ZAxis();

        xBias += mxr9150XAxis();
        yBias += mxr9150YAxis();

        delayMicroseconds(1000);
    }

    rightSideUp /= 5000.0f;

    cliPrint("Place accelerometer up side down\n");
    cliPrint("  Send a character when ready to proceed\n\n");

    while (cliAvailable() == false);
    cliRead();

    cliPrint("  Gathering Data...\n\n");

    for (index = 0; index < 5000; index++)
    {
        upSideDown += mxr9150ZAxis();

        xBias += mxr9150XAxis();
        yBias += mxr9150YAxis();

        delayMicroseconds(1000);
    }

    upSideDown /= 5000.0f;

    ///////////////////////////////////

    cliPrint("Place accelerometer left edge down\n");
    cliPrint("  Send a character when ready to proceed\n\n");

    while (cliAvailable() == false);
    cliRead();

    cliPrint("  Gathering Data...\n\n");

    for (index = 0; index < 5000; index++)
    {
        leftWingDown += mxr9150YAxis();

        zBias += mxr9150ZAxis();

        delayMicroseconds(1000);
    }

    leftWingDown /= 5000.0f;

    cliPrint("Place accelerometer right edge down\n");
    cliPrint("  Send a character when ready to proceed\n\n");

    while (cliAvailable() == false);
    cliRead();

    cliPrint("  Gathering Data...\n\n");

    for (index = 0; index < 5000; index++)
    {
        rightWingDown += mxr9150YAxis();

        zBias += mxr9150ZAxis();

        delayMicroseconds(1000);
    }

    rightWingDown /= 5000.0f;

    ///////////////////////////////////

    cliPrint("Place accelerometer rear edge down\n");
    cliPrint("  Send a character when ready to proceed\n\n");

    while (cliAvailable() == false);
    cliRead();

    cliPrint("  Gathering Data...\n\n");

    for (index = 0; index < 5000; index++)
    {
        noseUp += mxr9150XAxis();

        zBias += mxr9150ZAxis();

        delayMicroseconds(1000);
    }

    noseUp /= 5000.0f;

    cliPrint("Place accelerometer front edge down\n");
    cliPrint("  Send a character when ready to proceed\n\n");

    while (cliAvailable() == false);
    cliRead();

    cliPrint("  Gathering Data...\n\n");

    for (index = 0; index < 5000; index++)
    {
        noseDown += mxr9150XAxis();

        zBias += mxr9150ZAxis();

        delayMicroseconds(1000);
    }

    noseDown /= 5000.0f;

    ///////////////////////////////////

    eepromConfig.accelBiasMXR[ZAXIS]        = zBias / 20000.0f;
    eepromConfig.accelScaleFactorMXR[ZAXIS] = (2.0f * 9.8065f) / fabsf(rightSideUp - upSideDown);


    eepromConfig.accelBiasMXR[YAXIS]        = yBias / 10000.0f;
    eepromConfig.accelScaleFactorMXR[YAXIS] = (2.0f * 9.8065f) / fabsf(leftWingDown - rightWingDown);


    eepromConfig.accelBiasMXR[XAXIS]        = xBias / 10000.0f;
    eepromConfig.accelScaleFactorMXR[XAXIS] = (2.0f * 9.8065f) / fabsf(noseUp - noseDown);

    ///////////////////////////////////

	accelCalibrating = false;
}
示例#4
0
void SysTick_Handler(void)
{
    uint8_t index;
    uint32_t currentTime;
    float mxrTemp[3];

    sysTickCycleCounter = *DWT_CYCCNT;
    sysTickUptime++;

    watchDogsTick();

    if ((systemReady         == true)  &&
        (cliBusy             == false) &&
        (accelCalibrating    == false) &&
        (escCalibrating      == false) &&
        (magCalibrating      == false) &&
        (mpu6000Calibrating  == false))

    {
        #ifdef _DTIMING
//    	    LA2_ENABLE;
        #endif

        frameCounter++;
        if (frameCounter > FRAME_COUNT)
            frameCounter = 1;

        ///////////////////////////////

        currentTime = micros();
        deltaTime1000Hz = currentTime - previous1000HzTime;
        previous1000HzTime = currentTime;

        readMPU6000();

        accelSum500Hz[XAXIS] += rawAccel[XAXIS].value;
        accelSum500Hz[YAXIS] += rawAccel[YAXIS].value;
        accelSum500Hz[ZAXIS] += rawAccel[ZAXIS].value;

        accelSum100Hz[XAXIS] += rawAccel[XAXIS].value;
        accelSum100Hz[YAXIS] += rawAccel[YAXIS].value;
        accelSum100Hz[ZAXIS] += rawAccel[ZAXIS].value;

        gyroSum500Hz[ROLL ] += rawGyro[ROLL ].value;
        gyroSum500Hz[PITCH] += rawGyro[PITCH].value;
        gyroSum500Hz[YAW  ] += rawGyro[YAW  ].value;

        mxrTemp[XAXIS] = mxr9150XAxis();
        mxrTemp[YAXIS] = mxr9150YAxis();
        mxrTemp[ZAXIS] = mxr9150ZAxis();

        accelSum500HzMXR[XAXIS] += mxrTemp[XAXIS];
		accelSum500HzMXR[YAXIS] += mxrTemp[YAXIS];
		accelSum500HzMXR[ZAXIS] += mxrTemp[ZAXIS];

		accelSum100HzMXR[XAXIS] += mxrTemp[XAXIS];
		accelSum100HzMXR[YAXIS] += mxrTemp[YAXIS];
		accelSum100HzMXR[ZAXIS] += mxrTemp[ZAXIS];

        ///////////////////////////////

        if ((frameCounter % COUNT_500HZ) == 0)
        {
            frame_500Hz = true;

            for (index = 0; index < 3; index++)
            {
            	accelSummedSamples500Hz[index] = accelSum500Hz[index];
            	accelSum500Hz[index] = 0;

            	accelSummedSamples500HzMXR[index] = accelSum500HzMXR[index];
            	accelSum500HzMXR[index] = 0.0f;

            	gyroSummedSamples500Hz[index] = gyroSum500Hz[index];
                gyroSum500Hz[index] = 0;
            }
        }

        ///////////////////////////////

        if ((frameCounter % COUNT_100HZ) == 0)
        {
            frame_100Hz = true;

            for (index = 0; index < 3; index++)
            {
                accelSummedSamples100Hz[index] = accelSum100Hz[index];
                accelSum100Hz[index] = 0;

                accelSummedSamples100HzMXR[index] = accelSum100HzMXR[index];
                accelSum100HzMXR[index] = 0.0f;
            }

            if (!newTemperatureReading)
			{
				readTemperatureRequestPressure(MS5611_I2C);
			    newTemperatureReading = true;
			}
			else
			{
			    readPressureRequestTemperature(MS5611_I2C);
			    newPressureReading = true;
			}
        }

        ///////////////////////////////

        if ((frameCounter % COUNT_50HZ) == 0)
            frame_50Hz = true;

        ///////////////////////////////

        if (((frameCounter + 1) % COUNT_10HZ) == 0)
            newMagData = readMag(HMC5883L_I2C);

        if ((frameCounter % COUNT_10HZ) == 0)
            frame_10Hz = true;

        ///////////////////////////////

        if ((frameCounter % COUNT_5HZ) == 0)
            frame_5Hz = true;

        ///////////////////////////////

        if ((frameCounter % COUNT_1HZ) == 0)
            frame_1Hz = true;

        ///////////////////////////////////

        executionTime1000Hz = micros() - currentTime;

        ///////////////////////////////

        #ifdef _DTIMING
//            LA2_DISABLE;
        #endif
    }
}
示例#5
0
void accelCalibration(void)
{
    double noseUpMXR        = 0.0f;
	double noseDownMXR      = 0.0f;
	double leftWingDownMXR  = 0.0f;
	double rightWingDownMXR = 0.0f;
	double upSideDownMXR    = 0.0f;
    double rightSideUpMXR   = 0.0f;

    int16_t index;

    accelCalibrating = true;

    cliPrint("\nAccelerometer Calibration:\n\n");

    ///////////////////////////////////

    cliPrint("Place accelerometer right side up\n");
    cliPrint("  Send a character when ready to proceed\n\n");

    while (cliAvailable() == false);
    cliRead();

    cliPrint("  Gathering Data...\n\n");

    for (index = 0; index < 5000; index++)
    {
        rightSideUpMXR += mxr9150ZAxis();;
        delayMicroseconds(1000);
    }

    rightSideUpMXR /= 5000.0;

    cliPrint("Place accelerometer up side down\n");
    cliPrint("  Send a character when ready to proceed\n\n");

    while (cliAvailable() == false);
    cliRead();

    cliPrint("  Gathering Data...\n\n");

    for (index = 0; index < 5000; index++)
    {
        upSideDownMXR += mxr9150ZAxis();
        delayMicroseconds(1000);
    }

    upSideDownMXR /= 5000.0;

    eepromConfig.accelBiasMXR[ZAXIS] = (float)((rightSideUpMXR + upSideDownMXR) / 2.0);

    eepromConfig.accelScaleFactorMXR[ZAXIS] = (float)((2.0 * 9.8065) / (abs(rightSideUpMXR - eepromConfig.accelBiasMXR[ZAXIS]) +
    		                                                            abs(upSideDownMXR  - eepromConfig.accelBiasMXR[ZAXIS])));
    ///////////////////////////////////

    cliPrint("Place accelerometer left edge down\n");
    cliPrint("  Send a character when ready to proceed\n\n");

    while (cliAvailable() == false);
    cliRead();

    cliPrint("  Gathering Data...\n\n");

    for (index = 0; index < 5000; index++)
    {
        leftWingDownMXR += mxr9150YAxis();
        delayMicroseconds(1000);
    }

    leftWingDownMXR /= 5000.0;

    cliPrint("Place accelerometer right edge down\n");
    cliPrint("  Send a character when ready to proceed\n\n");

    while (cliAvailable() == false);
    cliRead();

    cliPrint("  Gathering Data...\n\n");

    for (index = 0; index < 5000; index++)
    {
        rightWingDownMXR += mxr9150YAxis();
        delayMicroseconds(1000);
    }

    rightWingDownMXR /= 5000.0;

    eepromConfig.accelBiasMXR[YAXIS] = (float)((leftWingDownMXR + rightWingDownMXR) / 2.0);

    eepromConfig.accelScaleFactorMXR[YAXIS] = (float)((2.0 * 9.8065) / (abs(leftWingDownMXR  - eepromConfig.accelBiasMXR[YAXIS]) +
    		                                                            abs(rightWingDownMXR - eepromConfig.accelBiasMXR[YAXIS])));
    ///////////////////////////////////

    cliPrint("Place accelerometer rear edge down\n");
    cliPrint("  Send a character when ready to proceed\n\n");

    while (cliAvailable() == false);
    cliRead();

    cliPrint("  Gathering Data...\n\n");

    for (index = 0; index < 5000; index++)
    {
        noseUpMXR += mxr9150XAxis();
        delayMicroseconds(1000);
    }

    noseUpMXR /= 5000.0;

    cliPrint("Place accelerometer front edge down\n");
    cliPrint("  Send a character when ready to proceed\n\n");

    while (cliAvailable() == false);
    cliRead();

    cliPrint("  Gathering Data...\n\n");

    for (index = 0; index < 5000; index++)
    {
        noseDownMXR += mxr9150XAxis();
        delayMicroseconds(1000);
    }

    noseDownMXR /= 5000.0;

    eepromConfig.accelBiasMXR[XAXIS] = (float)((noseUpMXR + noseDownMXR) / 2.0);

    eepromConfig.accelScaleFactorMXR[XAXIS] = (float)((2.0 * 9.8065) / (abs(noseUpMXR   - eepromConfig.accelBiasMXR[XAXIS]) +
    		                                                            abs(noseDownMXR - eepromConfig.accelBiasMXR[XAXIS])));
    ///////////////////////////////////

    accelCalibrating = false;
}