void UserInit(void){ StartCritical(); //println_W(startmessage);// All printfDEBUG functions do not need to be removed from code if debug is disabled //#if defined(DEBUG) // ConfigureUART(115200); // if(GetChannelMode(16)!=IS_UART_TX) // setMode(16,IS_UART_TX); //#endif setPrintLevelInfoPrint(); println_I(/*PSTR*/("\e[1;1H\e[2J ***Starting User initialization***")); InitFlagPins(); InitBankLEDs(); SetPowerState0(0,0); SetPowerState1(0,0); //_delay_ms(1); #if defined(WPIRBE) SPISlaveInit(); #endif //_delay_ms(100); println_I(/*PSTR*/("Starting Pin Functions")); InitPinFunction(); println_I(/*PSTR*/("Starting Pin Modes")); InitPinModes(); int i=0; //println_I(/*PSTR*/("Starting hardware modes")); for(i=0;i<GetNumberOfIOChannels();i++){ initPinState(i); configAdvancedAsyncNotEqual(i,10); setAsyncLocal(i,true) ; } //println_I(/*PSTR*/("DONE pin initialization")); //println_I(/*PSTR*/("Adding IO Initialization")); addNamespaceToList((NAMESPACE_LIST *)get_bcsIoNamespace()); //println_I(/*PSTR*/("Adding IO.SETMODE Initialization")); addNamespaceToList((NAMESPACE_LIST *)get_bcsIoSetmodeNamespace()); //println_I(/*PSTR*/("Adding Internal Initialization")); addNamespaceToList((NAMESPACE_LIST *)get_internalNamespace()); setNoAsyncMode(true); setIgnoreAddressing(true); //SetPinTris(0,OUTPUT); //SetDIO(0,OFF); #if defined(USE_AS_LIBRARY) InitializeUserCode(); #endif EndCritical(); println_I(/*PSTR*/("Starting Core")); // setMode(22,IS_DO); // setMode(23,IS_DI); // println_I(/*PSTR*/("Pin done")); }
//------------------------------------------------------------------------------ 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()); } } }