void SysTick_Handler(void)
{
uint8_t index;
uint32_t currentTime;
sysTickCycleCounter = *DWT_CYCCNT;
sysTickUptime++;
#if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
if ((systemReady == true) &&
(accelCalibrating == false) &&
(gyroCalibrating == false) &&
(magCalibrating == false))
#endif
#if defined(USE_CHR6DM_AHRS)
if ((systemReady == true) &&
(accelCalibrating == false) &&
(gyroCalibrating == false) &&
(magCalibrating == false) &&
(ahrsCalibrating == false))
#endif
{
frameCounter++;
if (frameCounter > FRAME_COUNT)
frameCounter = 1;
///////////////////////////////
currentTime = micros();
deltaTime1000Hz = currentTime - previous1000HzTime;
previous1000HzTime = currentTime;
readAccel();
#if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
accelSum200Hz[XAXIS] += rawAccel[XAXIS].value;
accelSum200Hz[YAXIS] += rawAccel[YAXIS].value;
accelSum200Hz[ZAXIS] += rawAccel[ZAXIS].value;
#endif
accelSum100Hz[XAXIS] += rawAccel[XAXIS].value;
accelSum100Hz[YAXIS] += rawAccel[YAXIS].value;
accelSum100Hz[ZAXIS] += rawAccel[ZAXIS].value;
readGyro();
gyroSum200Hz[ROLL] += rawGyro[ROLL].value;
gyroSum200Hz[PITCH] += rawGyro[PITCH].value;
gyroSum200Hz[YAW] += rawGyro[YAW].value;
gyroSum100Hz[ROLL] += rawGyro[ROLL].value;
gyroSum100Hz[PITCH] += rawGyro[PITCH].value;
gyroSum100Hz[YAW] += rawGyro[YAW].value;
gyroSum500Hz[ROLL] += rawGyro[ROLL].value;
gyroSum500Hz[PITCH] += rawGyro[PITCH].value;
gyroSum500Hz[YAW] += rawGyro[YAW].value;
///////////////////////////////
if ((frameCounter % COUNT_500HZ) == 0) {
frame_500Hz = true;
for (index = 0; index < 3; index++) {
gyroSummedSamples500Hz[index] = gyroSum500Hz[index];
gyroSum500Hz[index] = 0.0f;
}
}
///////////////////////////////
if ((frameCounter % COUNT_200HZ) == 0) {
frame_200Hz = true;
for (index = 0; index < 3; index++) {
#if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
accelSummedSamples200Hz[index] = accelSum200Hz[index];
accelSum200Hz[index] = 0.0f;
#endif
gyroSummedSamples200Hz[index] = gyroSum200Hz[index];
gyroSum200Hz[index] = 0.0f;
}
}
///////////////////////////////
if ((frameCounter % COUNT_100HZ) == 0) {
frame_100Hz = true;
for (index = 0; index < 3; index++) {
accelSummedSamples100Hz[index] = accelSum100Hz[index];
accelSum100Hz[index] = 0.0f;
gyroSummedSamples100Hz[index] = gyroSum100Hz[index];
gyroSum100Hz[index] = 0.0f;
}
if (frameCounter == COUNT_100HZ) {
readTemperatureRequestPressure();
} else if (frameCounter == FRAME_COUNT) {
readPressureRequestTemperature();
} else {
readPressureRequestPressure();
}
pressureSum += uncompensatedPressure.value;
}
///////////////////////////////
if (((frameCounter + 1) % COUNT_50HZ) == 0) {
readMag();
magSum[XAXIS] += rawMag[XAXIS].value;
magSum[YAXIS] += rawMag[YAXIS].value;
magSum[ZAXIS] += rawMag[ZAXIS].value;
}
if ((frameCounter % COUNT_50HZ) == 0) {
frame_50Hz = true;
}
///////////////////////////////
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;
///////////////////////////////
}
}
void SysTick_Handler(void)
{
uint8_t index;
uint32_t currentTime;
sysTickCycleCounter = *DWT_CYCCNT;
sysTickUptime++;
if ((systemReady == true ) &&
(cliBusy == false) &&
(accelCalibrating == false) &&
(escCalibrating == false) &&
(gyroCalibrating == false) &&
(magCalibrating == false))
{
frameCounter++;
if (frameCounter > FRAME_COUNT)
frameCounter = 1;
///////////////////////////////
currentTime = micros();
deltaTime1000Hz = currentTime - previous1000HzTime;
previous1000HzTime = currentTime;
readAccel();
accelSum200Hz[XAXIS] += rawAccel[XAXIS].value;
accelSum200Hz[YAXIS] += rawAccel[YAXIS].value;
accelSum200Hz[ZAXIS] += rawAccel[ZAXIS].value;
accelSum100Hz[XAXIS] += rawAccel[XAXIS].value;
accelSum100Hz[YAXIS] += rawAccel[YAXIS].value;
accelSum100Hz[ZAXIS] += rawAccel[ZAXIS].value;
readGyro();
gyroSum200Hz[ROLL ] += rawGyro[ROLL ].value;
gyroSum200Hz[PITCH] += rawGyro[PITCH].value;
gyroSum200Hz[YAW ] += rawGyro[YAW ].value;
///////////////////////////////
if ((frameCounter % COUNT_200HZ) == 0)
{
frame_200Hz = true;
for (index = 0; index < 3; index++)
{
accelSummedSamples200Hz[index] = accelSum200Hz[index];
accelSum200Hz[index] = 0.0f;
gyroSummedSamples200Hz[index] = gyroSum200Hz[index];
gyroSum200Hz[index] = 0.0f;
}
}
///////////////////////////////
if ((frameCounter % COUNT_100HZ) == 0)
{
frame_100Hz = true;
for (index = 0; index < 3; index++)
{
accelSummedSamples100Hz[index] = accelSum100Hz[index];
accelSum100Hz[index] = 0.0f;
}
if (frameCounter == COUNT_100HZ)
readTemperatureRequestPressure();
else if (frameCounter == FRAME_COUNT)
readPressureRequestTemperature();
else
readPressureRequestPressure();
pressureSum += uncompensatedPressure.value;
}
///////////////////////////////
if ((frameCounter % COUNT_50HZ) == 0)
{
frame_50Hz = true;
}
///////////////////////////////
if (((frameCounter + 1) % COUNT_10HZ) == 0)
newMagData = readMag();
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;
///////////////////////////////
}
}
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
}
}
