예제 #1
0
파일: main.cpp 프로젝트: Kreyl/nute
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);
    }
}
예제 #2
0
파일: Node.cpp 프로젝트: ngocphu811/R2MW
bool Node::remoteSubscribe(RemotePublisher * pub, LocalSubscriber * sub) {
	LocalSubscriber * p = _subscribers;
	eventid_t eid = 0;

	if (p == NULL) {
		_subscribers = sub;
		sub->subscribe(pub, EVENT_MASK(eid));
		return true;
	}


	// FIXME giusto farlo col sizeof()?
	while ((++eid < sizeof(eventid_t) * 4) && (p->next() != NULL)) {
		// FIXME ?
		p = p->next();
	}

	if (eid == 32) {
		return false;
	}

	p->next(sub);
	sub->subscribe(pub, EVENT_MASK(eid));

	return true;
}
예제 #3
0
static THD_FUNCTION(thSerEcho, arg)
{
  (void)arg;
  chRegSetThreadName("SerEcho");
  event_listener_t elSerData;
  eventflags_t flags;
  chEvtRegisterMask((event_source_t *)chnGetEventSource(&SD1), &elSerData, EVENT_MASK(1));

  while (!chThdShouldTerminateX())
  {
     chEvtWaitOneTimeout(EVENT_MASK(1), MS2ST(10));
     flags = chEvtGetAndClearFlags(&elSerData);
     if (flags & CHN_INPUT_AVAILABLE)
     {
        msg_t charbuf;
        do
        {
           charbuf = chnGetTimeout(&SD1, TIME_IMMEDIATE);
           if ( charbuf != Q_TIMEOUT )
           {
             chSequentialStreamPut(&SD1, charbuf);
           }
        }
        while (charbuf != Q_TIMEOUT);
     }
  }
}
예제 #4
0
파일: b3_shell.c 프로젝트: ac942/avionics14
static void cmd_gps_passthrough(BaseSequentialStream *chp, int argc, char *argv[]) {
    (void)argc;
    (void)argv;
    static const SerialConfig sc = {
        9600, 0, USART_CR2_STOP1_BITS | USART_CR2_LINEN, 0};
    sdStart(&SD1, &sc);
    EventListener elSerData;
    flagsmask_t flags;
    chEvtRegisterMask(chnGetEventSource(&SD1), &elSerData, EVENT_MASK(1));

    while (TRUE)
    {
       chEvtWaitOneTimeout(EVENT_MASK(1), MS2ST(10));
       flags = chEvtGetAndClearFlags(&elSerData);
       if (flags & CHN_INPUT_AVAILABLE)
       {
          msg_t charbuf;
          do
          {
             charbuf = chnGetTimeout(&SD1, TIME_IMMEDIATE);
             if ( charbuf != Q_TIMEOUT )
             {
               chSequentialStreamPut(chp, charbuf);
             }
          }
          while (charbuf != Q_TIMEOUT);
       }
    }
}
예제 #5
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;
}
예제 #6
0
파일: Node.cpp 프로젝트: ngocphu811/R2MW
void Node::spin(systime_t timeout) {
	eventmask_t mask;
	eventid_t eid;
	LocalSubscriber * sub;

	mask = chEvtWaitAnyTimeout(ALL_EVENTS, timeout);

	if (mask == 0) {
		return;
	}

	eid = 0;
	sub = _subscribers;
	while (sub && mask) {
		if (mask & EVENT_MASK(eid)) {
			mask &= ~EVENT_MASK(eid);

			if (sub->callback()) {
				BaseMessage * msg;
				while ((msg = sub->get()) != NULL) {
					sub->callback(msg);
					sub->release(msg);
				}
			}
		}
		eid++;
		sub = sub->next();
	}
}
예제 #7
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;
}
예제 #8
0
파일: autopilot.c 프로젝트: mcu786/u
/* прием и обработка комманд с земли*/
void process_cmd(mavlink_command_long_t *mavlink_command_long_struct){

  /* all this flags defined in MAV_CMD enum */
  switch(mavlink_command_long_struct->command){
  case MAV_CMD_DO_SET_MODE:
    /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty|  */
    mavlink_system_struct.mode = mavlink_command_long_struct->param1;
    command_accepted();
    break;

  /*
   * (пере)запуск калибровки
   */
  case MAV_CMD_PREFLIGHT_CALIBRATION:
     if (mavlink_system_struct.mode != MAV_MODE_PREFLIGHT){
      command_denied();
      return;
    }
    else{
      handle_calibration_cmd(mavlink_command_long_struct);
    }
    break;


  case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
    /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty|  */
    if (mavlink_system_struct.mode != MAV_MODE_PREFLIGHT){
      command_denied();
      return;
    }
    else{
      chEvtBroadcastFlags(&init_event, EVENT_MASK(SIGHALT_EVID));
      command_accepted();
    }
    break;
    /**
     * Команды для загрузки/вычитки параметров из EEPROM
     */
    case MAV_CMD_PREFLIGHT_STORAGE:
      if (mavlink_system_struct.mode != MAV_MODE_PREFLIGHT)
        return;

      if (mavlink_command_long_struct->param1 == 0)
        load_params_from_eeprom();
      else if (mavlink_command_long_struct->param1 == 1)
        save_params_to_eeprom();

      if (mavlink_command_long_struct->param2 == 0)
        load_mission_from_eeprom();
      else if (mavlink_command_long_struct->param2 == 1)
        save_mission_to_eeprom();

      break;

  default:
    return;
    break;
  }
}
예제 #9
0
void mpu6050_interrupt_handler(EXTDriver *extp, expchannel_t channel) {
		(void)extp;
		(void)channel;

		chSysLockFromIsr();
		chEvtBroadcastFlagsI(&imu_event, EVENT_MASK(0));
		chSysUnlockFromIsr();
}
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 );
            }
        }
    }
}
예제 #11
0
파일: main.c 프로젝트: rasmartins/fbv2kpa
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());
  }
}
예제 #12
0
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();
    }
}
void GKI_destroy_task(UINT8 task_id)
{
#if ( FALSE == GKI_PTHREAD_JOINABLE )
        int i = 0;
#else
        int result;
#endif
    if (gki_cb.com.OSRdyTbl[task_id] != TASK_DEAD)
    {
        gki_cb.com.OSRdyTbl[task_id] = TASK_DEAD;

        /* paranoi settings, make sure that we do not execute any mailbox events */
        gki_cb.com.OSWaitEvt[task_id] &= ~(TASK_MBOX_0_EVT_MASK|TASK_MBOX_1_EVT_MASK|
                                            TASK_MBOX_2_EVT_MASK|TASK_MBOX_3_EVT_MASK);

#if (GKI_NUM_TIMERS > 0)
        gki_cb.com.OSTaskTmr0R[task_id] = 0;
        gki_cb.com.OSTaskTmr0 [task_id] = 0;
#endif

#if (GKI_NUM_TIMERS > 1)
        gki_cb.com.OSTaskTmr1R[task_id] = 0;
        gki_cb.com.OSTaskTmr1 [task_id] = 0;
#endif

#if (GKI_NUM_TIMERS > 2)
        gki_cb.com.OSTaskTmr2R[task_id] = 0;
        gki_cb.com.OSTaskTmr2 [task_id] = 0;
#endif

#if (GKI_NUM_TIMERS > 3)
        gki_cb.com.OSTaskTmr3R[task_id] = 0;
        gki_cb.com.OSTaskTmr3 [task_id] = 0;
#endif

        GKI_send_event(task_id, EVENT_MASK(GKI_SHUTDOWN_EVT));

#if ( FALSE == GKI_PTHREAD_JOINABLE )
        i = 0;

        while ((gki_cb.com.OSWaitEvt[task_id] != 0) && (++i < 10))
            usleep(100 * 1000);
#else
        result = pthread_join( gki_cb.os.thread_id[task_id], NULL );
        if ( result < 0 )
        {
            GKI_ERROR_LOG( "pthread_join() FAILED: result: %d", result );
        }
#endif
        GKI_exit_task(task_id);
        GKI_INFO( "GKI_shutdown(): task [%s] terminated\n", gki_cb.com.OSTName[task_id]);
    }
}
/*******************************************************************************
**
** Function         GKI_isend_msg
**
** Description      Called from interrupt context to send a buffer to a task
**
** Returns          Nothing
**
*******************************************************************************/
void GKI_isend_msg (UINT8 task_id, UINT8 mbox, void *msg)
{
    BUFFER_HDR_T    *p_hdr;
    tGKI_COM_CB *p_cb = &gki_cb.com;

    /* If task non-existant or not started, drop buffer */
    if ((task_id >= GKI_MAX_TASKS) || (mbox >= NUM_TASK_MBOX) || (p_cb->OSRdyTbl[task_id] == TASK_DEAD))
    {
        GKI_exception(GKI_ERROR_SEND_MSG_BAD_DEST, "Sending to unknown dest");
        GKI_freebuf (msg);
        return;
    }

#if (GKI_ENABLE_BUF_CORRUPTION_CHECK == TRUE)
    if (gki_chk_buf_damage(msg))
    {
        GKI_exception(GKI_ERROR_BUF_CORRUPTED, "Send - Buffer corrupted");
        return;
    }
#endif

#if (GKI_ENABLE_OWNER_CHECK == TRUE)
    if (gki_chk_buf_owner(msg))
    {
        GKI_exception(GKI_ERROR_NOT_BUF_OWNER, "Send by non-owner");
        return;
    }
#endif

    p_hdr = (BUFFER_HDR_T *) ((UINT8 *) msg - BUFFER_HDR_SIZE);

    if (p_hdr->status != BUF_STATUS_UNLINKED)
    {
        GKI_exception(GKI_ERROR_SEND_MSG_BUF_LINKED, "Send - buffer linked");
        return;
    }

    if (p_cb->OSTaskQFirst[task_id][mbox])
        p_cb->OSTaskQLast[task_id][mbox]->p_next = p_hdr;
    else
        p_cb->OSTaskQFirst[task_id][mbox] = p_hdr;

    p_cb->OSTaskQLast[task_id][mbox] = p_hdr;

    p_hdr->p_next = NULL;
    p_hdr->status = BUF_STATUS_QUEUED;
    p_hdr->task_id = task_id;

    GKI_isend_event(task_id, (UINT16)EVENT_MASK(mbox));

    return;
}
예제 #15
0
파일: main.c 프로젝트: pfhnr/stm32
/*! \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;
}
예제 #16
0
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;
}
예제 #17
0
파일: chevents.c 프로젝트: hmchen1/ChibiOS
/**
 * @brief   Invokes the event handlers associated to an event flags mask.
 *
 * @param[in] events    mask of events to be dispatched
 * @param[in] handlers  an array of @p evhandler_t. The array must have size
 *                      equal to the number of bits in eventmask_t.
 *
 * @api
 */
