Example #1
0
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;
}
Example #2
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;);
Example #3
0
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;
}
Example #4
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;
}
Example #5
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;
}
Example #6
0
File: adc_local.c Project: mcu786/u
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 );
            }
        }
    }
}
Example #8
0
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;
}
Example #9
0
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;
}
Example #10
0
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);
  }
}
Example #11
0
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);
  }
}
Example #12
0
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;
}
Example #13
0
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);
	}
}
Example #14
0
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); 
    }
}
Example #15
0
File: main.c Project: mabl/ChibiOS
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);
}
Example #16
0
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);
  }
}
Example #17
0
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());
  }
}
Example #18
0
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
}
Example #19
0
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);
  }
}
Example #20
0
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;
}
Example #21
0
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;
}
Example #22
0
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;
			}
		}
	}
}
Example #23
0
File: lis3.c Project: mcu786/volat3
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;
}
Example #24
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;
}
Example #25
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();
    );
  }
Example #26
0
__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
}
Example #27
0
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;
}
Example #29
0
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;
}
Example #30
0
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;
}