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;
}
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;
}
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;
}
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
}
}
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;
}
