void pixyInit(void)
#endif
{
// write stack guard word
// STACK_GUARD = STACK_GUARD_WORD;
commonInit();
#ifdef KEIL
IPC_haltSlave();
#endif
// clear RC servo registers to prevent and glitches upon initialization
rcs_enable(0, 0);
rcs_enable(1, 0);
ADCInit();
SCTInit();
CameraInit();
// initialize shared memory interface before running M0
SMLink *smLink = new SMLink;
// start slave
#ifdef KEIL
if (slaveRomStart && slaveImage && imageSize)
{
IPC_downloadSlaveImage(slaveRomStart, slaveImage, imageSize);
IPC_startSlave();
}
#else
cr_start_m0(SLAVE_M0APP,&__core_m0app_START__);
#endif
// initialize chirp objects
USBLink *usbLink = new USBLink;
g_chirpUsb = new Chirp(false, false, usbLink);
g_chirpUsb->setSendTimeout(3000); // set a high timeout because the host can sometimes go AWOL for a second or two....
g_chirpUsb->setRecvTimeout(3000); // set a high timeout because the host can sometimes go AWOL for a second or two....
g_chirpM0 = new Chirp(false, true, smLink);
// initialize devices/modules
led_init();
prm_init(g_chirpUsb);
pwr_init();
cam_init();
}
extern int32_t ciaaMulticore_init_Arch(void)
{
int32_t rv = -1;
/* Init IPC queue, only in master core */
rv = ciaaLibs_circBufInit(ipc_queue,
CIAA_MULTICORE_IPC_QUEUE_ADDR + sizeof(ciaaLibs_CircBufType),
CIAA_MULTICORE_IPC_QUEUE_LEN);
if(rv == 1)
{
NVIC_EnableIRQ(M0APP_IRQn);
/* Start slave core */
cr_start_m0(SLAVE_M0APP, CIAA_MULTICORE_CORE_1_IMAGE);
rv = 0;
}
return rv;
}
//void pixyInit(uint32_t slaveRomStart, const unsigned char slaveImage[], uint32_t imageSize)
void pixyInit(void)
{
// write stack guard word
// STACK_GUARD = STACK_GUARD_WORD;
commonInit();
IPC_haltSlave();
ADCInit();
SCTInit();
CameraInit();
/* IPC_startSlave(); */
// start slave
/* if (slaveRomStart && slaveImage && imageSize)
{
IPC_downloadSlaveImage(slaveRomStart, slaveImage, imageSize);
} */
// initialize shared memory interface before running M0
SMLink *smLink = new SMLink;
// run M0
cr_start_m0(SLAVE_M0APP,&__core_m0app_START__);
// initialize chirp objects
USBLink *usbLink = new USBLink;
g_chirpUsb = new Chirp(false, false, usbLink);
g_chirpM0 = new Chirp(false, true, smLink);
// initialize devices/modules
led_init();
if (prm_init(g_chirpUsb)<0) // error, let user know (don't just continue like nothing's happened)
showError(1, 0x0000ff, "Flash is corrupt, parameters have been lost\n");
pwr_init();
cam_init();
rcs_init();
//cc_init();
}
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 {
