/*
* Application entry point.
*/
int main(void) {
static const evhandler_t evhndl[] = {
InsertHandler,
RemoveHandler
};
Thread *shelltp = NULL;
struct EventListener el0, el1;
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Activates the serial driver 2 using the driver default configuration.
*/
sdStart(&SD2, NULL);
/*
* Shell manager initialization.
*/
shellInit();
/*
* Initializes the MMC driver to work with SPI2.
*/
palSetPadMode(IOPORT2, GPIOB_SPI2NSS, PAL_MODE_OUTPUT_PUSHPULL);
palSetPad(IOPORT2, GPIOB_SPI2NSS);
mmcObjectInit(&MMCD1, &SPID2,
&ls_spicfg, &hs_spicfg,
mmc_is_protected, mmc_is_inserted);
mmcStart(&MMCD1, NULL);
/*
* Creates the blinker thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and listen for events.
*/
chEvtRegister(&MMCD1.inserted_event, &el0, 0);
chEvtRegister(&MMCD1.removed_event, &el1, 1);
while (TRUE) {
if (!shelltp)
shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);
else if (chThdTerminated(shelltp)) {
chThdRelease(shelltp); /* Recovers memory of the previous shell. */
shelltp = NULL; /* Triggers spawning of a new shell. */
}
chEvtDispatch(evhndl, chEvtWaitOne(ALL_EVENTS));
}
return 0;
}
/*
* Application entry point.
*/
int main(void) {
static const evhandler_t evhndl[] = {
TimerHandler,
InsertHandler,
RemoveHandler
};
static EvTimer evt;
struct EventListener el0, el1, el2;
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Activates the serial driver 1 using the driver default configuration.
*/
sdStart(&SD1, NULL);
/*
* Buzzer driver initialization.
*/
buzzInit();
/*
* Initializes the MMC driver to work with SPI2.
*/
mmcObjectInit(&MMCD1);
mmcStart(&MMCD1, &mmccfg);
/*
* Activates the card insertion monitor.
*/
tmr_init(&MMCD1);
/*
* Creates the blinker threads.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
chThdCreateStatic(waThread2, sizeof(waThread2), NORMALPRIO, Thread2, NULL);
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and listen for events.
*/
evtInit(&evt, MS2ST(500)); /* Initializes an event timer object. */
evtStart(&evt); /* Starts the event timer. */
chEvtRegister(&evt.et_es, &el0, 0); /* Registers on the timer event source. */
chEvtRegister(&inserted_event, &el1, 1);
chEvtRegister(&removed_event, &el2, 2);
while (TRUE)
chEvtDispatch(evhndl, chEvtWaitOne(ALL_EVENTS));
return 0;
}
/*! \brief sdlog thread.
*
* Test logging to microSD card on e407.
*
*/
msg_t sdlog_thread(void *p) {
void * arg __attribute__ ((unused)) = p;
static const evhandler_t evhndl_sdclog[] = {
sdc_log_data,
sdc_log_data,
sdc_log_data,
sdc_log_data,
sdc_log_data
};
struct EventListener el0, el1, el2, el3, el4;
chRegSetThreadName("sdlog_thread");
#ifdef DEBUG_SDCLOG
/*chThdSleepMilliseconds(1000);*/
#endif
SDCLOGDBG("Start sdlog thread\r\n");
// init structure
datafile_state.log_sequence = 0;
datafile_state.write_errors = 0;
datafile_state.sd_log_opened = false;
sdc_reset_fp_index();
sdc_init_eod((uint8_t)0xa5);
// Assert data is halfword aligned
if(((sizeof(GENERIC_message)*8) % 16) != 0) {
SDCLOGDBG("%s: GENERIC message is not halfword aligned.\r\n", __func__);
return (SDC_ASSERT_ERROR);
}
// Assert we will not overflow Payload
if( (sizeof(MPU9150_read_data) > (sizeof(datafile_state.log_data.data)-1)) ||
(sizeof(MPL3115A2_read_data) > (sizeof(datafile_state.log_data.data)-1)) ||
(sizeof(ADIS16405_burst_data) > (sizeof(datafile_state.log_data.data)-1))) {
SDCLOGDBG("%s: DATA size is too large\r\n");
return (SDC_ASSERT_ERROR);
}
chEvtRegister(&mpl3115a2_data_event , &el0, MPL3115A2);
chEvtRegister(&adis_spi_burst_data_captured, &el1, ADIS16405);
chEvtRegister(&mpu9150_data_event , &el2, MPU9150);
chEvtRegister(&sdc_halt_event , &el3, SENSOR_LOG_HALT);
chEvtRegister(&sdc_start_event , &el4, SENSOR_LOG_START);
while(1) {
if(!fs_stop) {
chEvtDispatch(evhndl_sdclog, chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(50)));
} else {
chEvtDispatch(evhndl_sdclog, chEvtWaitOneTimeout((1<<SENSOR_LOG_START), MS2ST(50)));
}
}
return -1;
}
static msg_t tUsbTx(void *arg) {
(void)arg;
chRegSetThreadName("usbTx");
msg_t msg;
usbPacket *usbBufp;
enum {UsbTxComleteID = 0, UsbResetID = 1, UsbConfiguredID = 2};
EventListener elUsbTxComplete;
EventListener elUsbReset;
EventListener elUsbConfigured;
eventmask_t activeEvents;
chEvtRegister(&esUsbTxComplete, &elUsbTxComplete, UsbTxComleteID);
chEvtRegister(&esUsbReset, &elUsbReset, UsbResetID);
chEvtRegister(&esUsbConfigured, &elUsbConfigured, UsbConfiguredID);
// Wait for the USB system to be configured. and clear all other event flags.
chEvtWaitOne(EVENT_MASK(UsbConfiguredID));
chEvtGetAndClearEvents(EVENT_MASK(UsbTxComleteID) | EVENT_MASK(UsbResetID));
while (TRUE) {
chMBFetch (&usbTXMailbox, &msg, TIME_INFINITE);
// Check if USB has been reconfigured while waiting for message from sysctrl
activeEvents = chEvtGetAndClearEvents(EVENT_MASK(UsbConfiguredID));
if (activeEvents == EVENT_MASK(UsbConfiguredID)) {
// If so, clear the reset event since it is no longer relevant.
activeEvents = chEvtGetAndClearEvents(EVENT_MASK(UsbResetID));
}
// Typecast Mailbox message to command package pointer for readability
usbBufp = (usbPacket*)msg;
// Prepare transmit and start the transmission. This operation will return immediately
usbPrepareTransmit(usbp, EP_IN, usbBufp->packet, (size_t)usbBufp->size);
chSysLock();
usbStartTransmitI(usbp, EP_IN);
chSysUnlock();
//Check for events from the USB system.
activeEvents = chEvtWaitAny(EVENT_MASK(UsbTxComleteID) | EVENT_MASK(UsbResetID));
if (activeEvents == EVENT_MASK(UsbResetID)) {
chEvtWaitOne(EVENT_MASK(UsbConfiguredID));
// Clear any events that has occurred while the usb was not configured.
chEvtGetAndClearEvents(EVENT_MASK(UsbTxComleteID) | EVENT_MASK(UsbResetID));
}
usbFreeMailboxBuffer (usbBufp);
}
return 0;
}
/*
* Application entry point.
*/
int main(void) {
static const evhandler_t evhndl[] = {
InsertHandler,
RemoveHandler
};
Thread *shelltp = NULL;
struct EventListener el0, el1;
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Activates the serial driver 1 and SDC driver 1 using default
* configuration.
*/
sdStart(&SD1, NULL);
sdcStart(&SDCD1, NULL);
/*
* Shell manager initialization.
*/
shellInit();
/*
* Activates the card insertion monitor.
*/
tmr_init(&SDCD1);
/*
* Creates the blinker thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and listen for events.
*/
chEvtRegister(&inserted_event, &el0, 0);
chEvtRegister(&removed_event, &el1, 1);
while (TRUE) {
if (!shelltp)
shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);
else if (chThdTerminated(shelltp)) {
chThdRelease(shelltp); /* Recovers memory of the previous shell. */
shelltp = NULL; /* Triggers spawning of a new shell. */
}
chEvtDispatch(evhndl, chEvtWaitOne(ALL_EVENTS));
}
}
/*------------------------------------------------------------------------*
* Simulator main. *
*------------------------------------------------------------------------*/
int main(void) {
EventListener sd1fel, sd2fel, tel;
/*
* HAL initialization.
*/
halInit();
/*
* ChibiOS/RT initialization.
*/
chSysInit();
/*
* Serial ports (simulated) initialization.
*/
sdStart(&SD1, NULL);
sdStart(&SD2, NULL);
/*
* Shell manager initialization.
*/
shellInit();
chEvtRegister(&shell_terminated, &tel, 0);
/*
* Console thread started.
*/
cdtp = chThdCreateFromHeap(NULL, CONSOLE_WA_SIZE, NORMALPRIO + 1,
console_thread, NULL);
/*
* Initializing connection/disconnection events.
*/
cputs("Shell service started on SD1, SD2");
cputs(" - Listening for connections on SD1");
(void) sdGetAndClearFlags(&SD1);
chEvtRegister(&SD1.sevent, &sd1fel, 1);
cputs(" - Listening for connections on SD2");
(void) sdGetAndClearFlags(&SD2);
chEvtRegister(&SD2.sevent, &sd2fel, 2);
/*
* Events servicing loop.
*/
while (!chThdShouldTerminate())
chEvtDispatch(fhandlers, chEvtWaitOne(ALL_EVENTS));
/*
* Clean simulator exit.
*/
chEvtUnregister(&SD1.sevent, &sd1fel);
chEvtUnregister(&SD2.sevent, &sd2fel);
return 0;
}
/*------------------------------------------------------------------------*
* Simulator main. *
*------------------------------------------------------------------------*/
int main(void) {
EventListener sd1fel, sd2fel, tel;
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Serial ports (simulated) initialization.
*/
sdStart(&SD1, NULL);
sdStart(&SD2, NULL);
/*
* Shell manager initialization.
*/
shellInit();
chEvtRegister(&shell_terminated, &tel, 0);
/*
* Console thread started.
*/
cdtp = chThdCreateFromHeap(NULL, CONSOLE_WA_SIZE, NORMALPRIO + 1,
console_thread, NULL);
/*
* Initializing connection/disconnection events.
*/
cputs("Shell service started on SD1, SD2");
cputs(" - Listening for connections on SD1");
(void) chIOGetAndClearFlags(&SD1);
chEvtRegister(chIOGetEventSource(&SD1), &sd1fel, 1);
cputs(" - Listening for connections on SD2");
(void) chIOGetAndClearFlags(&SD2);
chEvtRegister(chIOGetEventSource(&SD2), &sd2fel, 2);
/*
* Events servicing loop.
*/
while (!chThdShouldTerminate())
chEvtDispatch(fhandlers, chEvtWaitOne(ALL_EVENTS));
/*
* Clean simulator exit.
*/
chEvtUnregister(chIOGetEventSource(&SD1), &sd1fel);
chEvtUnregister(chIOGetEventSource(&SD2), &sd2fel);
return 0;
}
void init_fatfs( void )
{
// Activates the SDC driver 1 using default configuration.
sdcStart( &SDCD1, NULL );
// Activates the card insertion monitor.
tmr_init( &SDCD1 );
chEvtRegister( &inserted_event, &el0, 0 );
chEvtRegister( &removed_event, &el1, 1 );
}
/*------------------------------------------------------------------------*
* Simulator main. *
*------------------------------------------------------------------------*/
int main(void) {
initTestStream(&testStream);
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Serial ports (simulated) initialization.
*/
sdStart(&SD1, NULL);
sdStart(&SD2, NULL);
/*
* Console thread started.
*/
cdtp = chThdCreateFromHeap(NULL, CONSOLE_WA_SIZE, NORMALPRIO + 1, console_thread, NULL);
/*
* Initializing connection/disconnection events.
*/
cputs("Shell service started on SD1, SD2");
cputs(" - Listening for connections on SD1");
chEvtRegister(chnGetEventSource(&SD1), &sd1fel, 1);
cputs(" - Listening for connections on SD2");
chEvtRegister(chnGetEventSource(&SD2), &sd2fel, 2);
rusEfiFunctionalTest();
/*
* Events servicing loop.
*/
while (!chThdShouldTerminate()) {
chEvtDispatch(fhandlers, chEvtWaitOne(ALL_EVENTS));
printPendingMessages();
chThdSleepMilliseconds(100);
}
/*
* Clean simulator exit.
*/
chEvtUnregister(chnGetEventSource(&SD1), &sd1fel);
chEvtUnregister(chnGetEventSource(&SD2), &sd2fel);
return 0;
}
int main(void)
{
static const evhandler_t evhndl[] = {InsertHandler, RemoveHandler};
struct EventListener el0, el1;
// os init
halInit();
chSysInit();
// setup LED pads
palSetPadMode(GPIOD, 12, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
palClearPad(GPIOD, 12); // green LED
palSetPadMode(GPIOD, 15, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
palClearPad(GPIOD, 15); // blue LED
// setup pads to USART2 function (connect these pads through RS232 transceiver with PC, terminal emu needs 38400 baud)
sdStart(&SD2, NULL);
palSetPadMode(GPIOA, 2, PAL_MODE_ALTERNATE(7) | PAL_STM32_OSPEED_HIGHEST); // TX
palSetPadMode(GPIOA, 3, PAL_MODE_ALTERNATE(7)); // RX
// setup pads to SPI1 function (connect these pads to your SD card accordingly)
palSetPadMode(GPIOC, 4, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); // NSS
palSetPadMode(GPIOA, 5, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST); // SCK
palSetPadMode(GPIOA, 6, PAL_MODE_ALTERNATE(5)); // MISO
palSetPadMode(GPIOA, 7, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST); // MOSI
palSetPad(GPIOC, 4); // set NSS high
// initialize MMC driver
mmcObjectInit(&MMCD1, &SPID1, &ls_spicfg, &hs_spicfg, mmc_is_protected, mmc_is_inserted);
mmcStart(&MMCD1, NULL);
// ChibiOS has no I2S support yet;
// codec.c initializes everything necessary
// except the I2S TX DMA interrupt vector (because it would
// conflict with the ChibiOS kernel)
// we can make ChibiOS call our own handler by letting
// it create the DMA stream for SPI3
dmaStreamAllocate(SPID3.dmatx,
STM32_SPI_SPI3_IRQ_PRIORITY,
(stm32_dmaisr_t)I2SDmaTxInterrupt,
&SPID3);
// blink thread; also checks the user button
chThdCreateStatic(waBlinkThread, sizeof(waBlinkThread), NORMALPRIO, BlinkThread, NULL);
chEvtRegister(&MMCD1.inserted_event, &el0, 0);
chEvtRegister(&MMCD1.removed_event, &el1, 1);
while(TRUE)
{
chEvtDispatch(evhndl, chEvtWaitOne(ALL_EVENTS));
}
}
static msg_t tUsbRx(void *arg) {
(void)arg;
chRegSetThreadName("usbRx");
enum {UsbRxComleteID = 0, UsbResetID = 1, UsbConfiguredID = 2};
usbPacket *usbBufp;
EventListener elUsbRxComplete;
EventListener elUsbReset;
EventListener elUsbConfigured;
eventmask_t activeEvents;
chEvtRegister(&esUsbRxComplete, &elUsbRxComplete, UsbRxComleteID);
chEvtRegister(&esUsbReset, &elUsbReset, UsbResetID);
chEvtRegister(&esUsbConfigured, &elUsbConfigured, UsbConfiguredID);
// Wait for the USB system to be configured.
chEvtWaitOne(EVENT_MASK(UsbConfiguredID));
chEvtGetAndClearEvents(EVENT_MASK(UsbRxComleteID) | EVENT_MASK(UsbResetID));
while (TRUE) {
// Allocate buffer space for reception of package in the sysctrl mempool
usbBufp = usbAllocMailboxBuffer();
// Prepare receive operation and initiate the usb system to listen
usbPrepareReceive(usbp, EP_OUT, usbBufp->packet, 64);
chSysLock();
usbStartReceiveI(usbp, EP_OUT);
chSysUnlock();
// Wait for events from the USB system
activeEvents = chEvtWaitAny(EVENT_MASK(UsbRxComleteID) | EVENT_MASK(UsbResetID));
if (activeEvents == EVENT_MASK(UsbResetID)) {
// If the system was reset, clean up and wait for new configure.
usbFreeMailboxBuffer (usbBufp);
chEvtWaitOne(EVENT_MASK(UsbConfiguredID));
chEvtGetAndClearEvents(EVENT_MASK(UsbRxComleteID) | EVENT_MASK(UsbResetID));
}
else {
// Post pagckage to sysctrl if receive was successful
usbBufp->size = ep2outstate.rxcnt;
chMBPost (&usbRXMailbox, (msg_t)usbBufp, TIME_INFINITE);
}
}
return 0;
}
static THD_FUNCTION(cancom_thread, arg) {
(void)arg;
chRegSetThreadName("CAN");
event_listener_t el;
CANRxFrame rxmsg;
uint8_t buffer[9];
chEvtRegister(&CANDx.rxfull_event, &el, 0);
while(!chThdShouldTerminateX()) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0) {
continue;
}
while (canReceive(&CANDx, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK) {
buffer[0] = rxmsg.SID;
for (int i = 0;i < rxmsg.DLC;i++) {
buffer[i + 1] = rxmsg.data8[i];
}
packet_handler_int_process_packet(buffer, rxmsg.DLC + 1);
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
}
static msg_t can_rx(void *p) {
struct can_instance *cip = p;
EventListener el;
CANRxFrame rxmsg;
(void)p;
chRegSetThreadName("receiver");
chEvtRegister(&cip->canp->rxfull_event, &el, 0);
#if SPC5_CAN_USE_FILTERS
rxFlag = chEvtGetAndClearFlagsI(&el);
#endif
while(!chThdShouldTerminate()) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0)
continue;
#if !SPC5_CAN_USE_FILTERS
while (canReceive(cip->canp, CAN_ANY_MAILBOX,
&rxmsg, TIME_IMMEDIATE) == RDY_OK) {
/* Process message.*/
palTogglePad(PORT_D, cip->led);
}
#else
while (canReceive(cip->canp, rxFlag,
&rxmsg, TIME_IMMEDIATE) == RDY_OK) {
/* Process message.*/
palTogglePad(PORT_D, cip->led);
}
#endif
}
chEvtUnregister(&CAND1.rxfull_event, &el);
return 0;
}
int main(void) {
halInit();
chSysInit();
uint8_t i;
event_listener_t tel;
// Serial Port Setup
sdStart(&SD1, NULL);
palSetPadMode(GPIOC, 4, PAL_MODE_ALTERNATE(7));
palSetPadMode(GPIOC, 5, PAL_MODE_ALTERNATE(7));
chprintf((BaseSequentialStream*)&SD1, "Up and Running\n\r");
palSetPadMode(GPIOB, 3, PAL_MODE_ALTERNATE(6)); /* SCK. */
palSetPadMode(GPIOB, 4, PAL_MODE_ALTERNATE(6)); /* MISO.*/
palSetPadMode(GPIOB, 5, PAL_MODE_ALTERNATE(6)); /* MOSI.*/
palSetPadMode(GPIOC, GPIOC_PIN1, PAL_MODE_OUTPUT_PUSHPULL);
palSetPad(GPIOC, GPIOC_PIN1);
palSetPadMode(GPIOC, GPIOC_PIN2, PAL_MODE_OUTPUT_PUSHPULL);
palClearPad(GPIOC, GPIOC_PIN2);
palSetPadMode(GPIOC, GPIOC_PIN3, PAL_MODE_INPUT_PULLUP);
spiStart(&SPID3, &nrf24l01SPI);
chMtxObjectInit(&nrfMutex);
//FROM RX---
extStart(&EXTD1, &extcfg);
//---
nrf24l01ObjectInit(&nrf24l01);
nrf24l01Start(&nrf24l01, &nrf24l01Config);
//FROM RX ---
extChannelEnable(&EXTD1, 3);
//-----
initNRF24L01(&nrf24l01);
chprintf((BaseSequentialStream*)&SD1, "\n\rUp and Running\n\r");
shellInit();
chEvtRegister(&shell_terminated, &tel, 0);
shelltp1 = shellCreate(&shell_cfg1, sizeof(waShell), NORMALPRIO);
//FROM RX---
chThdCreateStatic(recieverWorkingArea, sizeof(recieverWorkingArea), NORMALPRIO, receiverThread, NULL);
//FROM RX^^^^
/*
for (i=0;i<32;i++) {
serialOutBuf[i] = 3;
}
*/
for (;;) {
chEvtDispatch(fhandlers, chEvtWaitOne(ALL_EVENTS));
}
}
msg_t WebThread(void *p) {
EvTimer evt1, evt2;
EventListener el0, el1, el2;
uip_ipaddr_t ipaddr;
(void)p;
/*
* Event sources setup.
*/
chEvtRegister(macGetReceiveEventSource(ÐD1), &el0, FRAME_RECEIVED_ID);
chEvtAddFlags(EVENT_MASK(FRAME_RECEIVED_ID)); /* In case some frames are already buffered */
evtInit(&evt1, MS2ST(500));
evtStart(&evt1);
chEvtRegister(&evt1.et_es, &el1, PERIODIC_TIMER_ID);
evtInit(&evt2, S2ST(10));
evtStart(&evt2);
chEvtRegister(&evt2.et_es, &el2, ARP_TIMER_ID);
/*
* EMAC driver start.
*/
macStart(ÐD1, &mac_config);
(void)macPollLinkStatus(ÐD1);
/*
* uIP initialization.
*/
uip_init();
uip_setethaddr(macaddr);
uip_ipaddr(ipaddr, IPADDR0, IPADDR1, IPADDR2, IPADDR3);
uip_sethostaddr(ipaddr);
httpd_init();
while (TRUE) {
chEvtDispatch(evhndl, chEvtWaitOne(ALL_EVENTS));
}
return 0;
}
/*! \brief ADIS DIO1 thread
*
* For burst mode transactions t_readrate is 1uS
*
*/
static msg_t Thread_adis_dio1(void *arg) {
(void)arg;
static const evhandler_t evhndl_dio1[] = {
adis_burst_read_handler,
//adis_read_id_handler,
adis_spi_cb_txdone_handler,
adis_release_bus
};
struct EventListener evl_dio;
struct EventListener evl_spi_ev;
struct EventListener evl_spi_release;
chRegSetThreadName("adis_dio");
chEvtRegister(&adis_dio1_event, &evl_dio, 0);
chEvtRegister(&adis_spi_cb_txdone_event, &evl_spi_ev, 1);
chEvtRegister(&adis_spi_cb_releasebus, &evl_spi_release, 2);
while (TRUE) {
chEvtDispatch(evhndl_dio1, chEvtWaitOneTimeout((EVENT_MASK(2)|EVENT_MASK(1)|EVENT_MASK(0)), US2ST(50)));
}
return -1;
}
static msg_t batterySurveyThd(void *arg)
{
(void)arg;
chRegSetThreadName ("battery survey");
chEvtRegister(&powerOutageSource, &powerOutageListener, 1);
chThdSleepMilliseconds (2000);
register_adc_watchdog((uint32_t) ADC1, 4,
V_ALERT, 0xfff, &powerOutageIsr);
chEvtWaitOne(EVENT_MASK(1));
chibios_logFinish ();
chThdExit(0);
return 0;
}
static msg_t PollIMUThread(void *arg){
(void)arg;
chRegSetThreadName("PollIMU");
struct EventListener self_el;
chEvtRegister(&imu_event, &self_el, 2);
while (TRUE) {
chEvtWaitAll(EVENT_MASK(0) && EVENT_MASK(4));
mpu_i2c_read_data(0x3B, 14); // Read accelerometer, temperature and gyro data
chEvtBroadcastFlags(&imu_event, EVENT_MASK(2));
}
return 0;
}
/*! \brief ADIS Newdata Thread
*/
static msg_t Thread_adis_newdata(void *arg) {
(void)arg;
chRegSetThreadName("adis_newdata");
static const evhandler_t evhndl_newdata[] = {
adis_newdata_handler
};
struct EventListener evl_spi_cb2;
chEvtRegister(&adis_spi_cb_newdata, &evl_spi_cb2, 0);
while (TRUE) {
chEvtDispatch(evhndl_newdata, chEvtWaitOneTimeout((eventmask_t)1, US2ST(50)));
}
return -1;
}
static msg_t Thread_mpu9150_int(void* arg) {
(void) arg;
static const evhandler_t evhndl_mpu9150[] = {
mpu9150_int_event_handler
};
struct EventListener evl_mpu9150;
chRegSetThreadName("mpu9150_int");
chEvtRegister(&mpu9150_int_event, &evl_mpu9150, 0);
while (TRUE) {
chEvtDispatch(evhndl_mpu9150, chEvtWaitOneTimeout(EVENT_MASK(0), MS2ST(50)));
}
return -1;
}
static msg_t PollAccelThread(void *semp){
chRegSetThreadName("PollAccel");
msg_t sem_status = RDY_OK;
struct EventListener self_el;
chEvtRegister(&init_event, &self_el, INIT_FAKE_EVID);
while (TRUE) {
sem_status = chBSemWaitTimeout((BinarySemaphore*)semp, MS2ST(20));
txbuf[0] = ACCEL_STATUS;
if ((i2c_transmit(mma8451addr, txbuf, 1, rxbuf, 7) == RDY_OK) && (sem_status == RDY_OK)){
raw_data.xacc = complement2signed(rxbuf[1], rxbuf[2]);
raw_data.yacc = complement2signed(rxbuf[3], rxbuf[4]);
raw_data.zacc = complement2signed(rxbuf[5], rxbuf[6]);
/* there is no need of correcting of placement. Just get milli g */
mavlink_raw_imu_struct.xacc = raw_data.xacc * *xpol;
mavlink_raw_imu_struct.yacc = raw_data.yacc * *ypol;
mavlink_raw_imu_struct.zacc = raw_data.zacc * *zpol;
comp_data.xacc = 1000 * (((int32_t)raw_data.xacc) * *xpol + *xoffset) / *xsens;
comp_data.yacc = 1000 * (((int32_t)raw_data.yacc) * *ypol + *yoffset) / *ysens;
comp_data.zacc = 1000 * (((int32_t)raw_data.zacc) * *zpol + *zoffset) / *zsens;
/* fill scaled debug struct */
mavlink_scaled_imu_struct.xacc = comp_data.xacc;
mavlink_scaled_imu_struct.yacc = comp_data.yacc;
mavlink_scaled_imu_struct.zacc = comp_data.zacc;
}
else{
raw_data.xacc = -32768;
raw_data.yacc = -32768;
raw_data.zacc = -32768;
mavlink_raw_imu_struct.xacc = -32768;
mavlink_raw_imu_struct.yacc = -32768;
mavlink_raw_imu_struct.zacc = -32768;
mavlink_scaled_imu_struct.xacc = -32768;
mavlink_scaled_imu_struct.yacc = -32768;
mavlink_scaled_imu_struct.zacc = -32768;
}
if (chThdSelf()->p_epending & EVENT_MASK(SIGHALT_EVID))
chThdExit(RDY_OK);
}
return 0;
}
static THD_FUNCTION(can_rx, p) {
event_listener_t el;
CANRxFrame rxmsg;
(void)p;
chRegSetThreadName("receiver");
chEvtRegister(&CAND1.rxfull_event, &el, 0);
while(!chThdShouldTerminateX()) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0)
continue;
while (canReceive(&CAND1, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK) {
/* Process message.*/
palTogglePad(GPIOE, GPIOE_LED3_RED);
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
}
static msg_t can_rx(void *p) {
EventListener el;
CANRxFrame rxmsg;
(void)p;
chRegSetThreadName("receiver");
chEvtRegister(&CAND1.rxfull_event, &el, 0);
while(!chThdShouldTerminate()) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0)
continue;
while (canReceive(&CAND1, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == RDY_OK) {
/* Process message.*/
palTogglePad(IOPORT3, GPIOC_LED);
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
return 0;
}
msg_t data_udp_send_thread(void *p) {
void * arg __attribute__ ((unused)) = p;
static const evhandler_t evhndl_mpu9150[] = {
data_udp_send_mpu9150_data
};
struct EventListener evl_mpu9150;
err_t err;
ip_addr_t ip_addr_sensor;
ip_addr_t ip_addr_fc;
chRegSetThreadName("data_udp_send_thread");
chEvtRegister(&mpu9150_data_event, &evl_mpu9150, 0);
IMU_A_IP_ADDR(&ip_addr_sensor);
IP_PSAS_FC(&ip_addr_fc);
mpu9150_mac_info.conn = netconn_new( NETCONN_UDP );
/* Bind to the local address, or to ANY address */
// netconn_bind(conn, NULL, DATA_UDP_TX_THREAD_PORT ); //local port, NULL is bind to ALL ADDRESSES! (IP_ADDR_ANY)
err = netconn_bind(mpu9150_mac_info.conn, &ip_addr_sensor, IMU_A_TX_PORT ); //local port
if (err == ERR_OK) {
/* Connect to specific address or a broadcast address */
/*
* \todo Understand why a UDP needs a connect...
* This may be a LwIP thing that chooses between tcp_/udp_/raw_ connections internally.
*
*/
// netconn_connect(conn, IP_ADDR_BROADCAST, DATA_UDP_TX_THREAD_PORT );
err = netconn_connect(mpu9150_mac_info.conn, &ip_addr_fc, FC_LISTEN_PORT_IMU_A );
if(err == ERR_OK) {
while (TRUE) {
chEvtDispatch(evhndl_mpu9150, chEvtWaitOneTimeout(EVENT_MASK(0), MS2ST(50)));
}
}
return RDY_RESET;
}
return RDY_RESET;
}
static THD_FUNCTION(can_rx, p) {
struct can_instance *cip = p;
event_listener_t el;
CANRxFrame rxmsg;
(void)p;
chRegSetThreadName("receiver");
chEvtRegister(&cip->canp->rxfull_event, &el, 0);
while(!chThdShouldTerminateX()) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, TIME_MS2I(100)) == 0)
continue;
while (canReceive(cip->canp, CAN_ANY_MAILBOX,
&rxmsg, TIME_IMMEDIATE) == MSG_OK) {
/* Process message.*/
palToggleLine(cip->led);
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
}
static void logic_scanThread(void * data)
{
(void) data;
chRegSetThreadName("logic scan");
EventListener el;
chEvtRegister(&event_i2c_buttons, &el,
BUTTON_EVENT_ID | BUTTON_NOW_EVENT_ID);
eventmask_t mask;
static uint32_t recevie = 0;
while (TRUE)
{
mask = chEvtWaitOne(BUTTON_NOW_EVENT_ID | BUTTON_EVENT_ID);
recevie++;
logic_button(active.bank, &foot_switch, mask);
}
}
static THD_FUNCTION(serialThread, arg) {
(void)arg;
event_listener_t new_data_listener;
event_listener_t sd1_listener;
event_listener_t sd2_listener;
chEvtRegister(&new_data_event, &new_data_listener, 0);
eventflags_t events = CHN_INPUT_AVAILABLE
| SD_PARITY_ERROR | SD_FRAMING_ERROR | SD_OVERRUN_ERROR | SD_NOISE_ERROR | SD_BREAK_DETECTED;
chEvtRegisterMaskWithFlags(chnGetEventSource(&SD1),
&sd1_listener,
EVENT_MASK(1),
events);
chEvtRegisterMaskWithFlags(chnGetEventSource(&SD2),
&sd2_listener,
EVENT_MASK(2),
events);
bool need_wait = false;
while(true) {
eventflags_t flags1 = 0;
eventflags_t flags2 = 0;
if (need_wait) {
eventmask_t mask = chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(1000));
if (mask & EVENT_MASK(1)) {
flags1 = chEvtGetAndClearFlags(&sd1_listener);
print_error("DOWNLINK", flags1, &SD1);
}
if (mask & EVENT_MASK(2)) {
flags2 = chEvtGetAndClearFlags(&sd2_listener);
print_error("UPLINK", flags2, &SD2);
}
}
// Always stay as master, even if the USB goes into sleep mode
is_master |= usbGetDriverStateI(&USBD1) == USB_ACTIVE;
router_set_master(is_master);
need_wait = true;
need_wait &= read_from_serial(&SD2, UP_LINK) == 0;
need_wait &= read_from_serial(&SD1, DOWN_LINK) == 0;
update_transport();
}
}
static THD_FUNCTION(can_rx, p) {
event_listener_t el;
CANRxFrame rxmsg;
(void)p;
chRegSetThreadName("receiver");
chEvtRegister(&CAND1.rxfull_event, &el, 0);
while (true) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0)
continue;
while (canReceive(&CAND1, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK) {
/* Process message.*/
oca_led_toggle(oca_led_act);
CanMessage UsbMessage = CanMessage_init_default;
UsbMessage.DLC = rxmsg.DLC;
UsbMessage.RTR = rxmsg.DLC;
UsbMessage.IDE = rxmsg.IDE;
UsbMessage.ID = rxmsg.EID;
UsbMessage.Data1 = rxmsg.data32[0];
UsbMessage.Data1 = rxmsg.data32[1];
uint8_t buffer[30];
pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
pb_encode(&stream, CanMessage_fields, &UsbMessage);
/*if(serusbcfg.usbp->state == USB_ACTIVE)
{
if(SDU1.state == SDU_READY)
chnWrite(&SDU1, buffer, stream.bytes_written);
}*/
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
}
void EvtSource::registerOne(chibios_rt::EvtListener *elp,
eventid_t eid) {
chEvtRegister(&ev_source, &elp->ev_listener, eid);
}
int main(void) {
static Thread *shelltp = NULL;
static const evhandler_t evhndl[] = {
InsertHandler,
RemoveHandler,
packetReceivedHandler
};
struct EventListener el0, el1, el2;
chEvtRegister(&packet_event, &el2, 0);
chEvtInit(&packet_event);
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Initializes a serial-over-USB CDC driver.
*/
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
/*
* Activates the USB driver and then the USB bus pull-up on D+.
* Note, a delay is inserted in order to not have to disconnect the cable
* after a reset.
*/
usbDisconnectBus(serusbcfg.usbp);
chThdSleepMilliseconds(1000);
usbStart(serusbcfg.usbp, &usbcfg);
usbConnectBus(serusbcfg.usbp);
/*
* Shell manager initialization.
*/
shellInit();
/*
* Activates the serial driver 6 and SDC driver 1 using default
* configuration.
*/
sdStart(&SD6, NULL);
sdcStart(&SDCD1, NULL);
/*
* Activates the card insertion monitor.
*/
tmr_init(&SDCD1);
/*
* Creates the blinker thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
/*
* Creates the LWIP threads (it changes priority internally).
*/
chThdCreateStatic(wa_lwip_thread, LWIP_THREAD_STACK_SIZE, NORMALPRIO + 2,
lwip_thread, NULL);
/*
* Creates the HTTP thread (it changes priority internally).
*/
/*
chThdCreateStatic(wa_http_server, sizeof(wa_http_server), NORMALPRIO + 1,
http_server, NULL);
*/
WORKING_AREA(wa_udp_receive_server, PWM_REC_STACK_SIZE);
chThdCreateStatic(wa_udp_receive_server, sizeof(wa_udp_receive_server), NORMALPRIO +1,
udp_receive_server_init, NULL);
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and listen for events.
*/
chEvtRegister(&inserted_event, &el0, 0);
chEvtRegister(&removed_event, &el1, 1);
while (TRUE) {
if (!shelltp && (SDU1.config->usbp->state == USB_ACTIVE))
shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);
else if (chThdTerminated(shelltp)) {
chThdRelease(shelltp); /* Recovers memory of the previous shell. */
shelltp = NULL; /* Triggers spawning of a new shell. */
}
if (palReadPad(GPIOA, GPIOA_BUTTON_WKUP) != 0) {
}
chEvtDispatch(evhndl, chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(500)));
}
}
