/**************************************************************************//**
*
* @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());
}
}
}
