自学教程：C++ work_queue函数代码示例

static int ads7843e_schedule(FAR struct ads7843e_dev_s *priv){  FAR struct ads7843e_config_s *config;  int                           ret;  /* Get a pointer the callbacks for convenience (and so the code is not so   * ugly).   */  config = priv->config;  DEBUGASSERT(config != NULL);  /* Disable further interrupts.  ADS7843E interrupts will be re-enabled   * after the worker thread executes.   */  config->enable(config, false);  /* Disable the watchdog timer.  It will be re-enabled in the worker thread   * while the pen remains down.   */  wd_cancel(priv->wdog);  /* Transfer processing to the worker thread.  Since ADS7843E interrupts are   * disabled while the work is pending, no special action should be required   * to protected the work queue.   */  DEBUGASSERT(priv->work.worker == NULL);  ret = work_queue(HPWORK, &priv->work, ads7843e_worker, priv, 0);  if (ret != 0)    {      illdbg("Failed to queue work: %d/n", ret);    }  return OK;}

voidMPU9250::cycle(){//	int ret = measure();	measure();//	if (ret != OK) {//		/* issue a reset command to the sensor *///		reset();//		start();//		return;//	}	if (_call_interval != 0) {		work_queue(HPWORK,			   &_work,			   (worker_t)&MPU9250::cycle_trampoline,			   this,			   USEC2TICK(_call_interval - MPU9250_TIMER_REDUCTION));	}}

static void lo_poll_expiry(int argc, wdparm_t arg, ...){  FAR struct lo_driver_s *priv = (FAR struct lo_driver_s *)arg;  /* Is our single work structure available?  It may not be if there are   * pending interrupt actions.   */  if (work_available(&priv->lo_work))    {      /* Schedule to perform the interrupt processing on the worker thread. */      work_queue(HPWORK, &priv->lo_work, lo_poll_work, priv, 0);    }  else    {      /* No.. Just re-start the watchdog poll timer, missing one polling       * cycle.       */      (void)wd_start(priv->lo_polldog, LO_WDDELAY, lo_poll_expiry, 1, arg);    }}

voidLM73::cycle(){	/* collection phase? */	if (_measurement_phase) {		/* perform temperature measurement */		if (OK != temp_measurement()) {			start();			return;		}		/* next phase is measurement */		_measurement_phase = true;		/* schedule a fresh cycle call when the measurement is done */		work_queue(HPWORK,			   &_work,			   (worker_t)&LM73::cycle_trampoline,			   this,			   USEC2TICK(LM73_CONVERSION_INTERVAL));	}}

voidETSAirspeed::start(){	/* reset the report ring and state machine */	_collect_phase = false;	_oldest_report = _next_report = 0;	/* schedule a cycle to start things */	work_queue(HPWORK, &_work, (worker_t)&ETSAirspeed::cycle_trampoline, this, 1);		/* notify about state change */	struct subsystem_info_s info = {		true,		true,		true,		SUBSYSTEM_TYPE_DIFFPRESSURE};	static orb_advert_t pub = -1;	if (pub > 0) {		orb_publish(ORB_ID(subsystem_info), pub, &info);	} else {		pub = orb_advertise(ORB_ID(subsystem_info), &info);	}}

int aio_queue(FAR struct aio_container_s *aioc, worker_t worker){  int ret;#ifdef CONFIG_PRIORITY_INHERITANCE  /* Prohibit context switches until we complete the queuing */  sched_lock();  /* Make sure that the low-priority worker thread is running at at least   * the priority specified for this action.   */  lpwork_boostpriority(aioc->aioc_prio);#endif  /* Schedule the work on the low priority worker thread */  ret = work_queue(LPWORK, &aioc->aioc_work, worker, aioc, 0);  if (ret < 0)    {      FAR struct aiocb *aiocbp = aioc->aioc_aiocbp;      DEBUGASSERT(aiocbp);      aiocbp->aio_result = ret;      set_errno(-ret);      ret = ERROR;    }#ifdef CONFIG_PRIORITY_INHERITANCE  /* Now the low-priority work queue might run at its new priority */  sched_unlock();#endif  return ret;}

voidBAROSIM::cycle(){	int ret;	unsigned dummy;	/* collection phase? */	if (_collect_phase) {		/* perform collection */		ret = collect();		if (ret != OK) {			/* issue a reset command to the sensor */			_interface->dev_ioctl(IOCTL_RESET, dummy);			/* reset the collection state machine and try again */			start_cycle();			return;		}		/* next phase is measurement */		_collect_phase = false;		/*		 * Is there a collect->measure gap?		 * Don't inject one after temperature measurements, so we can keep		 * doing pressure measurements at something close to the desired rate.		 */		if ((_measure_phase != 0) &&		    (_measure_ticks > USEC2TICK(BAROSIM_CONVERSION_INTERVAL))) {			/* schedule a fresh cycle call when we are ready to measure again */			work_queue(HPWORK,				   &_work,				   (worker_t)&BAROSIM::cycle_trampoline,				   this,				   _measure_ticks - USEC2TICK(BAROSIM_CONVERSION_INTERVAL));			return;		}	}	/* measurement phase */	ret = measure();	if (ret != OK) {		//DEVICE_LOG("measure error %d", ret);		/* issue a reset command to the sensor */		_interface->dev_ioctl(IOCTL_RESET, dummy);		/* reset the collection state machine and try again */		start_cycle();		return;	}	/* next phase is collection */	_collect_phase = true;	/* schedule a fresh cycle call when the measurement is done */	work_queue(HPWORK,		   &_work,		   (worker_t)&BAROSIM::cycle_trampoline,		   this,		   USEC2TICK(BAROSIM_CONVERSION_INTERVAL));}

voidSF0X::cycle(){	/* fds initialized? */	if (_fd < 0) {		/* open fd */		_fd = ::open(SF0X_DEFAULT_PORT, O_RDWR | O_NOCTTY | O_NONBLOCK);	}	/* collection phase? */	if (_collect_phase) {		/* perform collection */		int collect_ret = collect();		if (collect_ret == -EAGAIN) {			/* reschedule to grab the missing bits, time to transmit 10 bytes @9600 bps */			work_queue(HPWORK,				   &_work,				   (worker_t)&SF0X::cycle_trampoline,				   this,				   USEC2TICK(1100));			return;		}		if (OK != collect_ret) {			/* we know the sensor needs about four seconds to initialize */			if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {				log("collection error #%u", _consecutive_fail_count);			}			_consecutive_fail_count++;			/* restart the measurement state machine */			start();			return;		} else {			/* apparently success */			_consecutive_fail_count = 0;		}		/* next phase is measurement */		_collect_phase = false;		/*		 * Is there a collect->measure gap?		 */		if (_measure_ticks > USEC2TICK(SF0X_CONVERSION_INTERVAL)) {			/* schedule a fresh cycle call when we are ready to measure again */			work_queue(HPWORK,				   &_work,				   (worker_t)&SF0X::cycle_trampoline,				   this,				   _measure_ticks - USEC2TICK(SF0X_CONVERSION_INTERVAL));			return;		}	}	/* measurement phase */	if (OK != measure()) {		log("measure error");	}	/* next phase is collection */	_collect_phase = true;	/* schedule a fresh cycle call when the measurement is done */	work_queue(HPWORK,		   &_work,		   (worker_t)&SF0X::cycle_trampoline,		   this,		   USEC2TICK(SF0X_CONVERSION_INTERVAL));}

static void usbhost_callback(FAR void *arg, ssize_t nbytes){  FAR struct usbhost_class_s *hubclass;  FAR struct usbhost_hubpriv_s *priv;  uint32_t delay = 0;  DEBUGASSERT(arg != NULL);  hubclass = (FAR struct usbhost_class_s *)arg;  priv     = &((FAR struct usbhost_hubclass_s *)hubclass)->hubpriv;  /* Check for a failure.  On higher end host controllers, the asynchronous   * transfer will pend until data is available (OHCI and EHCI).  On lower   * end host controllers (like STM32 and EFM32), the transfer will fail   * immediately when the device NAKs the first attempted interrupt IN   * transfer (with nbytes == -EAGAIN).  In that case (or in the case of   * other errors), we must fall back to polling.   */  if (nbytes < 0)    {      /* This debug output is good to know, but really a nuisance for       * those configurations where we have to fall back to polling.       * FIX:  Don't output the message is the result is -EAGAIN.       */#if defined(CONFIG_DEBUG_USB) && !defined(CONFIG_DEBUG_VERBOSE)      if (nbytes != -EAGAIN)#endif        {          ulldbg("ERROR: Transfer failed: %d/n", (int)nbytes);        }      /* Indicate there there is nothing to do.  So when the work is       * performed, nothing will happen other than we will set to receive       * the next event.       */      priv->buffer[0] = 0;      /* We don't know the nature of the failure, but we need to do all that       * we can do to avoid a CPU hog error loop.       *       * Use the low-priority work queue and delay polling for the next       * event.  We want to use as little CPU bandwidth as possible in this       * case.       */      delay = POLL_DELAY;    }  /* The work structure should always be available since hub communications   * are serialized.  However, there is a remote chance that this may   * collide with a hub disconnection event.   */  if (work_available(&priv->work) && !priv->disconnected)    {      (void)work_queue(LPWORK, &priv->work, (worker_t)usbhost_hub_event,                       hubclass, delay);    }}

void gpio_led_cycle(FAR void *arg){	FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;	/* check for status updates*/	bool updated;	orb_check(priv->vehicle_status_sub, &updated);	if (updated) {		orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status);	}	/* select pattern for current status */	int pattern = 0;	if (priv->status.arming_state == ARMING_STATE_ARMED_ERROR) {		pattern = 0x2A;	// *_*_*_ fast blink (armed, error)	} else if (priv->status.arming_state == ARMING_STATE_ARMED) {		if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && !priv->status.failsafe) {			pattern = 0x3f;	// ****** solid (armed)		} else {			pattern = 0x3e;	// *****_ slow blink (armed, battery low or failsafe)		}	} else if (priv->status.arming_state == ARMING_STATE_STANDBY) {		pattern = 0x38;	// ***___ slow blink (disarmed, ready)	} else if (priv->status.arming_state == ARMING_STATE_STANDBY_ERROR) {		pattern = 0x28;	// *_*___ slow double blink (disarmed, error)	}	/* blink pattern */	bool led_state_new = (pattern & (1 << priv->counter)) != 0;	if (led_state_new != priv->led_state) {		priv->led_state = led_state_new;		if (led_state_new) {			ioctl(priv->gpio_fd, GPIO_SET, priv->pin);		} else {			ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);		}	}	priv->counter++;	if (priv->counter > 5) {		priv->counter = 0;	}	/* repeat cycle at 5 Hz */	if (gpio_led_started) {		work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000));	} else {		/* switch off LED on stop */		ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);	}}

voidTFMINI::cycle(){	/* fds initialized? */	if (_fd < 0) {		/* open fd */		_fd = ::open(_port, O_RDWR | O_NOCTTY | O_SYNC);	}	/* collection phase? */	if (_collect_phase) {		/* perform collection */		int collect_ret = collect();		if (collect_ret == -EAGAIN) {			/* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */			work_queue(HPWORK,				   &_work,				   (worker_t)&TFMINI::cycle_trampoline,				   this,				   USEC2TICK(1042 * 8));			return;		}		if (OK != collect_ret) {			/* we know the sensor needs about four seconds to initialize */			if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {				DEVICE_LOG("collection error #%u", _consecutive_fail_count);			}			_consecutive_fail_count++;			/* restart the measurement state machine */			start();			return;		} else {			/* apparently success */			_consecutive_fail_count = 0;		}		/* next phase is measurement */		_collect_phase = false;		/*		 * Is there a collect->measure gap?		 */		if (_measure_ticks > USEC2TICK(_conversion_interval)) {			/* schedule a fresh cycle call when we are ready to measure again */			work_queue(HPWORK,				   &_work,				   (worker_t)&TFMINI::cycle_trampoline,				   this,				   _measure_ticks - USEC2TICK(_conversion_interval));			return;		}	}	/* next phase is collection */	_collect_phase = true;	/* schedule a fresh cycle call when the measurement is done */	work_queue(HPWORK,		   &_work,		   (worker_t)&TFMINI::cycle_trampoline,		   this,		   USEC2TICK(_conversion_interval));}

intPCA9685::ioctl(struct file *filp, int cmd, unsigned long arg){	int ret = -EINVAL;	switch (cmd) {		case MIXERIOCRESET:			if (_mixers != nullptr) {				delete _mixers;				_mixers = nullptr;				_groups_required = 0;			}			ret = OK;			break;		case MIXERIOCADDSIMPLE: {			mixer_simple_s *mixinfo = (mixer_simple_s *)arg;			SimpleMixer *mixer = new SimpleMixer(control_callback,				(uintptr_t)_controls, mixinfo);			if (mixer->check()) {				delete mixer;				_groups_required = 0;				ret = -EINVAL;			} else {				if (_mixers == nullptr)					_mixers = new MixerGroup(control_callback,						(uintptr_t)_controls);				_mixers->add_mixer(mixer);				_mixers->groups_required(_groups_required);				ret = OK;			}			break;		}		case MIXERIOCLOADBUF: {			const char *buf = (const char *)arg;			unsigned buflen = strnlen(buf, 1024);			if (_mixers == nullptr) {				_mixers = new MixerGroup(control_callback,					(uintptr_t)_controls);			}			if (_mixers == nullptr) {				_groups_required = 0;				ret = -ENOMEM;			} else {				ret = _mixers->load_from_buf(buf, buflen);				if (ret != 0) {					DEVICE_DEBUG("mixer load failed with %d", ret);					delete _mixers;					_mixers = nullptr;					_groups_required = 0;					ret = -EINVAL;				} else {					_mixers->groups_required(_groups_required);					ret = OK;				}			}			break;		}	case IOX_SET_MODE:		if (_mode != (IOX_MODE)arg) {			switch ((IOX_MODE)arg) {			case IOX_MODE_OFF:				warnx("shutting down");				break;			case IOX_MODE_ON:				warnx("starting");				break;			case IOX_MODE_TEST_OUT:				warnx("test starting");				break;			default:				return -1;			}			_mode = (IOX_MODE)arg;		}		// if not active, kick it		if (!_running) {			_running = true;			work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, 1);		}//.........这里部分代码省略.........

voidMB12XX::cycle(){	if (_collect_phase) {		_index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */		set_device_address(_index_counter);		/* perform collection */		if (OK != collect()) {			DEVICE_DEBUG("collection error");			/* if error restart the measurement state machine */			start();			return;		}		/* next phase is measurement */		_collect_phase = false;		/* change i2c adress to next sonar */		_cycle_counter = _cycle_counter + 1;		if (_cycle_counter >= addr_ind.size()) {			_cycle_counter = 0;		}		/* Is there a collect->measure gap? Yes, and the timing is set equal to the cycling_rate		   Otherwise the next sonar would fire without the first one having received its reflected sonar pulse */		if (_measure_ticks > USEC2TICK(_cycling_rate)) {			/* schedule a fresh cycle call when we are ready to measure again */			work_queue(HPWORK,				   &_work,				   (worker_t)&MB12XX::cycle_trampoline,				   this,				   _measure_ticks - USEC2TICK(_cycling_rate));			return;		}	}	/* Measurement (firing) phase */	/* ensure sonar i2c adress is still correct */	_index_counter = addr_ind[_cycle_counter];	set_device_address(_index_counter);	/* Perform measurement */	if (OK != measure()) {		DEVICE_DEBUG("measure error sonar adress %d", _index_counter);	}	/* next phase is collection */	_collect_phase = true;	/* schedule a fresh cycle call when the measurement is done */	work_queue(HPWORK,		   &_work,		   (worker_t)&MB12XX::cycle_trampoline,		   this,		   USEC2TICK(_cycling_rate));}

int arch_tcinitialize(int minor){  FAR struct tc_dev_s *priv;  char devname[DEV_NAMELEN];  int ret;  ivdbg("minor: %d/n", minor);  DEBUGASSERT(minor >= 0 && minor < 100);  /* Configure all touchscreen pins as inputs, undriven */  putreg32(LCD_ALL_BITS, PIC32MX_IOPORTB_TRISSET);  /* Configure all pins for as digital. AD1PCFG specifies the configuration   * of device pins to be used as analog inputs. A pin is configured as an   * analog input when the corresponding PCFGn bit is 0.   */  putreg32(LCD_ALL_BITS, PIC32MX_ADC_CFGSET);  /* Create and initialize a touchscreen device driver instance */#ifndef CONFIG_TOUCHSCREEN_MULTIPLE  priv = &g_touchscreen;#else  priv = (FAR struct tc_dev_s *)kmm_malloc(sizeof(struct tc_dev_s));  if (!priv)    {      idbg("kmm_malloc(%d) failed/n", sizeof(struct tc_dev_s));      return -ENOMEM;    }#endif  /* Initialize the touchscreen device driver instance */  memset(priv, 0, sizeof(struct tc_dev_s));  sem_init(&priv->devsem,  0, 1); /* Initialize device structure semaphore */  sem_init(&priv->waitsem, 0, 0); /* Initialize pen event wait semaphore */  /* Register the device as an input device */  (void)snprintf(devname, DEV_NAMELEN, DEV_FORMAT, minor);  ivdbg("Registering %s/n", devname);  ret = register_driver(devname, &tc_fops, 0666, priv);  if (ret < 0)    {      idbg("register_driver() failed: %d/n", ret);      goto errout_with_priv;    }  /* Schedule work to perform the initial sampling and to set the data   * availability conditions.   */  priv->state = TC_READY;  ret = work_queue(HPWORK, &priv->work, tc_worker, priv, 0);  if (ret != 0)    {      idbg("Failed to queue work: %d/n", ret);      goto errout_with_priv;    }  /* And return success (?) */  return OK;errout_with_priv:  sem_destroy(&priv->devsem);#ifdef CONFIG_TOUCHSCREEN_MULTIPLE  kmm_free(priv);#endif  return ret;}

//.........这里部分代码省略.........      /* Ignore if the pen was already down (CONTACT_NONE == pen up and already       * reported.  CONTACT_UP == pen up, but not reported)       */      if (priv->sample.contact != CONTACT_NONE)        {          /* The pen is up.  We know from the above test, that this is a           * loss of contact condition.  This will be changed to CONTACT_NONE           * after the loss of contact is sampled.           */          priv->sample.contact = CONTACT_UP;          /* Indicate the availability of new sample data for this ID */          priv->sample.id = priv->id;          priv->penchange = true;          /* Notify any waiters that nes touchscreen data is available */          tc_notify(priv);        }      /* Set up for the next poll */      priv->sample.valid = false;      priv->state        = TC_READY;      delay              = TC_PENUP_POLL_TICKS;    }  /* Check if the sampling resulted in a pen down decision. */  else if (priv->state == TC_PENDOWN)    {      /* It is a pen down event.  If the last loss-of-contact event has not been       * processed yet, then we have to ignore the pen down event (or else it will       * look like a drag event)       */      if (priv->sample.contact != CONTACT_UP)        {          /* Perform a thresholding operation so that the results will be more stable.           * If the difference from the last sample is small, then ignore the event.           */          xdiff = (int16_t)priv->sample.x - (int16_t)newx;          if (xdiff < 0)            {              xdiff = -xdiff;            }          ydiff = (int16_t)priv->sample.y - (int16_t)priv->newy;          if (ydiff < 0)            {              ydiff = -ydiff;            }          if (xdiff >= CONFIG_TOUCHSCREEN_THRESHX ||              ydiff >= CONFIG_TOUCHSCREEN_THRESHY)            {              /* There is some change above the threshold... Report the change. */              priv->sample.x     = newx;              priv->sample.y     = priv->newy;              priv->sample.valid = true;              /* If this is the first (acknowledged) penddown report, then report               * this as the first contact.  If contact == CONTACT_DOWN, it will be               * set to set to CONTACT_MOVE after the contact is first sampled.               */              if (priv->sample.contact != CONTACT_MOVE)                {                  /* First contact */                  priv->sample.contact = CONTACT_DOWN;                }              /* Indicate the availability of new sample data for this ID */              priv->sample.id = priv->id;              priv->penchange = true;              /* Notify any waiters that nes touchscreen data is available */              tc_notify(priv);            }        }      /* Set up for the next poll */      priv->state = TC_READY;      delay       = TC_PENDOWN_POLL_TICKS;    }  /* Set up the next sample event */  ret = work_queue(HPWORK, &priv->work, tc_worker, priv, delay);  ASSERT(ret == 0);}

void bcmf_netdev_notify_rx(FAR struct bcmf_dev_s *priv){  /* Queue a job to process RX frames */  work_queue(BCMFWORK, &priv->bc_pollwork, bcmf_rxpoll, priv, 0);}

//.........这里部分代码省略.........           */          priv->sample.contact = CONTACT_UP;          /* Indicate the availability of new sample data for this ID */          priv->sample.id = priv->id;          priv->penchange = true;          /* Notify any waiters that new touchscreen data is available */          iinfo("1:X=%d, Y=%d/n", priv->sample.x, priv->sample.y);          tc_notify(priv);        }      /* Set up for the next poll */      priv->sample.valid = false;      priv->state        = TC_READY;      delay              = TC_PENUP_POLL_TICKS;    }  /* Check if the sampling resulted in a pen down decision. */  else if (priv->state == TC_PENDOWN)    {      /* It is a pen down event.  If the last loss-of-contact event has not been       * processed yet, then we have to ignore the pen down event (or else it will       * look like a drag event)       */      if (priv->sample.contact != CONTACT_UP)        {          /* Perform a thresholding operation so that the results will be more stable.           * If the difference from the last sample is small, then ignore the event.           */          xdiff = (int16_t)priv->sample.x - (int16_t)newx;          if (xdiff < 0)            {              xdiff = -xdiff;            }          ydiff = (int16_t)priv->sample.y - (int16_t)priv->newy;          if (ydiff < 0)            {              ydiff = -ydiff;            }          if (xdiff >= CONFIG_TOUCHSCREEN_THRESHX ||              ydiff >= CONFIG_TOUCHSCREEN_THRESHY)            {              /* There is some change above the threshold... Report the change. */#ifdef CONFIG_LCD_LANDSCAPE              priv->sample.x     = MAX_ADC - priv->newy;              priv->sample.y     = newx;#else              priv->sample.x     = newx;              priv->sample.y     = priv->newy;#endif              priv->sample.valid = true;              /* If this is the first (acknowledged) penddown report, then report               * this as the first contact.  If contact == CONTACT_DOWN, it will be               * set to set to CONTACT_MOVE after the contact is first sampled.               */              if (priv->sample.contact != CONTACT_MOVE)                {                  /* First contact */                  priv->sample.contact = CONTACT_DOWN;                }              /* Indicate the availability of new sample data for this ID */              priv->sample.id = priv->id;              priv->penchange = true;              /* Notify any waiters that nes touchscreen data is available */              iinfo("2:X=%d, Y=%d/n", priv->sample.x, priv->sample.y);              tc_notify(priv);            }        }      /* Set up for the next poll */      priv->state = TC_READY;      delay       = TC_PENDOWN_POLL_TICKS;    }  /* Set up the next sample event */  ret = work_queue(HPWORK, &priv->work, tc_worker, priv, delay);  DEBUGASSERT(ret == 0);}

int stm32_tsc_setup(int minor){  FAR struct tc_dev_s *priv;  char devname[DEV_NAMELEN];#ifdef CONFIG_TOUCHSCREEN_MULTIPLE  irqstate_t flags;#endif  int ret;  iinfo("minor: %d/n", minor);  DEBUGASSERT(minor >= 0 && minor < 100);  /* If we only have one touchscreen, check if we already did init */#ifndef CONFIG_TOUCHSCREEN_MULTIPLE  if (g_touchinitdone)    {      return OK;    }#endif  /* Configure the touchscreen DRIVEA and DRIVEB pins for output */  stm32_configgpio(GPIO_TP_DRIVEA);  stm32_configgpio(GPIO_TP_DRIVEB);  /* Configure Analog inputs for sampling X and Y coordinates */  stm32_configgpio(GPIO_TP_XL);  stm32_configgpio(GPIO_TP_YD);  tc_adc_init();  /* Create and initialize a touchscreen device driver instance */#ifndef CONFIG_TOUCHSCREEN_MULTIPLE  priv = &g_touchscreen;#else  priv = (FAR struct tc_dev_s *)kmm_malloc(sizeof(struct tc_dev_s));  if (!priv)    {      ierr("ERROR: kmm_malloc(%d) failed/n", sizeof(struct tc_dev_s));      return -ENOMEM;    }#endif  /* Initialize the touchscreen device driver instance */  memset(priv, 0, sizeof(struct tc_dev_s));  nxsem_init(&priv->devsem,  0, 1); /* Initialize device structure semaphore */  nxsem_init(&priv->waitsem, 0, 0); /* Initialize pen event wait semaphore */  /* Register the device as an input device */  (void)snprintf(devname, DEV_NAMELEN, DEV_FORMAT, minor);  iinfo("Registering %s/n", devname);  ret = register_driver(devname, &tc_fops, 0666, priv);  if (ret < 0)    {      ierr("ERROR: register_driver() failed: %d/n", ret);      goto errout_with_priv;    }  /* Schedule work to perform the initial sampling and to set the data   * availability conditions.   */  priv->state = TC_READY;  ret = work_queue(HPWORK, &priv->work, tc_worker, priv, 0);  if (ret != 0)    {      ierr("ERROR: Failed to queue work: %d/n", ret);      goto errout_with_priv;    }  /* And return success (?) */#ifndef CONFIG_TOUCHSCREEN_MULTIPLE  g_touchinitdone = true;#endif  return OK;errout_with_priv:  nxsem_destroy(&priv->devsem);#ifdef CONFIG_TOUCHSCREEN_MULTIPLE  kmm_free(priv);#endif  return ret;}

voidjob_accept (conf_t conf){    work_p  w;    m_msg_t m;    int     sd;    assert (conf != NULL);    assert (conf->ld >= 0);    if (!(w = work_init ((work_func_t) _job_exec, conf->nthreads))) {        log_errno (EMUNGE_SNAFU, LOG_ERR,            "Failed to create %d work thread%s", conf->nthreads,            ((conf->nthreads > 1) ? "s" : ""));    }    log_msg (LOG_INFO, "Created %d work thread%s", conf->nthreads,            ((conf->nthreads > 1) ? "s" : ""));    while (!done) {        if ((sd = accept (conf->ld, NULL, NULL)) < 0) {            switch (errno) {                case ECONNABORTED:                case EINTR:                    continue;                case EMFILE:                case ENFILE:                case ENOBUFS:                case ENOMEM:                    log_msg (LOG_INFO,                        "Suspended new connections while processing backlog");                    work_wait (w);                    continue;                default:                    log_errno (EMUNGE_SNAFU, LOG_ERR,                        "Failed to accept connection");                    break;            }        }        /*  With fd_timed_read_n(), a poll() is performed before any read()         *    in order to provide timeouts and ensure the read() won't block.         *    As such, it shouldn't be necessary to set the client socket as         *    non-blocking.  However according to the Linux poll(2) and         *    select(2) manpages, spurious readiness notifications can occur.         *    poll()/select() may report a socket as ready for reading while         *    the subsequent read() blocks.  This could happen when data has         *    arrived, but upon examination is discarded due to an invalid         *    checksum.  To protect against this, the client socket is set         *    non-blocking and EAGAIN is handled appropriately.         */        if (fd_set_nonblocking (sd) < 0) {            close (sd);            log_msg (LOG_WARNING,                "Failed to set nonblocking client socket: %s",                strerror (errno));        }        else if (m_msg_create (&m) != EMUNGE_SUCCESS) {            close (sd);            log_msg (LOG_WARNING, "Failed to create client request");        }        else if (m_msg_bind (m, sd) != EMUNGE_SUCCESS) {            m_msg_destroy (m);            log_msg (LOG_WARNING, "Failed to bind socket for client request");        }        else if (work_queue (w, m) < 0) {            m_msg_destroy (m);            log_msg (LOG_WARNING, "Failed to queue client request");        }    }    log_msg (LOG_NOTICE, "Exiting on signal=%d", done);    work_fini (w, 1);    return;}

//.........这里部分代码省略.........	if (_config_cmd_set) {		_config_cmd_set = false;		_attitude_compensation_roll = (int)_config_cmd.param2 == 1;		_attitude_compensation_pitch = (int)_config_cmd.param3 == 1;		_attitude_compensation_yaw = (int)_config_cmd.param4 == 1;		/* only check commanded mount mode when in offboard */		if (_control_mode.flag_control_offboard_enabled &&		    fabsf(_config_cmd.param1 - _mount_mode) > FLT_EPSILON) {			_mount_mode = int(_config_cmd.param1 + 0.5f);			updated = true;		}	}	if (_control_cmd_set) {		unsigned mountMode = _control_cmd.param7;		DEVICE_DEBUG("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode,			     (double)_control_cmd.param1, (double)_control_cmd.param2);		if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL &&		    mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {			/* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */			roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param1;			pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param2;			yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param3;			updated = true;		}		if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT &&		    mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {			float gimbalDirectionQuat[] = {_control_cmd.param1, _control_cmd.param2, _control_cmd.param3, _control_cmd.param4};			math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler();			roll += gimablDirectionEuler(0);			pitch += gimablDirectionEuler(1);			yaw += gimablDirectionEuler(2);			updated = true;		}	}	/* consider mount mode if parameter is set */	if (_parameters.use_mnt > 0) {		switch (_mount_mode) {		case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:			out_mount_mode = -1.0f;			roll = 0.0f;			pitch = 0.0f;			yaw = 0.0f;			break;		case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:		case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING:		case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:		case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:			out_mount_mode = 1.0f;			break;		default:			out_mount_mode = -1.0f;		}	}	if (updated) {		struct actuator_controls_s controls;		// DEVICE_DEBUG("publishing | roll: %8.4f pitch: %8.4f yaw: %8.4f", (double)roll, (double)pitch, (double)yaw);		/* fill in the final control values */		controls.timestamp = hrt_absolute_time();		controls.control[0] = roll;		controls.control[1] = pitch;		controls.control[2] = yaw;		//controls.control[3] = ; // camera shutter		controls.control[4] = out_mount_mode;		/* publish it */		orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_2_topic, &controls);	}	/* notify anyone waiting for data */	poll_notify(POLLIN);	perf_end(_sample_perf);	/* schedule a fresh cycle call when the measurement is done */	work_queue(LPWORK,		   &_work,		   (worker_t)&Gimbal::cycle_trampoline,		   this,		   USEC2TICK(GIMBAL_UPDATE_INTERVAL));}

//.........这里部分代码省略.........						led_color_3 = LED_RED;						led_color_4 = LED_OFF;						led_color_5 = LED_OFF;						led_color_6 = LED_OFF;						led_color_7 = LED_OFF;						led_color_8 = LED_OFF;						led_blink = LED_BLINK;						/* handle 4th led - flightmode indicator */						switch((int)vehicle_status_raw.flight_mode) {							case VEHICLE_FLIGHT_MODE_MANUAL:								led_color_4 = LED_AMBER;								break;							case VEHICLE_FLIGHT_MODE_STAB:								led_color_4 = LED_YELLOW;								break;							case VEHICLE_FLIGHT_MODE_HOLD:								led_color_4 = LED_BLUE;								break;							case VEHICLE_FLIGHT_MODE_AUTO:								led_color_4 = LED_GREEN;								break;						}						if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {							/* handling used sat
C++ workbook_add_worksheet函数代码示例
C++ work_pending函数代码示例