static void hardware_init(void) {
// Setup UART clocks
setup_pinmux();
spi_init();
spi_setup_device(SPI_DEVICE_1, SSP_BITS_8, SSP_FRAMEFORMAT_SPI, SSP_CLOCK_MODE0, true);
SDCardInit();
i2c_onboard_init();
}
/*
* @brief Initialized the disk drive (the SD Card).
*
* @param drive The drive which is to be initialized (Must be 0).
* @returns DSTATUS The status of the drive as flags.
*/
DSTATUS disk_initialize (BYTE drive){
/* There is only one drive */
if(drive != 0){
return (STA_NODISK | STA_NOINIT);
}
uint8 status = SDCardInit();
if(status != SD_CARD_STATUS_OK){
return STA_NOINIT;
}
//Initialization Succeed
return STA_OK;
}
int main(void) {
#ifdef NEED_EVENT
uint32_t DVSEventPointer;
uint32_t DVSEventTimeLow;
uint16_t DVSEvent;
uint32_t timeStampMemory = 0, timeStampDelta = 0;
#endif
int16_t angle = 0;
uint32_t cnt =0;
ExtraPinsInit();
disablePeripherals();
Chip_RIT_Init(LPC_RITIMER);
RTC_TIME_T build = { .time = { BUILD_SEC_INT, BUILD_MIN_INT, BUILD_HOUR_INT, BUILD_DAY_INT, 0, 1, BUILD_MONTH_INT,
BUILD_YEAR_INT } };
buildTime = build;
//This should be one of the first initializations routines to run.
sensorsInit();
#ifdef NEED_EVENT
DVS128ChipInit();
#endif
DacInit();
UARTInit(LPC_UART, BAUD_RATE_DEFAULT); /* baud rate setting */
initMotors();
PWMInit();
#if USE_IMU_DATA
timerDelayMs(100);
MPU9105Init();
#endif
#if USE_SDCARD
SDCardInit();
#endif
#if USE_PUSHBOT
MiniRobInit();
#endif
#ifdef TEST_RUN
test();
//This will not return
#endif
LED1SetOn();
// Start M0APP slave processor
cr_start_m0(&__core_m0app_START__);
LED1SetOff();
LED0SetOn();
LED0SetBlinking(ENABLE);
UARTShowVersion();
for (;;) {
if (ledBlinking && toggleLed0) {
LED0Toggle();
toggleLed0 = 0;
}
// *****************************************************************************
// UARTIterate();
// *****************************************************************************
while (bytesReceived(&uart)) { // incoming char available?
UART0ParseNewChar(popByteFromReceptionBuffer(&uart));
}
// *****************************************************************************
// Deal with audio data
// *****************************************************************************
/*
* do the fft cross correlation and figure out the angle
* manipulate the proceeding direction of the pushbot
*/
// if buffer reaches a length of 1024
/* if(process_flag != -1){ // -1 for not ready, 0 for buffer0, 1 for buffer1
// SysTick->CTRL &= ~0x1;
//xprintf("%d\n", process_flag);
angle = itd();
//SysTick->CTRL |= 0x1;
if(++cnt>150){
xprintf("angle = %d\n", angle);
cnt = 0;
}
}*/
// start doing math
#if USE_IMU_DATA
updateIMUData();
#endif
#if USE_PUSHBOT
refreshMiniRobSensors();
if (motor0.updateRequired) {
motor0.updateRequired = 0;
updateMotorController(MOTOR0);
}
if (motor1.updateRequired) {
motor1.updateRequired = 0;
updateMotorController(MOTOR1);
}
#endif
/*
// disable the data streaming through serial
if (sensorRefreshRequested) {
sensorRefreshRequested = 0;
for (int i = 0; i < sensorsEnabledCounter; ++i) {
if (enabledSensors[i]->triggered) {
enabledSensors[i]->refresh();
enabledSensors[i]->triggered = 0;
}
}
}
*/
#ifdef NEED_EVENT
// *****************************************************************************
// processEventsIterate();
// *****************************************************************************
if (events.eventBufferWritePointer == events.eventBufferReadPointer) { // more events in buffer to process?
continue;
}
if (eDVSProcessingMode < EDVS_PROCESS_EVENTS) { //Not processing events
if (freeSpaceForTranmission(&uart) < (TX_BUFFER_SIZE - 32) || !(eDVSProcessingMode & EDVS_STREAM_EVENTS)) {
#if LOW_POWER_MODE
uart.txSleepingFlag = 1;
__WFE();
#endif
continue; //Wait until the buffer is empty.
}
}
/*We are either processing events or streaming them.
* If streaming the buffer must be empty at this point
*/
events.ringBufferLock = true;
events.eventBufferReadPointer = ((events.eventBufferReadPointer + 1) & DVS_EVENTBUFFER_MASK); // increase read pointer
DVSEventPointer = events.eventBufferReadPointer; // cache the value to be faster
DVSEvent = events.eventBufferA[DVSEventPointer]; // fetch event from buffer
DVSEventTimeLow = events.eventBufferTimeLow[DVSEventPointer]; // fetch event from buffer
events.ringBufferLock = false;
if (eDVSProcessingMode & EDVS_STREAM_EVENTS) {
if (freeSpaceForTranmission(&uart) > 6) { // wait for TX to finish sending!
pushByteToTransmission(&uart, (DVSEvent >> 8) | 0x80); // 1st byte to send (Y-address)
pushByteToTransmission(&uart, DVSEvent & 0xFF); // 2nd byte to send (X-address)
if (eDVSDataFormat == EDVS_DATA_FORMAT_BIN_TSVB) {
// Calculate delta...
timeStampDelta = DVSEventTimeLow - timeStampMemory;
timeStampMemory = DVSEventTimeLow; // Save the current TS in delta
// check how many bytes we need to send
if (timeStampDelta < 0x7F) {
// Only 7 TS bits need to be sent
pushByteToTransmission(&uart, (timeStampDelta & 0x7F) | 0x80); // 3rd byte to send (7bit Delta TS, MSBit set to 1)
} else if (timeStampDelta < 0x3FFF) {
// Only 14 TS bits need to be sent
pushByteToTransmission(&uart, (timeStampDelta >> 7) & 0x7F); // 3rd byte to send (upper 7bit Delta TS, MSBit set to 0)
pushByteToTransmission(&uart, (timeStampDelta & 0x7F) | 0x80); // 4th byte to send (lower 7bit Delta TS, MSBit set to 1)
} else if (timeStampDelta < 0x1FFFFF) {
// Only 21 TS bits need to be sent
pushByteToTransmission(&uart, (timeStampDelta >> 14) & 0x7F); // 3rd byte to send (upper 7bit Delta TS, MSBit set to 0)
pushByteToTransmission(&uart, (timeStampDelta >> 7) & 0x7F); // 4th byte to send (middle 7bit Delta TS, MSBit set to 0)
pushByteToTransmission(&uart, (timeStampDelta & 0x7F) | 0x80); // 5th byte to send (lower 7bit Delta TS, MSBit set to 1)
} else {
//------------------------------------------------------------------------------
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());
}
}
}