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; }