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, &rxmsg, TIME_IMMEDIATE) == RDY_OK) {
/* Process message.*/
palTogglePad(IOPORT3, GPIOC_LED);
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
return 0;
}
msg_t data_udp_receive_thread(void *p) {
void * arg __attribute__ ((unused)) = p;
err_t err;
struct netconn *conn;
ip_addr_t ip_addr_sensor;
chRegSetThreadName("data_udp_receive_thread");
IMU_A_IP_ADDR(&ip_addr_sensor);
/*
* Create a new UDP connection handle
*/
conn = netconn_new(NETCONN_UDP);
LWIP_ERROR("data_udp_receive_thread: invalid conn", (conn != NULL), return RDY_RESET;);
static msg_t can_tx(void * p) {
CANTxFrame txmsg;
(void)p;
chRegSetThreadName("transmitter");
txmsg.IDE = CAN_IDE_EXT;
txmsg.EID = 0x01234567;
txmsg.RTR = CAN_RTR_DATA;
txmsg.DLC = 8;
txmsg.data32[0] = 0x55AA55AA;
txmsg.data32[1] = 0x00FF00FF;
while (!chThdShouldTerminate()) {
canTransmit(&CAND1, &txmsg, MS2ST(100));
chThdSleepMilliseconds(500);
}
return 0;
}
static msg_t MmcReaderDmThread(void *sdp){
chRegSetThreadName("MmcReaderDm");
struct EventListener el_storage_request_count_dm;
struct EventListener el_storage_request_dm;
chEvtRegisterMask(&event_mavlink_oblique_storage_request_count_dm,
&el_storage_request_count_dm,
EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_COUNT_DM);
chEvtRegisterMask(&event_mavlink_oblique_storage_request_dm,
&el_storage_request_dm,
EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_DM);
eventmask_t evt = 0;
while (!chThdShouldTerminate()){
evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_COUNT_DM | EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_DM, MS2ST(50));
if (!mmcIsCardInserted(&MMCD1))
continue;
else{
switch (evt){
case(EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_COUNT_DM):
bnapStorageAcquire(&Storage);
mavlink_oblique_storage_count_struct.count = Storage.used;
bnapStorageRelease(&Storage);
chEvtBroadcastFlags(&event_mavlink_oblique_storage_count, EVMSK_MAVLINK_OBLIQUE_STORAGE_COUNT);
break;
case(EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_DM):
_oblique_storage_request_handler_dm(sdp);
break;
default:
break;
}
}
}
chEvtUnregister(&event_mavlink_oblique_storage_request_count_dm,
&el_storage_request_count_dm);
chEvtUnregister(&event_mavlink_oblique_storage_request_dm,
&el_storage_request_dm);
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 msg_t PowerKeeperThread(void *arg){
chRegSetThreadName("PowerKeeper");
(void)arg;
uint32_t batcap = 0; /* battery capacitance in A*mS */
uint32_t batfill = 0; /* battery filling in A*mS */
int32_t i = -1;
/* get current battery capacitance from parameter structure */
i = _key_index_search("BAT_cap");
if (i == -1)
chDbgPanic("key not found");
else
batcap = 3600 * ((uint32_t)floorf(global_data[i].value));
/* get battery fill in percents and calculate fill in A*mS*/
i = _key_index_search("BAT_fill");
if (i == -1)
chDbgPanic("key not found");
else
batfill = (batcap * (uint32_t)floorf(global_data[i].value)) / 100;
systime_t time = chTimeNow(); // T0
while (TRUE) {
time += MS2ST(PWR_CHECK_PERIOD); // Next deadline
raw_data.main_current = samples[ADC_CURRENT_SENS_OFFSET];
raw_data.main_voltage = samples[ADC_MAIN_SUPPLY_OFFSET];
raw_data.secondary_voltage = samples[ADC_6V_SUPPLY_OFFSET];
comp_data.main_current = get_comp_main_current(raw_data.main_current);
comp_data.secondary_voltage = get_comp_secondary_voltage(raw_data.secondary_voltage);
batfill -= (comp_data.main_current * PWR_CHECK_PERIOD) / 1000;
mavlink_sys_status_struct.battery_remaining = (batfill * 100) / batcap;
mavlink_sys_status_struct.current_battery = (uint16_t)(comp_data.main_current / 10);
mavlink_sys_status_struct.voltage_battery = comp_data.secondary_voltage;
chThdSleepUntil(time);
}
return 0;
}
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 );
}
}
}
}
msg_t qeipub_node(void *arg) {
Node node("qeipub");
Publisher<tQEIMsg> qei_pub;
systime_t time;
tQEIMsg *msgp;
(void) arg;
chRegSetThreadName("qeipub");
qeiStart(&QEI_DRIVER, &qeicfg);
qeiEnable (&QEI_DRIVER);
switch (stm32_id8()) {
case M1:
node.advertise(qei_pub, "qei1");
break;
case M2:
node.advertise(qei_pub, "qei2");
break;
case M3:
node.advertise(qei_pub, "qei3");
break;
default:
node.advertise(qei_pub, "qei");
break;
}
for (;;) {
time = chTimeNow();
int16_t delta = qeiUpdate(&QEI_DRIVER);
if (qei_pub.alloc(msgp)) {
msgp->timestamp.sec = 0;
msgp->timestamp.nsec = 0;
msgp->delta = delta;
qei_pub.publish(*msgp);
}
time += MS2ST(50);
chThdSleepUntil(time);
}
return CH_SUCCESS;
}
msg_t encoder_node(void *arg) {
Node node("encoder");
Publisher<tEncoderMsg> enc_pub;
systime_t time;
tEncoderMsg *msgp;
(void) arg;
chRegSetThreadName("encoder");
qeiStart(&QEI_DRIVER, &qeicfg);
qeiEnable (&QEI_DRIVER);
switch (stm32_id8()) {
case M1:
node.advertise(enc_pub, "encoder1");
break;
case M2:
node.advertise(enc_pub, "encoder2");
break;
case M3:
node.advertise(enc_pub, "encoder3");
break;
default:
node.advertise(enc_pub, "encoder");
break;
}
for (;;) {
time = chTimeNow();
if (enc_pub.alloc(msgp)) {
msgp->timestamp.sec = chTimeNow();
msgp->timestamp.nsec = chTimeNow();
msgp->delta = T2R(qeiUpdate(&QEI_DRIVER) * 100);
enc_pub.publish(*msgp);
}
time += MS2ST(10);
chThdSleepUntil(time);
}
return CH_SUCCESS;
}
static __attribute__((noreturn)) msg_t errorLedThd(void *arg)
{
(void) arg;
chRegSetThreadName("thdErrorLed");
while (1) {
#if PROTECT_DATA_WITH_MUTEX
chMtxLock(&mutex);
#endif
const uint32_t now = chTimeNow();
for (uint32_t ledIdx=0; ledIdx<LED_COUNT; ledIdx++) {
const LedPinParams *lpp = &ledPinParams[ledIdx];
const GpioLed *gl = &ledPins[ledIdx];
const uint32_t durationSinceSet = now - lpp->setTimestamp;
/* if ((now > 3000) && (ledIdx == ERROR_H407_LED)) { */
/* chprintf (chp, "durationSinceSet=%d lpp->dutyWidth=%d lpp->onWidth=%d\r\n", */
/* durationSinceSet, lpp->dutyWidth, lpp->onWidth) ; */
/* } */
if ((lpp->duration != ERROR_LED_TIME_INFINITE) && (durationSinceSet > lpp->duration)) {
bb_palWritePad (gl->gpio.gpio, gl->gpio.pin, gl->activeLow);
} else {
if (lpp->nbFlash == 0) {
bb_palWritePad (gl->gpio.gpio, gl->gpio.pin, gl->activeLow);
} else {
const uint16_t allPulseDuration = (((lpp->nbFlash - 1) * 2) +1) * lpp->onWidth;
const uint16_t durationSinceFirstPulse = durationSinceSet % lpp->dutyWidth;
if (durationSinceFirstPulse > allPulseDuration) {
bb_palWritePad (gl->gpio.gpio, gl->gpio.pin, gl->activeLow);
} else {
//bb_palWritePad (gl->gpio.gpio, gl->gpio.pin, !gl->activeLow);
bb_palWritePad (gl->gpio.gpio, gl->gpio.pin,
gl->activeLow ^ (!((durationSinceFirstPulse / lpp->onWidth) % 2)));
}
}
}
}
#if PROTECT_DATA_WITH_MUTEX
chMtxUnlock ();
#endif
chThdSleepMilliseconds (10);
}
}
static msg_t Thread1(void *arg) {
(void)arg;
chRegSetThreadName("blinker");
while (TRUE) {
palClearPad(GPIOF, GPIOF_LED4);
palSetPad(GPIOF, GPIOF_LED1);
chThdSleepMilliseconds(250);
palClearPad(GPIOF, GPIOF_LED1);
palSetPad(GPIOF, GPIOF_LED2);
chThdSleepMilliseconds(250);
palClearPad(GPIOF, GPIOF_LED2);
palSetPad(GPIOF, GPIOF_LED3);
chThdSleepMilliseconds(250);
palClearPad(GPIOF, GPIOF_LED3);
palSetPad(GPIOF, GPIOF_LED4);
chThdSleepMilliseconds(250);
}
}
CCM_FUNC static THD_FUNCTION(ThreadSER1, arg)
{
(void)arg;
uint8_t buffer[SERIAL_BUFFERS_SIZE/2];
size_t read;
chRegSetThreadName("MTS");
while(SD1.state != SD_READY) chThdSleepMilliseconds(10);
while (TRUE) {
read = sdReadTimeout(&SD1, buffer, 6, MS2ST(40));
if (read >= 6)
{
readMtsPackets(buffer);
}
}
return;
}
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 __attribute__((noreturn)) msg_t SerOutThr1(void *arg){
chRegSetThreadName("serial_out");
int8_t accel_x,accel_y,accel_z;
(void)arg;
while(TRUE){
chThdSleepMilliseconds(100);
chSysLockFromIsr();
accel_x = acceleration_x;
accel_y = acceleration_y;
accel_z = acceleration_z;
chSysUnlockFromIsr();
get_lx_from_cnts();
chprintf((BaseSequentialStream *)&SD1,
"accel: x:\t%d\ty:\t%d\tz:\t%d\tcolor_lx: y=%U\tr=%U\tg=%U\tb=%U\n\r",
accel_x,accel_y,accel_z,
y_lx_val,r_lx_val,g_lx_val,b_lx_val);
}
}
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 THD_FUNCTION(Thread1, arg) {
static uint8_t txbuf[5];
static uint8_t rxbuf[5];
(void)arg;
chRegSetThreadName("Blinker");
while (true) {
palSetPad(GPIOB, GPIOB_LED);
/* Send the Manufacturer and Device ID Read command */
txbuf[0] = 0x9F;
spiSelect(&SPID1);
spiExchange(&SPID1, sizeof(txbuf), txbuf, rxbuf);
spiUnselect(&SPID1);
chThdSleepMilliseconds(1000);
}
}
static THD_FUNCTION(uart_thread, arg)
{
(void)arg;
chRegSetThreadName("UART thread");
// KPA.
event_listener_t kpa_event_listener;
chEvtRegisterMaskWithFlags((event_source_t*)chnGetEventSource(&SD1), &kpa_event_listener, EVENT_MASK(1), CHN_INPUT_AVAILABLE);
struct writer kpa_writer;
writer_init(&kpa_writer, &SD1, uart_write);
// FBV.
event_listener_t fbv_event_listener;
chEvtRegisterMaskWithFlags((event_source_t*)chnGetEventSource(&SD6), &fbv_event_listener, EVENT_MASK(2), CHN_INPUT_AVAILABLE);
struct writer fbv_writer;
writer_init(&fbv_writer, &SD6, uart_write);
// Bridge-
struct bridge bridge;
bridge_init(&bridge, &kpa_writer, &fbv_writer);
uint8_t byte = 0;
while (true)
{
eventflags_t evt = chEvtWaitAnyTimeout(EVENT_MASK(1) | EVENT_MASK(2), MS2ST(1));
if (evt & EVENT_MASK(1))
{
chEvtGetAndClearFlags(&kpa_event_listener);
while (readByte(&SD1, &byte))
bridge_update_kpa(&bridge, byte);
}
if (evt & EVENT_MASK(2))
{
chEvtGetAndClearFlags(&fbv_event_listener);
while (readByte(&SD6, &byte))
bridge_update_fbv(&bridge, byte);
}
bridge_update_time(&bridge, clock_get_ms());
}
}
static THD_FUNCTION(ThreadButton, arg) {
(void) arg;
chRegSetThreadName("button");
while (TRUE) {
while (!palReadPad(GPIOA, GPIOA_BUTTON)) {
chThdSleepMilliseconds(1);
}
if (schema == MAXSCH) {
schema = 0;
} else {
schema++;
}
chprintf((BaseSequentialStream *)&SDU1, "Schema set to %d\r\n", schema);
chThdSleepMilliseconds(500);
}
return 0; // nevar forget
}
static THD_FUNCTION(Thread1, arg) {
(void)arg;
chRegSetThreadName("blinker");
while (true) {
palSetLine(LINE_LED1);
chThdSleepMilliseconds(50);
palSetLine(LINE_LED2);
chThdSleepMilliseconds(50);
palSetLine(LINE_LED3);
chThdSleepMilliseconds(200);
palClearLine(LINE_LED1);
chThdSleepMilliseconds(50);
palClearLine(LINE_LED2);
chThdSleepMilliseconds(50);
palClearLine(LINE_LED3);
chThdSleepMilliseconds(200);
}
}
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(ThreadMonitor, arg)
{
(void)arg;
chRegSetThreadName("Monitor");
uint32_t run_offset, irq_ticks = 0, total_ticks;
thread_t* tp = NULL;
DWT->CTRL |= DWT_CTRL_EXCEVTENA_Msk;
while (TRUE)
{
tp = chRegFirstThread();
do {
tp->runtime = 0;
tp->irqtime = 0;
tp = chRegNextThread(tp);
} while (tp != NULL);
irq_ticks = 0;
run_offset = DWT->CYCCNT;
/* Populate load data */
chThdSleepMilliseconds(1000);
/* Convert to systick time base */
total_ticks = (DWT->CYCCNT - run_offset) / (STM32_SYSCLK/CH_CFG_ST_FREQUENCY);
tp = chRegFirstThread();
do {
irq_ticks += tp->irqtime;
tp = chRegNextThread(tp);
} while (tp != NULL);
tp = chRegFirstThread();
do {
tp->pct = ((tp->runtime*10000)/total_ticks) | RUNNING(tp);
tp = chRegNextThread(tp);
} while (tp != NULL);
irq_pct = ((irq_ticks*10000)/total_ticks);
}
return;
}
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 msg_t PollLis3Thread(void *arg){
chRegSetThreadName("PollLis3");
(void)arg;
while (TRUE) {
accel_tx_data[0] = ACCEL_OUT_DATA | AUTO_INCREMENT_BIT; /* register address */
i2cAcquireBus(&I2CD1);
i2cMasterTransmitTimeout(&I2CD1, lis3_addr, accel_tx_data, 1, accel_rx_data, 6, MS2ST(4));
i2cReleaseBus(&I2CD1);
raw_data.xacc = accel_rx_data[0] + (accel_rx_data[1] << 8);
raw_data.yacc = accel_rx_data[2] + (accel_rx_data[3] << 8);
raw_data.zacc = accel_rx_data[4] + (accel_rx_data[5] << 8);
chThdSleepMilliseconds(50);
}
return 0;
}
int main(void) {
halInit();
chSysInit();
chRegSetThreadName("main");
LocalStatus = &M2RStatus;
/*sbp_state_init(&sbp_state);*/
rockblock_init();
dispatch_init();
chThdCreateStatic(waThreadHB, sizeof(waThreadHB), LOWPRIO,
ThreadHeartbeat, NULL);
/* Configure and enable the watchdog timer, stopped in debug halt */
DBGMCU->APB1FZ |= DBGMCU_APB1_FZ_DBG_IWDG_STOP;
IWDG->KR = 0x5555;
IWDG->PR = 3;
IWDG->KR = 0xCCCC;
m2serial_shell = m2r_shell_run;
M2SerialSD = &SD2;
chThdCreateStatic(waM2Serial, sizeof(waM2Serial), HIGHPRIO,
m2serial_thread, NULL);
chThdCreateStatic(waM2Status, sizeof(waM2Status), HIGHPRIO,
m2status_thread, NULL);
chThdCreateStatic(waThreadUblox, sizeof(waThreadUblox), NORMALPRIO,
ublox_thread, NULL);
chThdCreateStatic(waThreadRadio, sizeof(waThreadRadio), NORMALPRIO,
radio_thread, NULL);
chThdCreateStatic(waDispatch, sizeof(waDispatch), NORMALPRIO,
dispatch_thread, NULL);
chThdSetPriority(LOWPRIO);
chThdSleep(TIME_INFINITE);
return 0;
}
static msg_t sbp_thread(void *arg)
{
(void)arg;
chRegSetThreadName("SBP");
uart_state_msg.latency.avg = -1;
uart_state_msg.latency.lmin = 0;
uart_state_msg.latency.lmax = 0;
uart_state_msg.latency.current = -1;
while (TRUE) {
chThdSleepMilliseconds(10);
sbp_process_messages();
DO_EVERY(100,
uart_state_msg.uart_a.tx_throughput = usart_tx_throughput(&uarta_state.tx);
uart_state_msg.uart_a.rx_throughput = usart_rx_throughput(&uarta_state.rx);
uart_state_msg.uart_a.io_error_count = uarta_state.rx.errors + uarta_state.tx.errors;
uart_state_msg.uart_b.tx_throughput = usart_tx_throughput(&uartb_state.tx);
uart_state_msg.uart_b.rx_throughput = usart_rx_throughput(&uartb_state.rx);
uart_state_msg.uart_b.io_error_count = uartb_state.rx.errors + uartb_state.tx.errors;
uart_state_msg.uart_ftdi.tx_throughput = usart_tx_throughput(&ftdi_state.tx);
uart_state_msg.uart_ftdi.rx_throughput = usart_rx_throughput(&ftdi_state.rx);
uart_state_msg.uart_ftdi.io_error_count = ftdi_state.rx.errors + ftdi_state.tx.errors;
if (latency_count > 0) {
uart_state_msg.latency.avg = (s32) (latency_accum_ms / latency_count);
}
sbp_send_msg(SBP_MSG_UART_STATE, sizeof(msg_uart_state_t),
(u8*)&uart_state_msg);
uart_state_msg.uart_a.tx_buffer_level = 0;
uart_state_msg.uart_a.rx_buffer_level = 0;
uart_state_msg.uart_b.tx_buffer_level = 0;
uart_state_msg.uart_b.rx_buffer_level = 0;
uart_state_msg.uart_ftdi.tx_buffer_level = 0;
uart_state_msg.uart_ftdi.rx_buffer_level = 0;
log_obs_latency_tick();
);
}
__noreturn
static void PillThread(void *arg) {
chRegSetThreadName("Pill");
while(true) {
chThdSleepMilliseconds(PILL_CHECK_PERIOD_MS);
PillMgr.Check();
switch(PillMgr.State) {
case pillJustConnected:
// Uart.Printf("Pill: %d; %X\r", PillMgr.Pill.TypeInt32, PillMgr.Pill.AbilityMsk);
App.SignalEvt(EVT_PILL_CONNECTED);
break;
case pillJustDisconnected:
App.SignalEvt(EVT_PILL_DISCONNECTED);
// Uart.Printf("Pill Discon\r");
break;
case pillNoChange:
break;
}
} // while true
}
static THD_FUNCTION(Thread1, arg) {
(void)arg;
chRegSetThreadName("blinker");
while (true) {
palClearPad(GPIOC, GPIOC_LED1);
chThdSleepMilliseconds(250);
palSetPad(GPIOC, GPIOC_LED1);
palClearPad(GPIOC, GPIOC_LED2);
chThdSleepMilliseconds(250);
palSetPad(GPIOC, GPIOC_LED2);
palClearPad(GPIOC, GPIOC_LED3);
chThdSleepMilliseconds(250);
palSetPad(GPIOC, GPIOC_LED3);
palClearPad(GPIOC, GPIOC_LED4);
chThdSleepMilliseconds(250);
palSetPad(GPIOC, GPIOC_LED4);
}
}
/*
* ROS rosserial publisher thread.
*/
msg_t rosserial_pub_thread(void * arg) {
std_msgs::String str_msg;
ros::Publisher pub("chatter", &str_msg);
(void) arg;
chRegSetThreadName("rosserial_pub");
nh.initNode();
nh.advertise(pub);
for (;;) {
char hello[] = "Hello world!";
str_msg.data = hello;
pub.publish(&str_msg);
nh.spinOnce();
chThdSleepMilliseconds(500);
}
return CH_SUCCESS;
}
static msg_t hipThread(void *arg) {
chRegSetThreadName("hip9011 init");
// some time to let the hardware start
enginePins.hipCs.setValue(true);
chThdSleepMilliseconds(100);
enginePins.hipCs.setValue(false);
chThdSleepMilliseconds(100);
enginePins.hipCs.setValue(true);
while (true) {
chThdSleepMilliseconds(100);
if (needToInit) {
hipStartupCode();
needToInit = false;
}
}
return -1;
}
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);
while(!chThdShouldTerminate()) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0)
continue;
while (canReceive(cip->canp, CAN_ANY_MAILBOX,
&rxmsg, TIME_IMMEDIATE) == RDY_OK) {
/* Process message.*/
palTogglePad(GPIO0, cip->led);
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
return 0;
}
