/**************************************************************************//** * * @brief Initialize board * *****************************************************************************/ void BoardInit(void) { GPIO_InitTypeDef GPIO_InitStructure; USART_InitTypeDef USART_InitStructure; Pin_Init(); LED_CLK(); BSP_LedSet(0); GPIO_InitStructure.GPIO_Pin = LED_PIN; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; GPIO_Init(LED_PORT, &GPIO_InitStructure); SystemCoreClockUpdate(); TimingInit(); PC_GPIO_CLK(); GPIO_InitStructure.GPIO_Pin = PC_PIN_RX; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; GPIO_Init(PC_PIN_PORT, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = PC_PIN_TX; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; GPIO_Init(PC_PIN_PORT, &GPIO_InitStructure); PC_UART_CLK(); USART_InitStructure.USART_BaudRate = 115200; USART_InitStructure.USART_WordLength = USART_WordLength_8b; USART_InitStructure.USART_StopBits = USART_StopBits_1; USART_InitStructure.USART_Parity = USART_Parity_No; USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; USART_Init(PC_UART, &USART_InitStructure); USART_Cmd(PC_UART, ENABLE); }
//------------------------------------------------------------------------------ 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()); } } }