void App_t::ITask() {
// Filters init
for(int i=0; i<CHNL_CNT; i++) LvlMtr[i].Reset();
while(true) {
uint32_t EvtMsk = chEvtWaitAny(ALL_EVENTS);
#if 1 // ==== USB ====
if(EvtMsk & EVTMSK_USB_READY) {
Uart.Printf("\rUsbReady");
LedAux.SetHi();
}
if(EvtMsk & EVTMSK_USB_SUSPEND) {
Uart.Printf("\rUsbSuspend");
LedAux.SetLo();
}
if(EvtMsk & EVTMSK_START_LISTEN) {
}
if(EvtMsk & EVTMSK_STOP_LISTEN) {
}
#endif
if(EvtMsk & EVTMSK_UART_NEW_CMD) {
OnCmd((Shell_t*)&Uart);
Uart.SignalCmdProcessed();
}
} // while true
}
void App_t::ITask() {
uint32_t EvtMsk = chEvtWaitAny(ALL_EVENTS);
if(EvtMsk & EVTMSK_ADC_READY) {
static int32_t summ = 0;
static int32_t count = 0;
int16_t x0 = (int16_t)(Adc.Rslt & ResolutionMask); //value
summ += (int32_t)x0;
count++;
if(count==10){
summ /= count;
Uart.Printf("%d\r", summ);
summ = 0;
count = 0;
}
Adc.Rslt =0;
}
if(EvtMsk & EVTMSK_UART_RX_POLL) { Uart.PollRx(); } // Check if new cmd received
if(EvtMsk & EVTMSK_ADC_RSLT_READY) {
/*
Uart.Printf("ADC1 = %d ADC2 = %d ADC3 = %d ADC4 = %d\r", (int32_t)iAdc.Result[0], \
(int32_t)iAdc.Result[1], \
(int32_t)iAdc.Result[2], \
(int32_t)iAdc.Result[3]);
*/
}
}
int main(void) {
// ==== Init clock system ====
uint8_t ClkResult = Clk.SwitchToHSE();
if(ClkResult == 0) Clk.HSIDisable();
Clk.SetupBusDividers(ahbDiv1, apbDiv1, apbDiv1);
Clk.UpdateFreqValues();
// ==== Init OS ====
halInit();
chSysInit();
// ==== Init Hard & Soft ====
Init();
if(ClkResult != 0) Uart.Printf("Clk Failure\r");
ir.RegisterEvtTxEnd(&EvtLstnrIrTxEnd, EVENT_MASK(0));
while(TRUE) {
uint16_t w = ID;
w <<= 8;
w |= 0x04;
//Uart.Printf("%X\r", w);
ir.TransmitWord(w
#if IR_DAC
, dc1000mA
#endif
#if IR_PWM
, 100
#endif
);
chEvtWaitAny(EVENT_MASK(0));
// Transmission completed
//GoSleep();
Uart.Printf("TxE\r");
chThdSleepMilliseconds(999);
}
}
static void AppThread(void *arg) {
chRegSetThreadName("App");
uint32_t EvtMsk;
bool PillConnected = false;
while(true) {
EvtMsk = chEvtWaitAny(ALL_EVENTS);
// ==== Process dose ====
// if(EvtMsk & EVTMSK_DOSE_INC) {
// // Check if radio damage occured. Will return 1 if no.
// uint32_t FDamage = Radio.Damage;
// //if(FDamage != 1) Uart.Printf("Dmg=%u\r", FDamage);
// Dose.Increase(FDamage, diUsual);
// //Uart.Printf("Dz=%u; Dmg=%u\r", Dose.Get(), FDamage);
// }
// ==== Store dose ====
// if(EvtMsk & EVTMSK_DOSE_STORE) {
// //if(Dose.Save() != OK) Uart.Printf("EE Fail\r"); // DEBUG
// }
// ==== Check pill ====
if(EvtMsk & EVTMSK_PILL_CHECK) {
// Check if new connection occured
if(PillMgr.CheckIfConnected(PILL_I2C_ADDR) == OK) {
if(!PillConnected) {
PillConnected = true;
App.IPillHandler();
}
}
else PillConnected = false;
} // if EVTMSK_PILL_CHECK
} // while 1
}
static msg_t detect_thread(void *arg) {
(void)arg;
chRegSetThreadName("Detect");
detect_tp = chThdSelf();
for(;;) {
chEvtWaitAny((eventmask_t) 1);
if (!conf_general_detect_motor_param(detect_current, detect_min_rpm,
detect_low_duty, &detect_cycle_int_limit, &detect_coupling_k)) {
detect_cycle_int_limit = 0.0;
detect_coupling_k = 0.0;
}
int32_t ind = 0;
send_buffer[ind++] = COMM_DETECT_MOTOR_PARAM;
buffer_append_int32(send_buffer, (int32_t)(detect_cycle_int_limit * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(detect_coupling_k * 1000.0), &ind);
send_packet(send_buffer, ind);
}
return 0;
}
void App_t::ITask() {
uint32_t EvtMsk = chEvtWaitAny(ALL_EVENTS);
// ==== Uart cmd ====
if(EvtMsk & EVTMSK_UART_RX_POLL) {
while(Uart.ProcessRx() == pdrNewCmd) OnUartCmd(&Uart);
}
}
static THD_FUNCTION(detect_thread, arg) {
(void)arg;
chRegSetThreadName("Detect");
detect_tp = chThdGetSelfX();
for(;;) {
chEvtWaitAny((eventmask_t) 1);
if (!conf_general_detect_motor_param(detect_current, detect_min_rpm,
detect_low_duty, &detect_cycle_int_limit, &detect_coupling_k,
detect_hall_table, &detect_hall_res)) {
detect_cycle_int_limit = 0.0;
detect_coupling_k = 0.0;
}
int32_t ind = 0;
send_buffer[ind++] = COMM_DETECT_MOTOR_PARAM;
buffer_append_int32(send_buffer, (int32_t)(detect_cycle_int_limit * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(detect_coupling_k * 1000.0), &ind);
memcpy(send_buffer + ind, detect_hall_table, 8);
ind += 8;
send_buffer[ind++] = detect_hall_res;
commands_send_packet(send_buffer, ind);
}
}
void MassStorage_t::UsbOutTask() {
while(true) {
if(!Usb.IsReady) chEvtWaitAny(EVTMSK_USB_READY);
// Receive header
Usb.PEpBulkOut->StartReceiveToBuf((uint8_t*)&CmdBlock, MS_CMD_SZ);
uint8_t rslt = Usb.PEpBulkOut->WaitUntilReady();
if(rslt == OK) SCSICmdHandler();
}
}
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;
}
static msg_t Thread2(void *arg) {
BaseSequentialStream *serp = (BaseSequentialStream *) &SD1;
thread2 = chThdSelf();
while (TRUE) {
chEvtWaitAny((eventmask_t) 1);
chprintf(serp, "OVERFLOW\r\n");
chEvtSignal(thread_main, (eventmask_t) 2);
}
return 0;
}
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;
}
void Sound_t::ITask() {
while(true) {
eventmask_t EvtMsk = chEvtWaitAny(ALL_EVENTS);
#if 1 // ==== DMA done ====
if(EvtMsk & VS_EVT_DMA_DONE) {
ISpi.WaitBsyLo(); // Wait SPI transaction end
if(Clk.AHBFreqHz > 12000000) Loop(450); // Make a solemn pause
XCS_Hi(); // }
XDCS_Hi(); // } Stop SPI
// Send next data if VS is ready
if(IDreq.IsHi()) ISendNextData(); // More data allowed, send it now
else IDreq.EnableIrq(IRQ_PRIO_MEDIUM); // Enable dreq irq
}
#endif
if(EvtMsk & VS_EVT_DREQ_IRQ) {
chThdSleepMilliseconds(1); // Make a pause after IRQ rise
ISendNextData();
}
// Play new request
if(EvtMsk & VS_EVT_COMPLETED) {
// Uart.Printf("\rComp");
AddCmd(VS_REG_MODE, 0x0004); // Soft reset
if(IFilename != NULL) IPlayNew();
else {
// AmpfOff(); // switch off the amplifier to save energy
if(IPAppThd != nullptr) chEvtSignal(IPAppThd, EVTMSK_PLAY_ENDS); // Raise event if nothing to play
}
}
// Stop request
else if(EvtMsk & VS_EVT_STOP) {
// Uart.Printf("\rStop");
PrepareToStop();
}
#if 1 // ==== Read next ====
else if(EvtMsk & VS_EVT_READ_NEXT) {
// Uart.Printf("\rreadNext; L= %u %u", Buf1.DataSz, Buf2.DataSz);
FRESULT rslt = FR_OK;
bool Eof = f_eof(&IFile);
// Read next if not EOF
if(!Eof) {
if (Buf1.DataSz == 0) { /*Uart.Printf(" r1");*/ rslt = Buf1.ReadFromFile(&IFile); }
else if(Buf2.DataSz == 0) { /*Uart.Printf(" r2");*/ rslt = Buf2.ReadFromFile(&IFile); }
}
if(rslt != FR_OK) Uart.Printf("\rsndReadErr=%u", rslt);
if(rslt == FR_OK and !Eof) StartTransmissionIfNotBusy();
}
#endif
} // while true
}
static msg_t Thread1(void *arg) {
BaseSequentialStream *serp = (BaseSequentialStream *) &SD1;
thread1 = chThdSelf();
while (TRUE) {
chEvtWaitAny((eventmask_t) 1);
chprintf(serp, "WIDTH[%lu ms, %u ticks] PERIOD[%lu ms, %u ticks]\r\n",
((uint32_t) width * 1000) / ICU_CLK,
width,
((uint32_t) period * 1000) / ICU_CLK,
period);
chEvtSignal(thread_main, (eventmask_t) 1);
}
return 0;
}
static msg_t isrThread(void *arg) {
(void) arg;
chRegSetThreadName("CC2520 EXTI");
isr_tp = chThdSelf();
for (;;) {
chEvtWaitAny((eventmask_t) 1);
if (gpio0_func) {
gpio0_func();
}
}
return 0;
}
void Sound_t::ITask() {
while(true) {
eventmask_t EvtMsk = chEvtWaitAny(ALL_EVENTS);
if(EvtMsk & VS_EVT_DMA_DONE) {
ISpi.WaitBsyLo(); // Wait SPI transaction end
XCS_Hi(); // }
XDCS_Hi(); // } Stop SPI
if(IDreq.IsHi()) ISendNextData(); // More data allowed, send it now
else {
chThdSleepMilliseconds(1); // Allow VS to end up with data processing
chSysLock();
IDreq.EnableIrq(IRQ_PRIO_MEDIUM); // Enable dreq irq
chSysUnlock();
}
}
if(EvtMsk & VS_EVT_DREQ_IRQ) ISendNextData();
// Play new request
if(EvtMsk & VS_EVT_COMPLETED) {
// Uart.Printf("\rComp");
AddCmd(VS_REG_MODE, 0x0004); // Soft reset
if(IFilename != NULL) IPlayNew();
else if(IPThd != nullptr) chEvtSignal(IPThd, EVTMASK_PLAY_ENDS); // Raise event if nothing to play
}
// Stop request
else if(EvtMsk & VS_EVT_STOP) {
// Uart.Printf("\rStop");
PrepareToStop();
}
// Data read request
else if(EvtMsk & VS_EVT_READ_NEXT) {
// Uart.Printf("\rreadNext");
FRESULT rslt = FR_OK;
bool EofAtStart = f_eof(&IFile);
// Read next if not EOF
if(!EofAtStart) {
if (Buf1.DataSz == 0) { /*Uart.Printf("1");*/ rslt = Buf1.ReadFromFile(&IFile); }
else if(Buf2.DataSz == 0) { /*Uart.Printf("2");*/ rslt = Buf2.ReadFromFile(&IFile); }
}
// Check if was EOF or if error occured during reading. Do not do it if EOF occured during reading.
// if(rslt != FR_OK) Uart.Printf("\rsndReadErr=%u", rslt);
if((rslt != FR_OK) or EofAtStart) PrepareToStop();
else StartTransmissionIfNotBusy();
}
} // while true
}
static msg_t fnet_receive_thread(void *arg) {
(void) arg;
EventListener el1;
chRegSetThreadName("FNET receive thread");
chEvtRegisterMask(macGetReceiveEventSource(ÐD1), &el1, FRAME_RECEIVED_ID);
chEvtAddEvents(FRAME_RECEIVED_ID);
while (TRUE) {
eventmask_t mask = chEvtWaitAny(ALL_EVENTS );
if (mask & FRAME_RECEIVED_ID) {
fnet_stm32_input();
}
}
return RDY_OK;
}
__noreturn
void App_t::ITask() {
// Effects.AllTogetherSmoothly(clGreen, 360);
while(true) {
// chThdSleepMilliseconds(2700);
// Effects.AllTogetherNow(clGreen);
// chThdSleepMilliseconds(270);
// Effects.AllTogetherNow(clRed);
// chThdSleepMilliseconds(270);
// Effects.AllTogetherNow(clBlue);
// chThdSleepMilliseconds(270);
// Effects.AllTogetherSmoothly(clGreen, 180);
// chEvtWaitAny(EVT_LEDS_DONE);
// Effects.AllTogetherSmoothly(clRed, 180);
// chEvtWaitAny(EVT_LEDS_DONE);
// Effects.AllTogetherSmoothly(clBlue, 180);
// chEvtWaitAny(EVT_LEDS_DONE);
// Effects.AllTogetherSmoothly(clYellow, 180);
// chEvtWaitAny(EVT_LEDS_DONE);
// Effects.AllTogetherSmoothly(clCyan, 180);
// chEvtWaitAny(EVT_LEDS_DONE);
// Effects.AllTogetherSmoothly(clMagenta, 180);
// chEvtWaitAny(EVT_LEDS_DONE);
// Effects.AllTogetherSmoothly(clWhite, 180);
// chEvtWaitAny(EVT_LEDS_DONE);
// Effects.AllTogetherSmoothly(clBlack, 180);
// chEvtWaitAny(EVT_LEDS_DONE);
__unused eventmask_t Evt = chEvtWaitAny(ALL_EVENTS);
#if UART_RX_ENABLED
if(EvtMsk & EVTMSK_UART_NEW_CMD) {
OnCmd((Shell_t*)&Uart);
Uart.SignalCmdProcessed();
}
#endif
#if 1 // ==== Radio cmd ====
if(Evt & EVT_RADIO_NEW_CMD) {
Effects.AllTogetherSmoothly(clBlack, 1800);
}
#endif
} // while true
}
static THD_FUNCTION( esp8266, arg )
{
(void)arg;
chRegSetThreadName( "esp8266" );
event_listener_t serialListener;
/* Registering on the serial driver 6 as event 1, interested in
* error flags and data-available only, other flags will not wakeup
* the thread.
*/
chEvtRegisterMaskWithFlags(
(struct event_source_t *)chnGetEventSource( &SD6 ),
&serialListener,
EVENT_MASK( 1 ),
SD_FRAMING_ERROR | SD_PARITY_ERROR |
CHN_INPUT_AVAILABLE );
while( true )
{
// Waiting for any of the events we're registered on.
eventmask_t evt = chEvtWaitAny( ALL_EVENTS );
// Serving events.
if( evt & EVENT_MASK(1) )
{
/* Event from the serial interface, getting serial
* flags as first thing.
*/
eventflags_t flags = chEvtGetAndClearFlags( &serialListener );
//Handling errors first.
if( flags & (SD_FRAMING_ERROR | SD_PARITY_ERROR) )
{
DPRINT( 4, KRED "FRAMING/PARITY ERROR" );
}
if( flags & CHN_INPUT_AVAILABLE )
{
char c;
c = sdGet( &SD6 );
sdPut( &SD3, c );
}
}
}
}
static msg_t fnet_timer_thread(void *period_ms) {
EventListener el0;
chRegSetThreadName("FNET timer thread");
evtInit(&fnetEventTimer, MS2ST(period_ms) );
evtStart(&fnetEventTimer);
chEvtRegisterMask(&fnetEventTimer.et_es, &el0, PERIODIC_TIMER_ID);
chEvtAddEvents(PERIODIC_TIMER_ID);
while (TRUE) {
eventmask_t mask = chEvtWaitAny(ALL_EVENTS );
if (mask & PERIODIC_TIMER_ID) {
fnet_timer_ticks_inc();
fnet_timer_handler_bottom(NULL );
}
}
return RDY_OK;
}
static THD_FUNCTION(serial_process_thread, arg) {
(void)arg;
chRegSetThreadName("USB-Serial process");
process_tp = chThdGetSelfX();
for(;;) {
chEvtWaitAny((eventmask_t) 1);
while (serial_rx_read_pos != serial_rx_write_pos) {
packet_process_byte(serial_rx_buffer[serial_rx_read_pos++], PACKET_HANDLER);
if (serial_rx_read_pos == SERIAL_RX_BUFFER_SIZE) {
serial_rx_read_pos = 0;
}
}
}
}
static THD_FUNCTION(tx_thread, arg) {
(void)arg;
chRegSetThreadName("CC2520 Tx");
tx_tp = chThdGetSelfX();
for(;;) {
chEvtWaitAny((eventmask_t) 1);
while(tx_slot_read != tx_slot_write) {
basicRfSendPacket(CC2520_DEST_ADDRESS, tx_buffer[tx_slot_read], tx_slot_len[tx_slot_read]);
led_toggle(LED_GREEN);
tx_slot_read++;
if (tx_slot_read >= TX_BUFFER_SLOTS) {
tx_slot_read = 0;
}
}
}
}
static msg_t serial_process_thread(void *arg) {
(void)arg;
chRegSetThreadName("Serial process");
process_tp = chThdSelf();
for(;;) {
chEvtWaitAny((eventmask_t) 1);
while (serial_rx_read_pos != serial_rx_write_pos) {
packet_process_byte(serial_rx_buffer[serial_rx_read_pos++]);
if (serial_rx_read_pos == SERIAL_RX_BUFFER_SIZE) {
serial_rx_read_pos = 0;
}
}
}
return 0;
}
static msg_t sample_send_thread(void *arg) {
(void)arg;
chRegSetThreadName("Main sample");
sample_send_tp = chThdSelf();
for(;;) {
chEvtWaitAny((eventmask_t) 1);
for (int i = 0;i < sample_len;i++) {
uint8_t buffer[20];
int index = 0;
buffer[index++] = curr0_samples[i] >> 8;
buffer[index++] = curr0_samples[i];
buffer[index++] = curr1_samples[i] >> 8;
buffer[index++] = curr1_samples[i];
buffer[index++] = ph1_samples[i] >> 8;
buffer[index++] = ph1_samples[i];
buffer[index++] = ph2_samples[i] >> 8;
buffer[index++] = ph2_samples[i];
buffer[index++] = ph3_samples[i] >> 8;
buffer[index++] = ph3_samples[i];
buffer[index++] = vzero_samples[i] >> 8;
buffer[index++] = vzero_samples[i];
buffer[index++] = status_samples[i];
buffer[index++] = curr_fir_samples[i] >> 8;
buffer[index++] = curr_fir_samples[i];
comm_send_samples(buffer, index);
// TODO: wait??
chThdSleep(2);
}
}
return 0;
}
static THD_FUNCTION(packet_process_thread, arg) {
(void)arg;
chRegSetThreadName("comm_uart");
process_tp = chThdGetSelfX();
for(;;) {
chEvtWaitAny((eventmask_t) 1);
/*
* Wait for data to become available and process it as long as there is data.
*/
while (serial_rx_read_pos != serial_rx_write_pos) {
bldc_interface_uart_process_byte(serial_rx_buffer[serial_rx_read_pos++]);
if (serial_rx_read_pos == SERIAL_RX_BUFFER_SIZE) {
serial_rx_read_pos = 0;
}
}
}
}
static msg_t rawd_push_loop(void *data)
{
int i;
chRegSetThreadName("net/rawd_push");
for (;;) {
chEvtWaitAny(ALL_EVENTS);
chMtxLock(&rawd_mutex);
for (i = 0; i < NC_COUNT; i++) {
if (!ncs[i])
continue;
if (ncs[i]->pcb.tcp->state != ESTABLISHED)
continue;
netconn_write(ncs[i], &buf_frame, sizeof(buf_frame),
NETCONN_COPY);
}
chMtxUnlock();
}
return 0;
}
static THD_FUNCTION(sample_send_thread, arg) {
(void)arg;
chRegSetThreadName("Main sample");
sample_send_tp = chThdGetSelfX();
for(;;) {
chEvtWaitAny((eventmask_t) 1);
for (int i = 0; i < sample_len; i++) {
uint8_t buffer[20];
int index = 0;
buffer[index++] = curr0_samples[i] >> 8;
buffer[index++] = curr0_samples[i];
buffer[index++] = curr1_samples[i] >> 8;
buffer[index++] = curr1_samples[i];
buffer[index++] = ph1_samples[i] >> 8;
buffer[index++] = ph1_samples[i];
buffer[index++] = ph2_samples[i] >> 8;
buffer[index++] = ph2_samples[i];
buffer[index++] = ph3_samples[i] >> 8;
buffer[index++] = ph3_samples[i];
buffer[index++] = vzero_samples[i] >> 8;
buffer[index++] = vzero_samples[i];
buffer[index++] = status_samples[i];
buffer[index++] = curr_fir_samples[i] >> 8;
buffer[index++] = curr_fir_samples[i];
buffer[index++] = f_sw_samples[i] >> 8;
buffer[index++] = f_sw_samples[i];
commands_send_samples(buffer, index);
}
}
}
static void evt2_execute(void) {
eventmask_t m;
event_listener_t el1, el2;
systime_t target_time;
/*
* Test on chEvtWaitOne() without wait.
*/
chEvtAddEvents(5);
m = chEvtWaitOne(ALL_EVENTS);
test_assert(1, m == 1, "single event error");
m = chEvtWaitOne(ALL_EVENTS);
test_assert(2, m == 4, "single event error");
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(3, m == 0, "stuck event");
/*
* Test on chEvtWaitOne() with wait.
*/
test_wait_tick();
target_time = chVTGetSystemTime() + MS2ST(50);
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX() - 1,
thread1, chThdGetSelfX());
m = chEvtWaitOne(ALL_EVENTS);
test_assert_time_window(4, target_time, target_time + ALLOWED_DELAY);
test_assert(5, m == 1, "single event error");
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(6, m == 0, "stuck event");
test_wait_threads();
/*
* Test on chEvtWaitAny() without wait.
*/
chEvtAddEvents(5);
m = chEvtWaitAny(ALL_EVENTS);
test_assert(7, m == 5, "unexpected pending bit");
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(8, m == 0, "stuck event");
/*
* Test on chEvtWaitAny() with wait.
*/
test_wait_tick();
target_time = chVTGetSystemTime() + MS2ST(50);
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX() - 1,
thread1, chThdGetSelfX());
m = chEvtWaitAny(ALL_EVENTS);
test_assert_time_window(9, target_time, target_time + ALLOWED_DELAY);
test_assert(10, m == 1, "single event error");
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(11, m == 0, "stuck event");
test_wait_threads();
/*
* Test on chEvtWaitAll().
*/
chEvtObjectInit(&es1);
chEvtObjectInit(&es2);
chEvtRegisterMask(&es1, &el1, 1);
chEvtRegisterMask(&es2, &el2, 4);
test_wait_tick();
target_time = chVTGetSystemTime() + MS2ST(50);
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX() - 1,
thread2, "A");
m = chEvtWaitAll(5);
test_assert_time_window(12, target_time, target_time + ALLOWED_DELAY);
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(13, m == 0, "stuck event");
test_wait_threads();
chEvtUnregister(&es1, &el1);
chEvtUnregister(&es2, &el2);
test_assert(14, !chEvtIsListeningI(&es1), "stuck listener");
test_assert(15, !chEvtIsListeningI(&es2), "stuck listener");
}
static THD_FUNCTION(stream, arg)
{
(void)arg;
chRegSetThreadName("stream");
static event_listener_t sensor_event_listener;
chEvtRegisterMaskWithFlags(&sensor_events, &sensor_event_listener,
(eventmask_t)ONBOARDSENSOR_EVENT,
(eventflags_t)SENSOR_EVENT_MPU6000 | SENSOR_EVENT_H3LIS331DL | SENSOR_EVENT_HMC5883L | SENSOR_EVENT_MS5611);
static char dtgrm[100];
static cmp_mem_access_t mem;
static cmp_ctx_t cmp;
while (1) {
eventmask_t events = chEvtWaitAny(ONBOARDSENSOR_EVENT);
if (events & ONBOARDSENSOR_EVENT) {
eventflags_t event_flags = chEvtGetAndClearFlags(&sensor_event_listener);
if (event_flags & SENSOR_EVENT_MPU6000) {
rate_gyro_sample_t gyro;
accelerometer_sample_t acc;
onboard_sensor_get_mpu6000_gyro_sample(&gyro);
onboard_sensor_get_mpu6000_acc_sample(&acc);
cmp_mem_access_init(&cmp, &mem, dtgrm, sizeof(dtgrm));
bool err = false;
err = err || !msg_header_write(&cmp, "imu");
err = err || !cmp_write_map(&cmp, 3);
err = err || !CMP_WRITE_C_STRING(cmp, "gyro");
err = err || !cmp_write_array(&cmp, 3);
err = err || !cmp_write_float(&cmp, gyro.rate[0]);
err = err || !cmp_write_float(&cmp, gyro.rate[1]);
err = err || !cmp_write_float(&cmp, gyro.rate[2]);
err = err || !CMP_WRITE_C_STRING(cmp, "acc");
err = err || !cmp_write_array(&cmp, 3);
err = err || !cmp_write_float(&cmp, acc.acceleration[0]);
err = err || !cmp_write_float(&cmp, acc.acceleration[1]);
err = err || !cmp_write_float(&cmp, acc.acceleration[2]);
err = err || !CMP_WRITE_C_STRING(cmp, "time");
err = err || !cmp_write_uint(&cmp, gyro.timestamp);
if (!err) {
datagram_message_send(dtgrm, cmp_mem_access_get_pos(&mem));
}
cmp_mem_access_init(&cmp, &mem, dtgrm, sizeof(dtgrm));
err = false;
float att[4];
attitude_determination_get_attitude(att);
err = err || !msg_header_write(&cmp, "att");
err = err || !cmp_write_array(&cmp, 4);
err = err || !cmp_write_float(&cmp, att[0]);
err = err || !cmp_write_float(&cmp, att[1]);
err = err || !cmp_write_float(&cmp, att[2]);
err = err || !cmp_write_float(&cmp, att[3]);
if (!err) {
datagram_message_send(dtgrm, cmp_mem_access_get_pos(&mem));
}
}
if (event_flags & SENSOR_EVENT_HMC5883L) {
magnetometer_sample_t magnetometer;
onboard_sensor_get_hmc5883l_mag_sample(&magnetometer);
cmp_mem_access_init(&cmp, &mem, dtgrm, sizeof(dtgrm));
bool err = false;
err = err || !msg_header_write(&cmp, "mag");
err = err || !cmp_write_map(&cmp, 2);
err = err || !CMP_WRITE_C_STRING(cmp, "field");
err = err || !cmp_write_array(&cmp, 3);
err = err || !cmp_write_float(&cmp, magnetometer.magnetic_field[0]);
err = err || !cmp_write_float(&cmp, magnetometer.magnetic_field[1]);
err = err || !cmp_write_float(&cmp, magnetometer.magnetic_field[2]);
err = err || !CMP_WRITE_C_STRING(cmp, "time");
err = err || !cmp_write_uint(&cmp, magnetometer.timestamp);
if (!err) {
datagram_message_send(dtgrm, cmp_mem_access_get_pos(&mem));
}
}
if (event_flags & SENSOR_EVENT_H3LIS331DL) {
accelerometer_sample_t acc;
onboard_sensor_get_h3lis331dl_acc_sample(&acc);
cmp_mem_access_init(&cmp, &mem, dtgrm, sizeof(dtgrm));
bool err = false;
err = err || !msg_header_write(&cmp, "hi_acc");
err = err || !cmp_write_map(&cmp, 2);
err = err || !CMP_WRITE_C_STRING(cmp, "acc");
err = err || !cmp_write_array(&cmp, 3);
err = err || !cmp_write_float(&cmp, acc.acceleration[0]);
err = err || !cmp_write_float(&cmp, acc.acceleration[1]);
err = err || !cmp_write_float(&cmp, acc.acceleration[2]);
err = err || !CMP_WRITE_C_STRING(cmp, "time");
err = err || !cmp_write_uint(&cmp, acc.timestamp);
if (!err) {
datagram_message_send(dtgrm, cmp_mem_access_get_pos(&mem));
}
}
if (event_flags & SENSOR_EVENT_MS5611) {
barometer_sample_t baro;
onboard_sensor_get_ms5511_baro_sample(&baro);
cmp_mem_access_init(&cmp, &mem, dtgrm, sizeof(dtgrm));
bool err = false;
err = err || !msg_header_write(&cmp, "baro");
err = err || !cmp_write_map(&cmp, 3);
err = err || !CMP_WRITE_C_STRING(cmp, "static_press");
err = err || !cmp_write_float(&cmp, baro.pressure);
err = err || !CMP_WRITE_C_STRING(cmp, "air_temp");
err = err || !cmp_write_float(&cmp, baro.temperature);
err = err || !CMP_WRITE_C_STRING(cmp, "time");
err = err || !cmp_write_uint(&cmp, baro.timestamp);
if (!err) {
datagram_message_send(dtgrm, cmp_mem_access_get_pos(&mem));
}
}
} // ONBOARDSENSOR_EVENT
}
}
eventmask_t BaseThread::waitAnyEvent(eventmask_t ewmask) {
return chEvtWaitAny(ewmask);
}
/**
* @brief LWIP handling thread.
*
* @param[in] p pointer to a @p lwipthread_opts structure or @p NULL
* @return The function does not return.
*/
msg_t lwip_thread(void *p) {
event_timer_t evt;
event_listener_t el0, el1;
struct ip_addr ip, gateway, netmask;
static struct netif thisif;
static const MACConfig mac_config = {thisif.hwaddr};
chRegSetThreadName("lwipthread");
/* Initializes the thing.*/
tcpip_init(NULL, NULL);
/* TCP/IP parameters, runtime or compile time.*/
if (p) {
struct lwipthread_opts *opts = p;
unsigned i;
for (i = 0; i < 6; i++)
thisif.hwaddr[i] = opts->macaddress[i];
ip.addr = opts->address;
gateway.addr = opts->gateway;
netmask.addr = opts->netmask;
}
else {
thisif.hwaddr[0] = LWIP_ETHADDR_0;
thisif.hwaddr[1] = LWIP_ETHADDR_1;
thisif.hwaddr[2] = LWIP_ETHADDR_2;
thisif.hwaddr[3] = LWIP_ETHADDR_3;
thisif.hwaddr[4] = LWIP_ETHADDR_4;
thisif.hwaddr[5] = LWIP_ETHADDR_5;
LWIP_IPADDR(&ip);
LWIP_GATEWAY(&gateway);
LWIP_NETMASK(&netmask);
}
macStart(ÐD1, &mac_config);
netif_add(&thisif, &ip, &netmask, &gateway, NULL, ethernetif_init, tcpip_input);
netif_set_default(&thisif);
netif_set_up(&thisif);
/* Setup event sources.*/
evtObjectInit(&evt, LWIP_LINK_POLL_INTERVAL);
evtStart(&evt);
chEvtRegisterMask(&evt.et_es, &el0, PERIODIC_TIMER_ID);
chEvtRegisterMask(macGetReceiveEventSource(ÐD1), &el1, FRAME_RECEIVED_ID);
chEvtAddEvents(PERIODIC_TIMER_ID | FRAME_RECEIVED_ID);
/* Goes to the final priority after initialization.*/
chThdSetPriority(LWIP_THREAD_PRIORITY);
while (TRUE) {
eventmask_t mask = chEvtWaitAny(ALL_EVENTS);
if (mask & PERIODIC_TIMER_ID) {
bool_t current_link_status = macPollLinkStatus(ÐD1);
if (current_link_status != netif_is_link_up(&thisif)) {
if (current_link_status)
tcpip_callback_with_block((tcpip_callback_fn) netif_set_link_up,
&thisif, 0);
else
tcpip_callback_with_block((tcpip_callback_fn) netif_set_link_down,
&thisif, 0);
}
}
if (mask & FRAME_RECEIVED_ID) {
struct pbuf *p;
while ((p = low_level_input(&thisif)) != NULL) {
struct eth_hdr *ethhdr = p->payload;
switch (htons(ethhdr->type)) {
/* IP or ARP packet? */
case ETHTYPE_IP:
case ETHTYPE_ARP:
#if PPPOE_SUPPORT
/* PPPoE packet? */
case ETHTYPE_PPPOEDISC:
case ETHTYPE_PPPOE:
#endif /* PPPOE_SUPPORT */
/* full packet send to tcpip_thread to process */
if (thisif.input(p, &thisif) == ERR_OK)
break;
LWIP_DEBUGF(NETIF_DEBUG, ("ethernetif_input: IP input error\n"));
default:
pbuf_free(p);
}
}
}
}
return 0;
}