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