rtems_task Init(
rtems_task_argument ignored
){
#ifndef SERIO_TESTING
Fbw(init);
Ap(init);
while (1) {
update_bat(12.0);
Fbw(handle_periodic_tasks);
Ap(handle_periodic_tasks);
Fbw(event_task);
Ap(event_task);
}
#else
UART1Init();
while(1){
Ap(event_task);
}
#endif
return ;
}
void UBloxInit(void)
{
SCU_APBPeriphClockConfig(__GPIO6, ENABLE); // Enable the GPIO6 Clock
SCU_APBPeriphClockConfig(__UART0, ENABLE); // Enable the UART0 Clock
GPIO_InitTypeDef gpio_init;
// Configure pin GPIO6.6 to be UART0 Rx
gpio_init.GPIO_Direction = GPIO_PinInput;
gpio_init.GPIO_Pin = GPIO_Pin_6;
gpio_init.GPIO_Type = GPIO_Type_PushPull;
gpio_init.GPIO_IPInputConnected = GPIO_IPInputConnected_Enable;
gpio_init.GPIO_Alternate = GPIO_InputAlt1; // UART0 Rx
GPIO_Init(GPIO6, &gpio_init);
// Configure pin GPIO6.6 to be UART0 Tx
gpio_init.GPIO_Direction = GPIO_PinOutput;
gpio_init.GPIO_Pin = GPIO_Pin_7;
gpio_init.GPIO_Type = GPIO_Type_PushPull;
gpio_init.GPIO_IPInputConnected = GPIO_IPInputConnected_Disable;
gpio_init.GPIO_Alternate = GPIO_OutputAlt3; // UART0 Tx
GPIO_Init(GPIO6, &gpio_init);
UART1Init(UBLOX_INITIAL_BAUD);
{
// Set the port to UART UBX @ 57600.
const uint8_t tx_buffer[28] = { 0xb5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01,
0x00, 0x00, 0x00, 0xd0, 0x08, 0x00, 0x00, 0x00, 0xe1, 0x00, 0x00, 0x01,
0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd6, 0x8d };
UBloxTxBuffer(tx_buffer, 28);
}
Wait(150);
UART1Init(UBLOX_OPERATING_BAUD);
// Enable UART Rx interrupt.
UART_ITConfig(UART0, UART_IT_Receive, ENABLE);
VIC_Config(UART0_ITLine, VIC_IRQ, IRQ_PRIORITY_UART0);
VIC_ITCmd(UART0_ITLine, ENABLE);
{ // Configure USB for UBX input with no output.
const uint8_t tx_buffer[28] = { 0xb5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x03,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1e, 0x8c };
UBloxTxBuffer(tx_buffer, 28);
}
{ // Set antenna flags to 0x0b and pins to 0x380f.
const uint8_t tx_buffer[12] = { 0xb5, 0x62, 0x06, 0x13, 0x04, 0x00, 0x0b,
0x00, 0x0f, 0x38, 0x6f, 0x4f };
UBloxTxBuffer(tx_buffer, 12);
}
{ // Set measurement period to 200ms (5Hz) with UTC reference.
const uint8_t tx_buffer[14] = { 0xb5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xc8,
0x00, 0x01, 0x00, 0x00, 0x00, 0xdd, 0x68 };
UBloxTxBuffer(tx_buffer, 14);
}
{ // Configure TimPulse.
const uint8_t tx_buffer[28] = { 0xb5, 0x62, 0x06, 0x07, 0x14, 0x00, 0x40,
0x42, 0x0f, 0x00, 0x90, 0x86, 0x03, 0x00, 0xff, 0x01, 0x00, 0x00, 0x32,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0x70 };
UBloxTxBuffer(tx_buffer, 28);
}
{ // Configure SBAS.
const uint8_t tx_buffer[16] = { 0xb5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03,
0x03, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x2b, 0xbd };
UBloxTxBuffer(tx_buffer, 16);
}
{ // Configure navigation engine.
const uint8_t tx_buffer[44] = { 0xb5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xff,
0xff, 0x06, 0x02, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x08,
0x3c, 0x50, 0x00, 0x32, 0x00, 0x23, 0x00, 0x23, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x97,
0xfa };
UBloxTxBuffer(tx_buffer, 44);
}
{ // Configure navigation engine expert settings.
const uint8_t tx_buffer[48] = { 0xb5, 0x62, 0x06, 0x23, 0x28, 0x00, 0x00,
0x00, 0x4c, 0x06, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x10, 0x14,
0x00, 0x01, 0x00, 0x00, 0x00, 0xf8, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0xc9, 0xea };
UBloxTxBuffer(tx_buffer, 48);
}
{ // Request NAV-POSLLH message to be output every measurement cycle.
const uint8_t tx_buffer[11] = { 0xb5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01,
0x02, 0x01, 0x0e, 0x47 };
UBloxTxBuffer(tx_buffer, 11);
}
{ // Request NAV-VELNED message to be output every measurement cycle.
const uint8_t tx_buffer[11] = { 0xb5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01,
0x12, 0x01, 0x1e, 0x67 };
UBloxTxBuffer(tx_buffer, 11);
}
{ // Request NAV-SOL message to be output every measurement cycle.
const uint8_t tx_buffer[11] = { 0xb5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01,
0x06, 0x01, 0x12, 0x4f };
UBloxTxBuffer(tx_buffer, 11);
}
{ // Request Time-UTC message to be output every 5 measurement cycles.
const uint8_t tx_buffer[11] = { 0xb5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01,
0x21, 0x05, 0x31, 0x89 };
UBloxTxBuffer(tx_buffer, 11);
}
}
//------------------------------------------------------------------------------
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());
}
}
}