void imu6Read(Axis3f* gyroOut, Axis3f* accOut) { mpu6500GetMotion6(&accelMpu.y, &accelMpu.x, &accelMpu.z, &gyroMpu.y, &gyroMpu.x, &gyroMpu.z); imuAddBiasValue(&gyroBias, &gyroMpu); #ifdef IMU_TAKE_ACCEL_BIAS if (!accelBias.isBiasValueFound) { imuAddBiasValue(&accelBias, &accelMpu); } #endif if (!gyroBias.isBiasValueFound) { imuFindBiasValue(&gyroBias); if (gyroBias.isBiasValueFound) { soundSetEffect(SND_CALIB); ledseqRun(SYS_LED, seq_calibrated); } } #ifdef IMU_TAKE_ACCEL_BIAS if (gyroBias.isBiasValueFound && !accelBias.isBiasValueFound) { Axis3i32 mean; imuCalculateBiasMean(&accelBias, &mean); accelBias.bias.x = mean.x; accelBias.bias.y = mean.y; accelBias.bias.z = mean.z - IMU_1G_RAW; accelBias.isBiasValueFound = true; } #endif imuAccIIRLPFilter(&accelMpu, &accelLPF, &accelStoredFilterValues, (int32_t)imuAccLpfAttFactor); imuAccAlignToGravity(&accelLPF, &accelLPFAligned); // Re-map outputs gyroOut->x = -(gyroMpu.x - gyroBias.bias.x) * IMU_DEG_PER_LSB_CFG; gyroOut->y = (gyroMpu.y - gyroBias.bias.y) * IMU_DEG_PER_LSB_CFG; gyroOut->z = (gyroMpu.z - gyroBias.bias.z) * IMU_DEG_PER_LSB_CFG; #ifdef IMU_TAKE_ACCEL_BIAS accOut->x = (accelLPFAligned.x - accelBias.bias.x) * IMU_G_PER_LSB_CFG; accOut->y = (accelLPFAligned.y - accelBias.bias.y) * IMU_G_PER_LSB_CFG; accOut->z = (accelLPFAligned.z - accelBias.bias.z) * IMU_G_PER_LSB_CFG; #else accOut->x = -(accelLPFAligned.x) * IMU_G_PER_LSB_CFG; accOut->y = (accelLPFAligned.y) * IMU_G_PER_LSB_CFG; accOut->z = (accelLPFAligned.z) * IMU_G_PER_LSB_CFG; #endif }
/** * Calculates the bias first when the gyro variance is below threshold. Requires a buffer * but calibrates platform first when it is stable. */ static bool processGyroBias(int16_t gx, int16_t gy, int16_t gz, Axis3f *gyroBiasOut) { sensorsAddBiasValue(&gyroBiasRunning, gx, gy, gz); if (!gyroBiasRunning.isBiasValueFound) { sensorsFindBiasValue(&gyroBiasRunning); if (gyroBiasRunning.isBiasValueFound) { soundSetEffect(SND_CALIB); ledseqRun(SYS_LED, seq_calibrated); } } gyroBiasOut->x = gyroBiasRunning.bias.x; gyroBiasOut->y = gyroBiasRunning.bias.y; gyroBiasOut->z = gyroBiasRunning.bias.z; return gyroBiasRunning.isBiasValueFound; }
void systemTask(void *arg) { bool pass = true; ledInit(); ledSet(CHG_LED, 1); #ifdef DEBUG_QUEUE_MONITOR queueMonitorInit(); #endif #ifdef ENABLE_UART1 uart1Init(); #endif #ifdef ENABLE_UART2 uart2Init(); #endif //Init the high-levels modules systemInit(); commInit(); commanderInit(); StateEstimatorType estimator = anyEstimator; deckInit(); estimator = deckGetRequiredEstimator(); stabilizerInit(estimator); if (deckGetRequiredLowInterferenceRadioMode()) { platformSetLowInterferenceRadioMode(); } soundInit(); memInit(); #ifdef PROXIMITY_ENABLED proximityInit(); #endif //Test the modules pass &= systemTest(); pass &= configblockTest(); pass &= commTest(); pass &= commanderTest(); pass &= stabilizerTest(); pass &= deckTest(); pass &= soundTest(); pass &= memTest(); pass &= watchdogNormalStartTest(); //Start the firmware if(pass) { selftestPassed = 1; systemStart(); soundSetEffect(SND_STARTUP); ledseqRun(SYS_LED, seq_alive); ledseqRun(LINK_LED, seq_testPassed); } else { selftestPassed = 0; if (systemTest()) { while(1) { ledseqRun(SYS_LED, seq_testPassed); //Red passed == not passed! vTaskDelay(M2T(2000)); // System can be forced to start by setting the param to 1 from the cfclient if (selftestPassed) { DEBUG_PRINT("Start forced.\n"); systemStart(); break; } } } else { ledInit(); ledSet(SYS_LED, true); } } DEBUG_PRINT("Free heap: %d bytes\n", xPortGetFreeHeapSize()); workerLoop(); //Should never reach this point! while(1) vTaskDelay(portMAX_DELAY); }
void systemTask(void *arg) { bool pass = true; ledInit(); ledSet(CHG_LED, 1); #ifdef DEBUG_QUEUE_MONITOR queueMonitorInit(); #endif uartInit(); #ifdef ENABLE_UART1 uart1Init(); #endif #ifdef ENABLE_UART2 uart2Init(); #endif //Init the high-levels modules systemInit(); #ifndef USE_RADIOLINK_CRTP #ifdef UART_OUTPUT_TRACE_DATA //debugInitTrace(); #endif #ifdef ENABLE_UART // uartInit(); #endif #endif //ndef USE_RADIOLINK_CRTP commInit(); commanderAdvancedInit(); stabilizerInit(); #ifdef PLATFORM_CF2 deckInit(); #endif soundInit(); memInit(); #ifdef PROXIMITY_ENABLED proximityInit(); #endif //Test the modules pass &= systemTest(); pass &= configblockTest(); pass &= commTest(); pass &= commanderAdvancedTest(); pass &= stabilizerTest(); #ifdef PLATFORM_CF2 pass &= deckTest(); #endif pass &= soundTest(); pass &= memTest(); pass &= watchdogNormalStartTest(); //Start the firmware if(pass) { selftestPassed = 1; systemStart(); soundSetEffect(SND_STARTUP); ledseqRun(SYS_LED, seq_alive); ledseqRun(LINK_LED, seq_testPassed); } else { selftestPassed = 0; if (systemTest()) { while(1) { ledseqRun(SYS_LED, seq_testPassed); //Red passed == not passed! vTaskDelay(M2T(2000)); // System can be forced to start by setting the param to 1 from the cfclient if (selftestPassed) { DEBUG_PRINT("Start forced.\n"); systemStart(); break; } } } else { ledInit(); ledSet(SYS_LED, true); } } DEBUG_PRINT("Free heap: %d bytes\n", xPortGetFreeHeapSize()); workerLoop(); //Should never reach this point! while(1) vTaskDelay(portMAX_DELAY); }