void SensorCalibration() { char InitState = GetSensorInitState(); char calibration_sensor = GetChar(); #ifdef OPTION_RC TIMER_Enable(false); RC_Enable(false); #endif if((calibration_sensor=='a')&&(InitState&SENSOR_ACC)) { // Do 'a'cc calibration AccCalibration(); } else if((calibration_sensor=='g')&&(InitState&SENSOR_GYRO)) {// Do 'g'yro calibration GyroCalibration(); } else if((calibration_sensor=='m')&&(InitState&SENSOR_MAG)) { // Do 'g'yro calibration MagCalibration(); if(CalQFactor<MAG_CAL_SUCESS_TH) UpdateFlashCal(SENSOR_MAG, false); SensorInitMAG(); } else // Fail doing calibration CalibrationFail(); #ifdef OPTION_RC TIMER_Enable(true); RC_Enable(true); #endif }
void SensorCalibration() { char InitState = GetSensorInitState(); char calibration_sensor = GetChar(); TIMER_Enable(false); if((calibration_sensor=='a')&&(InitState&SENSOR_ACC)) // Do 'a'cc calibration { AccCalibration(); } else if((calibration_sensor=='g')&&(InitState&SENSOR_GYRO)) // Do 'g'yro calibration { GyroCalibration(); } else if((calibration_sensor=='m')&&(InitState&SENSOR_MAG)) // Do 'g'yro calibration { MagCalibration(); UpdateFlashCal(SENSOR_MAG, false); } else // Fail doing calibration CalibrationFail(); TIMER_Enable(true); }
//------------------------------------------------------------------------------ int main(void) { VICConfig(); TimingInit(); LEDInit(); UART1Init(); UART2Init(); I2CInit(); SPISlaveInit(); Wait(100); UART1Printf("University of Tokyo NaviCtrl firmware V2"); ReadEEPROM(); UBloxInit(); LSM303DLInit(); FlightCtrlCommsInit(); SDCardInit(); NavigationInit(); ExternalButtonInit(); // Enable the "new data" interrupt. VIC_Config(EXTIT0_ITLine, VIC_IRQ, IRQ_PRIORITY_NEW_DATA); VIC_ITCmd(EXTIT0_ITLine, ENABLE); // Enable the 50Hz Interrupt. VIC_Config(EXTIT3_ITLine, VIC_IRQ, IRQ_PRIORITY_50HZ); VIC_ITCmd(EXTIT3_ITLine, ENABLE); // Main loop. uint32_t led_timer = GetTimestamp(); for (;;) { #ifndef VISION // Check for new data from the magnetometer. ProcessIncomingLSM303DL(); // Skip the rest of the main loop if mag calibration is ongoing. if (MagCalibration(mag_calibration_)) continue; // Check for new data on the GPS UART port. ProcessIncomingUBlox(); #endif // Check for new data from the FlightCtrl. if (NewDataFromFlightCtrl()) { ClearNewDataFromFlightCtrlFlag(); #ifdef VISION KalmanAccelerometerUpdate(); #endif UpdateNavigation(); PrepareFlightCtrlDataExchange(); #ifndef VISION RequestLSM303DL(); #endif // Check if new data has come while processing the data. This indicates // that processing did not complete fast enough. if (NewDataFromFlightCtrl()) { overrun_counter_++; } } #ifndef VISION CheckUBXFreshness(); CheckLSM303DLFreshness(); // Normally the magnetometer is read every time new data comes from the // FlightCtrl. The following statement is a backup that ensures the // magnetometer is updated even if there is no connection to the FlightCtrl // and also deals with read errors. if (LSM303DLDataStale()) { if (MillisSinceTimestamp(LSM303DLLastRequestTimestamp()) > 20) RequestLSM303DL(); if (LSM303DLErrorBits() & LSM303DL_ERROR_BIT_I2C_BUSY) I2CReset(); } #else CheckVisionFreshness(); #endif // Check for incoming data on the "update & debug" UART port. ProcessIncomingUART1(); // Check for incoming data on the "FligthCtrl" UART port. ProcessIncomingUART2(); ProcessLogging(); if (TimestampInPast(led_timer)) { GreenLEDToggle(); while (TimestampInPast(led_timer)) led_timer += 100; // Debug output for GPS and magnetomter. Remove after testing is completed // UART1Printf("%+5.2f,%+5.2f,%+5.2f", // MagneticVector()[0], // MagneticVector()[1], // MagneticVector()[2]); // UART1Printf("%i,%i,%i", // MagnetometerVector()[0], // MagnetometerVector()[1], // MagnetometerVector()[2]); // UART1Printf("%i,%i,%i", // MagnetometerBiasVector()[0], // MagnetometerBiasVector()[1], // MagnetometerBiasVector()[2]); // UART1Printf("%f", CurrentHeading()); // UART1Printf("%f,%f,%f", // (float)(UBXPosLLH()->longitude * 1e-7), // (float)(UBXPosLLH()->latitude * 1e-7), // (float)(UBXPosLLH()->height_above_ellipsoid * 1e-3)); UART1PrintfSafe("%+5.2f,%+5.2f,%+5.2f,%+5.2f", PositionVector()[0], PositionVector()[1], PositionVector()[2], CurrentHeading()); } } }