void chEvtDispatch(const evhandler_t *handlers, eventmask_t events) {
  eventid_t eid;

  chDbgCheck(handlers != NULL);

  eid = 0;
  while (events) {
    if (events & EVENT_MASK(eid)) {
      chDbgAssert(handlers[eid] != NULL, "null handler");
      events &= ~EVENT_MASK(eid);
      handlers[eid](eid);
    }
    eid++;
  }
}
예제 #18
0
파일: main.c 프로젝트: CInsights/stm32
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;
}
예제 #19
0
파일: mma8451.c 프로젝트: mcu786/u
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;
}
예제 #20
0
파일: data_udp.c 프로젝트: wa2tqi/stm32
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;
}
예제 #21
0
msg_t WebThread(void *p) {
  EvTimer evt1, evt2;
  EventListener el0, el1, el2;
  uip_ipaddr_t ipaddr;

  (void)p;

  /*
   * Event sources setup.
   */
  chEvtRegister(macGetReceiveEventSource(&ETHD1), &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(&ETHD1, &mac_config);
  (void)macPollLinkStatus(&ETHD1);

  /*
   * 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;
}
예제 #22
0
void AP_IOMCU_FW::update()
{
    eventmask_t mask = chEvtWaitAnyTimeout(~0, chTimeMS2I(1));

    if (do_reboot && (AP_HAL::millis() > reboot_time)) {
        hal.scheduler->reboot(true);
        while (true) {}
    }

    if ((mask & EVENT_MASK(IOEVENT_PWM)) ||
        (last_safety_off != reg_status.flag_safety_off)) {
        last_safety_off = reg_status.flag_safety_off;
        pwm_out_update();
    }

    uint32_t now = AP_HAL::millis();

    // output SBUS if enabled
    if ((reg_setup.features & P_SETUP_FEATURES_SBUS1_OUT) &&
        reg_status.flag_safety_off &&
        now - sbus_last_ms >= sbus_interval_ms) {
        // output a new SBUS frame
        sbus_last_ms = now;
        sbus_out_write(reg_servo.pwm, IOMCU_MAX_CHANNELS);
    }

    // run remaining functions at 1kHz
    if (now != last_loop_ms) {
        last_loop_ms = now;
        heater_update();
        rcin_update();
        safety_update();
        rcout_mode_update();
        hal.rcout->timer_tick();
    }
}
예제 #23
0
파일: main.c 프로젝트: sdalu/ChibiOS
/*
 * Application entry point.
 */
int main(void) {
  thread_t *shelltp1 = NULL;
  thread_t *shelltp2 = NULL;
  event_listener_t shell_el;

  /*
   * 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 two serial-over-USB CDC drivers.
   */
  sduObjectInit(&SDU1);
  sduStart(&SDU1, &serusbcfg1);
  sduObjectInit(&SDU2);
  sduStart(&SDU2, &serusbcfg2);

  /*
   * 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(serusbcfg1.usbp);
  chThdSleepMilliseconds(1500);
  usbStart(serusbcfg1.usbp, &usbcfg);
  usbConnectBus(serusbcfg1.usbp);

  /*
   * Shell manager initialization.
   * Event zero is shell exit.
   */
  shellInit();
  chEvtRegister(&shell_terminated, &shell_el, 0);

  /*
   * Creates the blinker thread.
   */
  chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);

  /*
   * Normal main() thread activity, managing two shells.
   */
  while (true) {
    if (SDU1.config->usbp->state == USB_ACTIVE) {
      /* Starting shells.*/
      if (shelltp1 == NULL) {
        shelltp1 = chThdCreateFromHeap(NULL, SHELL_WA_SIZE,
                                       "shell1", NORMALPRIO + 1,
                                       shellThread, (void *)&shell_cfg1);
      }
      if (shelltp2 == NULL) {
        shelltp2 = chThdCreateFromHeap(NULL, SHELL_WA_SIZE,
                                       "shell2", NORMALPRIO + 1,
                                       shellThread, (void *)&shell_cfg2);
      }

      /* Waiting for an exit event then freeing terminated shells.*/
      chEvtWaitAny(EVENT_MASK(0));
      if (chThdTerminatedX(shelltp1)) {
        chThdFreeToHeap(shelltp1);
        shelltp1 = NULL;
      }
      if (chThdTerminatedX(shelltp2)) {
        chThdFreeToHeap(shelltp2);
        shelltp2 = NULL;
      }
    }
    else {
      chThdSleepMilliseconds(1000);
    }
  }
}
예제 #24
0
/**
 * @brief Main estimation thread.
 *
 * @param[in/out] arg   Unused.
 */
static THD_FUNCTION(ThreadEstimation, arg)
{
    (void)arg;

    chRegSetThreadName("Estimation");

    tp = chThdGetSelfX();

    /* Initialization data */
    //uint32_t i;
    //quaternion_t q_init = {1.0f, 0.0f, 0.0f, 0.0f};
    //vector3f_t am, wb_init = {0.0f, 0.0f, 0.0f}; //, acc_init, mag_init;

    /* Event registration for new sensor data */
    event_listener_t el;

    /* Register to new data from accelerometer and gyroscope */
    chEvtRegisterMaskWithFlags(ptrGetNewDataEventSource(),
                      &el,
                      EVENT_MASK(0),
                      ACCGYRO_DATA_AVAILABLE_EVENTMASK |
                      MAG_DATA_AVAILABLE_EVENTMASK |
                      BARO_DATA_AVAILABLE_EVENTMASK);

    /* Force an initialization of the estimation */
    //chEvtAddEvents(ESTIMATION_RESET_EVENTMASK);

    //AttitudeEstimationInit(&states, &data, &q_init, &wb_init);
    vInitializeMotionCaptureEstimator(&states);

    while(1)
    {
        //if (isnan(states.q.q0) || isnan(states.q.q1) || isnan(states.q.q2) || isnan(states.q.q3) ||
        //    isnan(states.w.x) || isnan(states.w.y) || isnan(states.w.z) ||
        //    isnan(states.wb.x) || isnan(states.wb.y) || isnan(states.wb.z))
        //    AttitudeEstimationInit(&states, &data, &q_init, &wb_init);

        /* Check if there has been a request to reset the filter */
        //if (chEvtWaitOneTimeout(ESTIMATION_RESET_EVENTMASK, TIME_IMMEDIATE))
       // {
            /* Initialize the estimation */
            //AttitudeEstimationInit(&states, &data, &q_init, &wb_init);
        //}

        /* Wait for new measurement data */
        chEvtWaitAny(ALL_EVENTS);

        eventflags_t flags = chEvtGetAndClearFlags(&el);

        if (flags & ACCGYRO_DATA_AVAILABLE_EVENTMASK)
        {

            /* Get sensor data */
            GetIMUData(&imu_data);

            /* Run estimation */
            vInnovateMotionCaptureEstimator(&states,
                                            &imu_data,
                                            SENSOR_ACCGYRO_DT,
                                            0.0007f);

            /*InnovateAttitudeEKF(&states,
                                &data,
                                imu_data.gyroscope,
                                imu_data.accelerometer,
                                imu_data.magnetometer,
                                0.0f,
                                0.0f,
                                ESTIMATION_DT);*/

            //states.w.x = -imu_data.gyroscope[0];
            //states.w.y = -imu_data.gyroscope[1];
            //states.w.z = imu_data.gyroscope[2];

            //am.x = -imu_data.accelerometer[0];
            //am.y = -imu_data.accelerometer[1];
            //am.z = imu_data.accelerometer[2];

            //states.q = MadgwickAHRSupdateIMU(states.w,
            //                                 am,
            //                                 states.q,
            //                                 0.15f,
            //                                 dt);

            /* Broadcast new estimation available */
            chEvtBroadcastFlags(&estimation_events_es,
                                ESTIMATION_NEW_ESTIMATION_EVENTMASK);

        }
    }
}
예제 #25
0
#include "ui_widget.hpp"
#include "ui_painter.hpp"

#include "portapack.hpp"
#include "portapack_shared_memory.hpp"

#include "message.hpp"

#include "touch.hpp"

#include "ch.h"

#include <cstdint>

constexpr auto EVT_MASK_RTC_TICK		= EVENT_MASK(0);
constexpr auto EVT_MASK_LCD_FRAME_SYNC	= EVENT_MASK(1);
constexpr auto EVT_MASK_SWITCHES		= EVENT_MASK(3);
constexpr auto EVT_MASK_ENCODER			= EVENT_MASK(4);
constexpr auto EVT_MASK_TOUCH			= EVENT_MASK(5);
constexpr auto EVT_MASK_APPLICATION		= EVENT_MASK(6);
constexpr auto EVT_MASK_CAPTURE_THREAD	= EVENT_MASK(7);

class EventDispatcher {
public:
	EventDispatcher(
		ui::Widget* const top_widget,
		ui::Context& context
	);

	void run();
예제 #26
0
void GKI_shutdown(void)
{
    UINT8 task_id;
    volatile int    *p_run_cond = &gki_cb.os.no_timer_suspend;
    int     oldCOnd = 0;
#if ( FALSE == GKI_PTHREAD_JOINABLE )
    int i = 0;
#else
    int result;
#endif

    /* release threads and set as TASK_DEAD. going from low to high priority fixes
     * GKI_exception problem due to btu->hci sleep request events  */
    for (task_id = GKI_MAX_TASKS; task_id > 0; task_id--)
    {
        if (gki_cb.com.OSRdyTbl[task_id - 1] != TASK_DEAD)
        {
            gki_cb.com.OSRdyTbl[task_id - 1] = TASK_DEAD;

            /* paranoi settings, make sure that we do not execute any mailbox events */
            gki_cb.com.OSWaitEvt[task_id-1] &= ~(TASK_MBOX_0_EVT_MASK|TASK_MBOX_1_EVT_MASK|
                                                TASK_MBOX_2_EVT_MASK|TASK_MBOX_3_EVT_MASK);
            GKI_send_event(task_id - 1, EVENT_MASK(GKI_SHUTDOWN_EVT));

#if ( FALSE == GKI_PTHREAD_JOINABLE )
            i = 0;

            while ((gki_cb.com.OSWaitEvt[task_id - 1] != 0) && (++i < 10))
                usleep(100 * 1000);
#else
            /* wait for proper Arnold Schwarzenegger task state */
            result = pthread_join( gki_cb.os.thread_id[task_id-1], NULL );
            if ( result < 0 )
            {
                GKI_TRACE_1( "pthread_join() FAILED: result: %d", result );
            }
#endif
            GKI_TRACE_1( "GKI_shutdown(): task %s dead", gki_cb.com.OSTName[task_id]);
            GKI_exit_task(task_id - 1);
        }
    }

    /* Destroy mutex and condition variable objects */
    pthread_mutex_destroy(&gki_cb.os.GKI_mutex);
    /*    pthread_mutex_destroy(&GKI_sched_mutex); */
#if (GKI_DEBUG == TRUE)
    pthread_mutex_destroy(&gki_cb.os.GKI_trace_mutex);
#endif
    /*    pthread_mutex_destroy(&thread_delay_mutex);
     pthread_cond_destroy (&thread_delay_cond); */
#if ( FALSE == GKI_PTHREAD_JOINABLE )
    i = 0;
#endif

#ifdef NO_GKI_RUN_RETURN
    shutdown_timer = 1;
#endif
    if (gki_cb.os.gki_timer_wake_lock_on)
    {
        GKI_TRACE_0("GKI_shutdown :  release_wake_lock(brcm_btld)");
        release_wake_lock(WAKE_LOCK_ID);
        gki_cb.os.gki_timer_wake_lock_on = 0;
    }
    oldCOnd = *p_run_cond;
    *p_run_cond = GKI_TIMER_TICK_EXIT_COND;
    if (oldCOnd == GKI_TIMER_TICK_STOP_COND)
        pthread_cond_signal( &gki_cb.os.gki_timer_cond );

}
void GKI_shutdown(void)
{
    UINT8 task_id;
#if ( FALSE == GKI_PTHREAD_JOINABLE )
    int i = 0;
#else
    int result;
#endif

#ifdef GKI_USE_DEFERED_ALLOC_BUF_POOLS
    gki_dealloc_free_queue();
#endif

    /* release threads and set as TASK_DEAD. going from low to high priority fixes
     * GKI_exception problem due to btu->hci sleep request events  */
    for (task_id = GKI_MAX_TASKS; task_id > 0; task_id--)
    {
        if (gki_cb.com.OSRdyTbl[task_id - 1] != TASK_DEAD)
        {
            gki_cb.com.OSRdyTbl[task_id - 1] = TASK_DEAD;

            /* paranoi settings, make sure that we do not execute any mailbox events */
            gki_cb.com.OSWaitEvt[task_id-1] &= ~(TASK_MBOX_0_EVT_MASK|TASK_MBOX_1_EVT_MASK|
                                                TASK_MBOX_2_EVT_MASK|TASK_MBOX_3_EVT_MASK);
            GKI_send_event(task_id - 1, EVENT_MASK(GKI_SHUTDOWN_EVT));

#if ( FALSE == GKI_PTHREAD_JOINABLE )
            i = 0;

            while ((gki_cb.com.OSWaitEvt[task_id - 1] != 0) && (++i < 10))
                usleep(100 * 1000);
#else
            result = pthread_join( gki_cb.os.thread_id[task_id-1], NULL );

            if ( result < 0 )
            {
                ALOGE( "pthread_join() FAILED: result: %d", result );
            }
#endif
            // GKI_ERROR_LOG( "GKI_shutdown(): task %s dead\n", gki_cb.com.OSTName[task_id]);
            GKI_exit_task(task_id - 1);
        }
    }

    /* Destroy mutex and condition variable objects */
    pthread_mutex_destroy(&gki_cb.os.GKI_mutex);

    /*    pthread_mutex_destroy(&GKI_sched_mutex); */
#if (GKI_DEBUG == TRUE)
    pthread_mutex_destroy(&gki_cb.os.GKI_trace_mutex);
#endif
    /*    pthread_mutex_destroy(&thread_delay_mutex);
     pthread_cond_destroy (&thread_delay_cond); */
#if ( FALSE == GKI_PTHREAD_JOINABLE )
    i = 0;
#endif

#ifdef NO_GKI_RUN_RETURN
    shutdown_timer = 1;
#endif
    if (g_GkiTimerWakeLockOn)
    {
        GKI_TRACE("GKI_shutdown :  release_wake_lock(brcm_btld)");
        release_wake_lock(WAKE_LOCK_ID);
        g_GkiTimerWakeLockOn = 0;
    }
}
예제 #28
0
/*******************************************************************************
**
** Function         GKI_wait
**
** Description      This function is called by tasks to wait for a specific
**                  event or set of events. The task may specify the duration
**                  that it wants to wait for, or 0 if infinite.
**
** Parameters:      flag -    (input) the event or set of events to wait for
**                  timeout - (input) the duration that the task wants to wait
**                                    for the specific events (in system ticks)
**
**
** Returns          the event mask of received events or zero if timeout
**
*******************************************************************************/
UINT16 GKI_wait (UINT16 flag, UINT32 timeout)
{
    UINT16 evt;
    UINT8 rtask;
    struct timespec abstime = { 0, 0 };
    int sec;
    int nano_sec;

    rtask = GKI_get_taskid();
    GKI_TRACE_3("GKI_wait %d %x %d", rtask, flag, timeout);
    if (rtask >= GKI_MAX_TASKS) {
        pthread_exit(NULL);
        return 0;
    }

    gki_pthread_info_t* p_pthread_info = &gki_pthread_info[rtask];
    if (p_pthread_info->pCond != NULL && p_pthread_info->pMutex != NULL) {
        int ret;
        GKI_TRACE_3("GKI_wait task=%i, pCond/pMutex = %x/%x", rtask, p_pthread_info->pCond, p_pthread_info->pMutex);
        ret = pthread_mutex_lock(p_pthread_info->pMutex);
        ret = pthread_cond_signal(p_pthread_info->pCond);
        ret = pthread_mutex_unlock(p_pthread_info->pMutex);
        p_pthread_info->pMutex = NULL;
        p_pthread_info->pCond = NULL;
    }
    gki_cb.com.OSWaitForEvt[rtask] = flag;

    /* protect OSWaitEvt[rtask] from modification from an other thread */
    pthread_mutex_lock(&gki_cb.os.thread_evt_mutex[rtask]);

#if 0 /* for clean scheduling we probably should always call pthread_cond_wait() */
    /* Check if anything in any of the mailboxes. There is a potential race condition where OSTaskQFirst[rtask]
     has been modified. however this should only result in addtional call to  pthread_cond_wait() but as
     the cond is met, it will exit immediately (depending on schedulling) */
    if (gki_cb.com.OSTaskQFirst[rtask][0])
    gki_cb.com.OSWaitEvt[rtask] |= TASK_MBOX_0_EVT_MASK;
    if (gki_cb.com.OSTaskQFirst[rtask][1])
    gki_cb.com.OSWaitEvt[rtask] |= TASK_MBOX_1_EVT_MASK;
    if (gki_cb.com.OSTaskQFirst[rtask][2])
    gki_cb.com.OSWaitEvt[rtask] |= TASK_MBOX_2_EVT_MASK;
    if (gki_cb.com.OSTaskQFirst[rtask][3])
    gki_cb.com.OSWaitEvt[rtask] |= TASK_MBOX_3_EVT_MASK;
#endif

    if (!(gki_cb.com.OSWaitEvt[rtask] & flag))
    {
        if (timeout)
        {
            //            timeout = GKI_MS_TO_TICKS(timeout);     /* convert from milliseconds to ticks */

            /* get current system time */
            //            clock_gettime(CLOCK_MONOTONIC, &currSysTime);
            //            abstime.tv_sec = currSysTime.time;
            //            abstime.tv_nsec = NANOSEC_PER_MILLISEC * currSysTime.millitm;
            clock_gettime(CLOCK_MONOTONIC, &abstime);

            /* add timeout */
            sec = timeout / 1000;
            nano_sec = (timeout % 1000) * NANOSEC_PER_MILLISEC;
            abstime.tv_nsec += nano_sec;
            if (abstime.tv_nsec > NSEC_PER_SEC)
            {
                abstime.tv_sec += (abstime.tv_nsec / NSEC_PER_SEC);
                abstime.tv_nsec = abstime.tv_nsec % NSEC_PER_SEC;
            }
            abstime.tv_sec += sec;

            pthread_cond_timedwait_monotonic(&gki_cb.os.thread_evt_cond[rtask],
                    &gki_cb.os.thread_evt_mutex[rtask], &abstime);

        }
        else
        {
            pthread_cond_wait(&gki_cb.os.thread_evt_cond[rtask], &gki_cb.os.thread_evt_mutex[rtask]);
        }

        /* TODO: check, this is probably neither not needed depending on phtread_cond_wait() implmentation,
         e.g. it looks like it is implemented as a counter in which case multiple cond_signal
         should NOT be lost! */
        // we are waking up after waiting for some events, so refresh variables
        // no need to call GKI_disable() here as we know that we will have some events as we've been waking up after condition pending or timeout
        if (gki_cb.com.OSTaskQFirst[rtask][0])
            gki_cb.com.OSWaitEvt[rtask] |= TASK_MBOX_0_EVT_MASK;
        if (gki_cb.com.OSTaskQFirst[rtask][1])
            gki_cb.com.OSWaitEvt[rtask] |= TASK_MBOX_1_EVT_MASK;
        if (gki_cb.com.OSTaskQFirst[rtask][2])
            gki_cb.com.OSWaitEvt[rtask] |= TASK_MBOX_2_EVT_MASK;
        if (gki_cb.com.OSTaskQFirst[rtask][3])
            gki_cb.com.OSWaitEvt[rtask] |= TASK_MBOX_3_EVT_MASK;

        if (gki_cb.com.OSRdyTbl[rtask] == TASK_DEAD)
        {
            gki_cb.com.OSWaitEvt[rtask] = 0;
            /* unlock thread_evt_mutex as pthread_cond_wait() does auto lock when cond is met */
            pthread_mutex_unlock(&gki_cb.os.thread_evt_mutex[rtask]);
            BT_TRACE_1( TRACE_LAYER_HCI, TRACE_TYPE_DEBUG, "GKI TASK_DEAD received. exit thread %d...", rtask );

            gki_cb.os.thread_id[rtask] = 0;
            pthread_exit(NULL);
            return (EVENT_MASK(GKI_SHUTDOWN_EVT));
        }
    }

    /* Clear the wait for event mask */
    gki_cb.com.OSWaitForEvt[rtask] = 0;

    /* Return only those bits which user wants... */
    evt = gki_cb.com.OSWaitEvt[rtask] & flag;

    /* Clear only those bits which user wants... */
    gki_cb.com.OSWaitEvt[rtask] &= ~flag;

    /* unlock thread_evt_mutex as pthread_cond_wait() does auto lock mutex when cond is met */
    pthread_mutex_unlock(&gki_cb.os.thread_evt_mutex[rtask]);
    GKI_TRACE_4("GKI_wait %d %x %d %x resumed", rtask, flag, timeout, evt);

    return (evt);
}
/*******************************************************************************
**
** Function         GKI_wait
**
** Description      This function is called by tasks to wait for a specific
**                  event or set of events. The task may specify the duration
**                  that it wants to wait for, or 0 if infinite.
**
** Parameters:      flag -    (input) the event or set of events to wait for
**                  timeout - (input) the duration that the task wants to wait
**                                    for the specific events (in system ticks)
**
**
** Returns          the event mask of received events or zero if timeout
**
*******************************************************************************/
UINT16 GKI_wait (UINT16 flag, UINT32 timeout)
{
    UINT16 evt;
    UINT8 rtask;
    struct timespec abstime = { 0, 0 };

    int sec;
    int nano_sec;

    rtask = GKI_get_taskid();

    GKI_TRACE("GKI_wait %d %x %d", (int)rtask, (int)flag, (int)timeout);

    gki_cb.com.OSWaitForEvt[rtask] = flag;

    /* protect OSWaitEvt[rtask] from modification from an other thread */
    pthread_mutex_lock(&gki_cb.os.thread_evt_mutex[rtask]);

    if (!(gki_cb.com.OSWaitEvt[rtask] & flag))
    {
        if (timeout)
        {
            clock_gettime(CLOCK_MONOTONIC, &abstime);

            /* add timeout */
            sec = timeout / 1000;
            nano_sec = (timeout % 1000) * NANOSEC_PER_MILLISEC;
            abstime.tv_nsec += nano_sec;
            if (abstime.tv_nsec > NSEC_PER_SEC)
            {
                abstime.tv_sec += (abstime.tv_nsec / NSEC_PER_SEC);
                abstime.tv_nsec = abstime.tv_nsec % NSEC_PER_SEC;
            }
            abstime.tv_sec += sec;

            pthread_cond_timedwait_monotonic(&gki_cb.os.thread_evt_cond[rtask],
                    &gki_cb.os.thread_evt_mutex[rtask], &abstime);

        }
        else
        {
            pthread_cond_wait(&gki_cb.os.thread_evt_cond[rtask], &gki_cb.os.thread_evt_mutex[rtask]);
        }

        /* TODO: check, this is probably neither not needed depending on phtread_cond_wait() implmentation,
         e.g. it looks like it is implemented as a counter in which case multiple cond_signal
         should NOT be lost! */

        /* we are waking up after waiting for some events, so refresh variables
           no need to call GKI_disable() here as we know that we will have some events as we've been waking
           up after condition pending or timeout */

        if (gki_cb.com.OSTaskQFirst[rtask][0])
            gki_cb.com.OSWaitEvt[rtask] |= TASK_MBOX_0_EVT_MASK;
        if (gki_cb.com.OSTaskQFirst[rtask][1])
            gki_cb.com.OSWaitEvt[rtask] |= TASK_MBOX_1_EVT_MASK;
        if (gki_cb.com.OSTaskQFirst[rtask][2])
            gki_cb.com.OSWaitEvt[rtask] |= TASK_MBOX_2_EVT_MASK;
        if (gki_cb.com.OSTaskQFirst[rtask][3])
            gki_cb.com.OSWaitEvt[rtask] |= TASK_MBOX_3_EVT_MASK;

        if (gki_cb.com.OSRdyTbl[rtask] == TASK_DEAD)
        {
            gki_cb.com.OSWaitEvt[rtask] = 0;
            /* unlock thread_evt_mutex as pthread_cond_wait() does auto lock when cond is met */
            pthread_mutex_unlock(&gki_cb.os.thread_evt_mutex[rtask]);
            return (EVENT_MASK(GKI_SHUTDOWN_EVT));
        }
    }

    /* Clear the wait for event mask */
    gki_cb.com.OSWaitForEvt[rtask] = 0;

    /* Return only those bits which user wants... */
    evt = gki_cb.com.OSWaitEvt[rtask] & flag;

    /* Clear only those bits which user wants... */
    gki_cb.com.OSWaitEvt[rtask] &= ~flag;

    /* unlock thread_evt_mutex as pthread_cond_wait() does auto lock mutex when cond is met */
    pthread_mutex_unlock(&gki_cb.os.thread_evt_mutex[rtask]);

    GKI_TRACE("GKI_wait %d %x %d %x done", (int)rtask, (int)flag, (int)timeout, (int)evt);
    return (evt);
}
예제 #30
0
bool AP_IOMCU_FW::handle_code_write()
{
    switch (rx_io_packet.page) {
    case PAGE_SETUP:
        switch (rx_io_packet.offset) {
        case PAGE_REG_SETUP_ARMING:
            reg_setup.arming = rx_io_packet.regs[0];
            break;
        case PAGE_REG_SETUP_FORCE_SAFETY_OFF:
            if (rx_io_packet.regs[0] == FORCE_SAFETY_MAGIC) {
                hal.rcout->force_safety_off();
                reg_status.flag_safety_off = true;
            } else {
                return false;
            }
            break;
        case PAGE_REG_SETUP_FORCE_SAFETY_ON:
            if (rx_io_packet.regs[0] == FORCE_SAFETY_MAGIC) {
                hal.rcout->force_safety_on();
                reg_status.flag_safety_off = false;
            } else {
                return false;
            }
            break;
        case PAGE_REG_SETUP_ALTRATE:
            reg_setup.pwm_altrate = rx_io_packet.regs[0];
            update_rcout_freq = true;
            break;
        case PAGE_REG_SETUP_PWM_RATE_MASK:
            reg_setup.pwm_rates = rx_io_packet.regs[0];
            update_rcout_freq = true;
            break;
        case PAGE_REG_SETUP_DEFAULTRATE:
            if (rx_io_packet.regs[0] < 25 && reg_setup.pwm_altclock == 1) {
                rx_io_packet.regs[0] = 25;
            }

            if (rx_io_packet.regs[0] > 400 && reg_setup.pwm_altclock == 1) {
                rx_io_packet.regs[0] = 400;
            }
            reg_setup.pwm_defaultrate = rx_io_packet.regs[0];
            update_default_rate = true;
            break;
        case PAGE_REG_SETUP_SBUS_RATE:
            reg_setup.sbus_rate = rx_io_packet.regs[0];
            sbus_interval_ms = MAX(1000U / reg_setup.sbus_rate,3);
            break;
        case PAGE_REG_SETUP_FEATURES:
            reg_setup.features = rx_io_packet.regs[0];
            /* disable the conflicting options with SBUS 1 */
            if (reg_setup.features & (P_SETUP_FEATURES_SBUS1_OUT)) {
                reg_setup.features &= ~(P_SETUP_FEATURES_PWM_RSSI |
                                        P_SETUP_FEATURES_ADC_RSSI |
                                        P_SETUP_FEATURES_SBUS2_OUT);

                // enable SBUS output at specified rate
                sbus_interval_ms = MAX(1000U / reg_setup.sbus_rate,3);
                palClearLine(HAL_GPIO_PIN_SBUS_OUT_EN);
            } else {
                palSetLine(HAL_GPIO_PIN_SBUS_OUT_EN);
            }
            break;
        case PAGE_REG_SETUP_HEATER_DUTY_CYCLE:
            reg_setup.heater_duty_cycle = rx_io_packet.regs[0];
            last_heater_ms = AP_HAL::millis();
            break;

        case PAGE_REG_SETUP_REBOOT_BL:
            if (reg_status.flag_safety_off) {
                // don't allow reboot while armed
                return false;
            }

            // check the magic value
            if (rx_io_packet.regs[0] != REBOOT_BL_MAGIC) {
                return false;
            }
            schedule_reboot(100);
            break;

        default:
            break;
        }
        break;
    case PAGE_DIRECT_PWM: {
        /* copy channel data */
        uint8_t i = 0, offset = rx_io_packet.offset, num_values = rx_io_packet.count;
        while ((offset < IOMCU_MAX_CHANNELS) && (num_values > 0)) {
            /* XXX range-check value? */
            if (rx_io_packet.regs[i] != PWM_IGNORE_THIS_CHANNEL) {
                reg_direct_pwm.pwm[offset] = rx_io_packet.regs[i];
            }

            offset++;
            num_values--;
            i++;
        }
        fmu_data_received_time = AP_HAL::millis();
        reg_status.flag_fmu_ok = true;
        reg_status.flag_raw_pwm = true;
        chEvtSignalI(thread_ctx, EVENT_MASK(IOEVENT_PWM));
        break;
    }

    default:
        break;
    }
    memset(&tx_io_packet, 0xFF, sizeof(tx_io_packet));
    tx_io_packet.count = 0;
    tx_io_packet.code = CODE_SUCCESS;
    tx_io_packet.crc = 0;
    tx_io_packet.crc =  crc_crc8((const uint8_t *)&tx_io_packet, tx_io_packet.get_size());
    return true;
}