Exemple #1
0
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;
}
Exemple #2
0
void
MPU9250::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));
	}
}
Exemple #3
0
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);
    }
}
Exemple #4
0
void
LM73::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));
	}
}
Exemple #5
0
void
ETSAirspeed::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);
	}
}
Exemple #6
0
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;
}
Exemple #7
0
void
BAROSIM::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));
}
Exemple #8
0
void
SF0X::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);
    }
}
Exemple #10
0
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);
	}
}
Exemple #11
0
void
TFMINI::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));
}
Exemple #12
0
int
PCA9685::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);
		}


		return OK;

	default:
		// see if the parent class can make any use of it
		ret = CDev::ioctl(filp, cmd, arg);
		break;
	}

	return ret;
}
Exemple #13
0
void
MB12XX::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));

}
Exemple #14
0
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;
}
Exemple #15
0
static void tc_worker(FAR void *arg)
{
  FAR struct tc_dev_s *priv = (FAR struct tc_dev_s *)arg;
  uint32_t delay;
  uint16_t value;
  uint16_t newx;
  int16_t xdiff;
  int16_t ydiff;
  int ret;

  ASSERT(priv != NULL);

  /* Perform the next action based on the state of the conversions */

  switch (priv->state)
    {
    /* The touchscreen is IDLE and we are ready to begin the next sample */

    case TC_READY:
      {
        /* Start Y- sampling */

        tc_yminus_sample();

        /* Allow time for the Y- pend down sampling */

        priv->state = TC_YMPENDOWN;
        delay       = TC_SAMPLE_TICKS;
      }
      break;

    /* The Y- sampling time has elapsed and the Y- value should be ready
     * for conversion
     */

    case TC_YMPENDOWN:
      {
        /* Convert the Y- sample value */

        value = tc_adc_convert();

        /* A converted value at the minimum would mean that there is no touch
         * and that the sampling period is complete.
         */

        if (!tc_valid_sample(value))
          {
            priv->state = TC_PENUP;
          }
        else
          {
            /* Allow time for touch inputs to stabilize */

            priv->state = TC_DEBOUNCE;
            delay       = TC_DEBOUNCE_TICKS;
          }
      }
      break;

    /* The debounce time period has elapsed and we are ready to re-sample
     * the touchscreen.
     */

    case TC_RESAMPLE:
    case TC_DEBOUNCE:
      {
        /* (Re-)start Y- sampling */

        tc_yminus_sample();

        /* Allow time for the Y- sampling */

        priv->state = TC_YMSAMPLE;
        delay       = TC_SAMPLE_TICKS;
      }
      break;

    /* The Y- sampling period has elapsed and we are ready to perform the
     * conversion.
     */

    case TC_YMSAMPLE:
      {
        /* Convert and save the Y- sample value */

        value = tc_adc_convert();

        /* A converted value at the minimum would mean that there is no touch
         * and that the sampling period is complete.  At converted value at
         * the maximum value is probably bad too.
         */

        if (!tc_valid_sample(value))
          {
            priv->state = TC_PENUP;
          }
        else
          {
            /* Save the Y- sample and start Y+ sampling */

            priv->value = value;
            tc_yplus_sample();

            /* Allow time for the Y+ sampling */

            priv->state = TC_YPSAMPLE;
            delay       = TC_SAMPLE_TICKS;
          }
      }
      break;

    /* The Y+ sampling period has elapsed and we are ready to perform the
     * conversion.
     */

    case TC_YPSAMPLE:                         /* Allowing time for the Y+ sampling */
      {
        /* Read the Y+ axis position */

        value = tc_adc_convert();

        /* A converted value at the minimum would mean that we lost the contact
         * before all of the conversions were completed.  At converted value at
         * the maximum value is probably bad too.
         */

        if (!tc_valid_sample(value))
          {
#ifdef CONFIG_TOUCHSCREEN_RESAMPLE
            priv->state = TC_RESAMPLE;
            delay       = TC_RESAMPLE_TICKS;
#else
            priv->state = TC_PENUP;
#endif
          }
        else
          {
            value      = MAX_ADC - value;
            priv->newy = (value + priv->value) >> 1;
            ivdbg("Y-=%d Y+=%d[%d] Y=%d\n", priv->value, value, MAX_ADC - value, priv->newy);

            /* Start X+ sampling */

            tc_xplus_sample();

            /* Allow time for the X+ sampling */

            priv->state = TC_XPSAMPLE;
            delay       = TC_SAMPLE_TICKS;
          }
      }
      break;

    /* The X+ sampling period has elapsed and we are ready to perform the
     * conversion.
     */

    case TC_XPSAMPLE:
      {
        /* Convert the X+ sample value */

        value = tc_adc_convert();

        /* A converted value at the minimum would mean that we lost the contact
         * before all of the conversions were completed.  At converted value at
         * the maximum value is probably bad too.
         */

        if (!tc_valid_sample(value))
          {
#ifdef CONFIG_TOUCHSCREEN_RESAMPLE
            priv->state = TC_RESAMPLE;
            delay       = TC_RESAMPLE_TICKS;
#else
            priv->state = TC_PENUP;
#endif
          }
        else
          {
            /* Save the X+ sample value */

            priv->value = value;

            /* Start X- sampling */

            tc_xminus_sample();

            /* Allow time for the X- pend down sampling */

            priv->state = TC_XMSAMPLE;
            delay       = TC_SAMPLE_TICKS;
          }
      }
      break;

    /* The X+ sampling period has elapsed and we are ready to perform the
     * conversion.
     */

    case TC_XMSAMPLE:                         /* Allowing time for the X- sampling */
      {
        /* Read the converted X- axis position */

        value = tc_adc_convert();

        /* A converted value at the minimum would mean that we lost the contact
         * before all of the conversions were completed.  At converted value at
         * the maximum value is probably bad too.
         */

        if (!tc_valid_sample(value))
          {
#ifdef CONFIG_TOUCHSCREEN_RESAMPLE
            priv->state = TC_RESAMPLE;
            delay       = TC_RESAMPLE_TICKS;
#else
            priv->state = TC_PENUP;
#endif
          }
        else
          {
            /* Calculate the X- axis position */

            value = MAX_ADC - value;
            newx  = (value + priv->value) >> 1;
            ivdbg("X+=%d X-=%d[%d] X=%d\n", priv->value, value, MAX_ADC - value, newx);

            /* Samples are available */

            priv->state = TC_PENDOWN;
          }
      }
      break;
    }

  /* Check for terminal conditions.. */

  /* Check if the sampling resulted in a pen up decision.  If so, we need to
   * handle the change from pen down to pen up.
   */

  if (priv->state == TC_PENUP)
    {
      /* 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);
}
Exemple #16
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);
}
Exemple #17
0
static void tc_worker(FAR void *arg)
{
  FAR struct tc_dev_s *priv = (FAR struct tc_dev_s *)arg;
  uint32_t delay = TC_PENUP_POLL_TICKS;
  uint16_t value;
  uint16_t newx = 0;
  int16_t xdiff;
  int16_t ydiff;
  int ret;

  DEBUGASSERT(priv != NULL);

  /* Perform the next action based on the state of the conversions */

  switch (priv->state)
    {
    /* The touchscreen is IDLE and we are ready to begin the next sample */

    case TC_READY:
      {
        /* Select DRIVE for Y sampling */

        /* Configure XL, XR with drive voltages and disable YU drive.  Note that
         * this is configuring the DRIVEA and DRIVEB outputs to enable the on-board
         * transistor drive logic to energize the touch panel.
         */

        *((uint32_t *) LCD_TP_PORT_SETRESET) = LCD_SAMPY_BITS;

        /* Allow time for the Y DRIVE to settle */

        priv->resamplecount = 0;
        priv->sampcount = 0;
        priv->value = 0;
        priv->state = TC_READY_SETTLE;
        delay       = TC_SETTLE_TICKS;
      }
      break;

    case TC_READY_SETTLE:
      {
        /* Start Y sampling */

        tc_y_sample();

        /* Allow time for the Y pend down sampling */

        priv->state = TC_YPENDOWN;
        delay       = TC_SAMPLE_TICKS;
      }
      break;

    /* The Y sampling time has elapsed and the Y value should be ready
     * for conversion
     */

    case TC_YPENDOWN:
      {
        /* Convert the Y sample value */

        value = tc_adc_read_sample();

        /* A converted value at the minimum would mean that there is no touch
         * and that the sampling period is complete.
         */

        if (!tc_valid_sample(value))
          {
            priv->state = TC_PENUP;
          }
        else
          {
            /* Allow time for touch inputs to stabilize */

            priv->state = TC_DEBOUNCE;
            delay       = TC_DEBOUNCE_TICKS;
          }
      }
      break;

    /* The debounce time period has elapsed and we are ready to re-sample
     * the touchscreen.
     */

    case TC_RESAMPLE:
      {
        /* Select DRIVE for Y sampling */

        /* Configure XL, XR with drive voltages and disable YU drive.  Note that
         * this is configuring the DRIVEA and DRIVEB outputs to enable the on-board
         * transistor drive logic to energize the touch panel.
         */

        *((uint32_t *) LCD_TP_PORT_SETRESET) = LCD_SAMPY_BITS;

        /* Allow time for the Y DRIVE to settle */

        priv->state = TC_DEBOUNCE;
        delay       = TC_SETTLE_TICKS;
      }
      break;

    case TC_DEBOUNCE:
      {
        /* (Re-)start Y sampling */

        tc_y_sample();

        /* Allow time for the Y sampling */

        priv->state = TC_YSAMPLE;
        delay       = TC_SAMPLE_TICKS;
      }
      break;

    /* The Y sampling period has elapsed and we are ready to perform the
     * conversion.
     */

    case TC_YSAMPLE:                          /* Allowing time for the Y sampling */
      {
        /* Read the Y axis position */

        value = tc_adc_read_sample();

        /* A converted value at the minimum would mean that we lost the contact
         * before all of the conversions were completed.  At converted value at
         * the maximum value is probably bad too.
         */

        if (!tc_valid_sample(value))
          {
#ifdef CONFIG_TOUCHSCREEN_RESAMPLE
            priv->state = TC_RESAMPLE;
            delay       = TC_RESAMPLE_TICKS;
#else
            priv->state = TC_PENUP;
#endif
          }
        else
          {
            value       = MAX_ADC - value;
            priv->value += value;
            if (++priv->sampcount < CONFIG_TOUCHSCREEN_AVG_SAMPLES)
              {
                priv->state = TC_READY_SETTLE;
                delay = 1;
                break;
              }

            priv->newy  = value / CONFIG_TOUCHSCREEN_AVG_SAMPLES;
            priv->value = 0;
            priv->sampcount = 0;
            iinfo("Y=%d\n", priv->newy);

            /* Configure YU and YD with drive voltages and disable XR drive.  Note that
             * this is configuring the DRIVEA and DRIVEB outputs to enable the on-board
             * transistor drive logic to energize the touch panel.
             */

            *((uint32_t *) LCD_TP_PORT_SETRESET) = LCD_SAMPX_BITS;

            /* Allow time for the X sampling */

            priv->state = TC_XSETTLE;
            delay       = TC_SETTLE_TICKS;
          }
      }
      break;

    case TC_XRESAMPLE:                /* Perform X resampling */
      {
        if (priv->resamplecount-- == 0)
          {
            priv->state = TC_PENUP;
            break;
          }
      }

    case TC_XSETTLE:                  /* Allowing time X to settle after changing DRIVE */
      {
        /* The X Drive settling time has elaspsed and it's time to start
         * the conversion
         */

        /* Start X sampling */

        tc_x_sample();

        /* Allow time for the X sampling */

        priv->state = TC_XSAMPLE;
        delay       = TC_SAMPLE_TICKS;
      }
      break;

    case TC_XSAMPLE:                 /* Allowing time for the X sampling */
      {
        /* Read the converted X axis position */

        value = tc_adc_read_sample();

        /* A converted value at the minimum would mean that we lost the contact
         * before all of the conversions were completed.  At converted value at
         * the maximum value is probably bad too.
         */

        if (!tc_valid_sample(value))
          {
#ifdef CONFIG_TOUCHSCREEN_RESAMPLE
            priv->state = TC_XRESAMPLE;
            if (priv->resamplecount == 0)
                priv->resamplecount = 1;
            delay       = TC_RESAMPLE_TICKS;
#else
            priv->state = TC_PENUP;
#endif
          }
        else
          {
            /* Calculate the X axis position */

            //value = MAX_ADC - value;
            priv->value += value;
            if (++priv->sampcount < CONFIG_TOUCHSCREEN_AVG_SAMPLES)
              {
                priv->state = TC_XSETTLE;
                delay = 1;
                break;
              }

            newx  = value / CONFIG_TOUCHSCREEN_AVG_SAMPLES;
            iinfo("X=%d\n", newx);

            /* Samples are available */

            priv->state = TC_PENDOWN;
          }
      }
      break;
    }

  /* Check for terminal conditions.. */

  /* Check if the sampling resulted in a pen up decision.  If so, we need to
   * handle the change from pen down to pen up.
   */

  if (priv->state == TC_PENUP)
    {
      /* 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 &&
          priv->sample.contact != CONTACT_UP)
        {
          /* 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 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);
}
Exemple #18
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;
}
Exemple #19
0
Fichier : job.c Projet : dun/munge
void
job_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;
}
Exemple #20
0
void
Gimbal::cycle()
{
	if (!_initialized) {
		/* get subscription handles */
		_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
		_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
		_vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
		_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
		_params_sub = orb_subscribe(ORB_ID(parameter_update));

		/* get a publication handle on actuator output topic */
		struct actuator_controls_s zero_report;
		memset(&zero_report, 0, sizeof(zero_report));
		zero_report.timestamp = hrt_absolute_time();
		_actuator_controls_2_topic = orb_advertise(ORB_ID(actuator_controls_2), &zero_report);

		if (_actuator_controls_2_topic == nullptr) {
			warnx("advert err");
		}

		_mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT;

		_initialized = true;
	}

	bool	updated = false;

	perf_begin(_sample_perf);

	float roll = 0.0f;
	float pitch = 0.0f;
	float yaw = 0.0f;
	float out_mount_mode = 0.0f;

	/* Parameter update */
	bool params_updated;

	orb_check(_params_sub, &params_updated);

	if (params_updated) {
		struct parameter_update_s param_update;
		orb_copy(ORB_ID(parameter_update), _params_sub, &param_update); // XXX: is this actually necessary?

		update_params();
	}

	/* Control mode update */
	bool vehicle_control_mode_updated;

	orb_check(_vehicle_control_mode_sub, &vehicle_control_mode_updated);

	if (vehicle_control_mode_updated) {
		orb_copy(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode);
	}

	/* Check attitude */
	struct vehicle_attitude_s att;

	orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att);

	if (_attitude_compensation_roll) {
		roll = 1.0f / M_PI_F * -att.roll;
		updated = true;
	}

	if (_attitude_compensation_pitch) {
		pitch = 1.0f / M_PI_F * -att.pitch;
		updated = true;
	}

	if (_attitude_compensation_yaw) {
		yaw = 1.0f / M_PI_F * att.yaw;
		updated = true;
	}

	/* Check manual input */
	bool manual_updated;

	orb_check(_manual_control_sub, &manual_updated);

	if (manual_updated) {
		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual_control);

		/* only check manual input for mount mode when not in offboard and aux chan is configured */
		if (!_control_mode.flag_control_offboard_enabled && _parameters.aux_mnt_chn > 0) {
			float aux = 0.0f;

			switch (_parameters.aux_mnt_chn) {
			case 1:
				aux = _manual_control.aux1;
				break;

			case 2:
				aux = _manual_control.aux2;
				break;

			case 3:
				aux = _manual_control.aux3;
				break;
			}

			if (aux < 0.0f && _mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT) {
				_mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT;
				updated = true;
			}

			if (aux > 0.0f && _mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL) {
				_mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL;
				updated = true;
			}
		}
	}

	/* Check command input */
	struct vehicle_command_s cmd;

	bool cmd_updated;

	orb_check(_vehicle_command_sub, &cmd_updated);

	if (cmd_updated) {

		orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);

		if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL
		    || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) {

			_control_cmd = cmd;
			_control_cmd_set = true;

		} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) {

			_config_cmd = cmd;
			_config_cmd_set = true;

		}

	}

	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));
}
Exemple #21
0
void
BlinkM::led()
{

	static int vehicle_status_sub_fd;
	static int vehicle_gps_position_sub_fd;

	static int num_of_cells = 0;
	static int detected_cells_runcount = 0;

	static int t_led_color[8] = { 0, 0, 0, 0, 0, 0, 0, 0};
	static int t_led_blink = 0;
	static int led_thread_runcount=0;
	static int led_interval = 1000;

	static int no_data_vehicle_status = 0;
	static int no_data_vehicle_gps_position = 0;

	static bool topic_initialized = false;
	static bool detected_cells_blinked = false;
	static bool led_thread_ready = true;

	int num_of_used_sats = 0;

	if(!topic_initialized) {
		vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
		orb_set_interval(vehicle_status_sub_fd, 1000);

		vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
		orb_set_interval(vehicle_gps_position_sub_fd, 1000);

		topic_initialized = true;
	}

	if(led_thread_ready == true) {
		if(!detected_cells_blinked) {
			if(num_of_cells > 0) {
				t_led_color[0] = LED_PURPLE;
			}
			if(num_of_cells > 1) {
				t_led_color[1] = LED_PURPLE;
			}
			if(num_of_cells > 2) {
				t_led_color[2] = LED_PURPLE;
			}
			if(num_of_cells > 3) {
				t_led_color[3] = LED_PURPLE;
			}
			if(num_of_cells > 4) {
				t_led_color[4] = LED_PURPLE;
			}
			t_led_color[5] = LED_OFF;
			t_led_color[6] = LED_OFF;
			t_led_color[7] = LED_OFF;
			t_led_blink = LED_BLINK;
		} else {
			t_led_color[0] = led_color_1;
			t_led_color[1] = led_color_2;
			t_led_color[2] = led_color_3;
			t_led_color[3] = led_color_4;
			t_led_color[4] = led_color_5;
			t_led_color[5] = led_color_6;
			t_led_color[6] = led_color_7;
			t_led_color[7] = led_color_8;
			t_led_blink = led_blink;
		}
		led_thread_ready = false;
	}

	if (led_thread_runcount & 1) {
		if (t_led_blink)
			setLEDColor(LED_OFF);
		led_interval = LED_OFFTIME;
	} else {
		setLEDColor(t_led_color[(led_thread_runcount / 2) % 8]);
		//led_interval = (led_thread_runcount & 1) : LED_ONTIME;
		led_interval = LED_ONTIME;
	}

	if (led_thread_runcount == 15) {
		/* obtained data for the first file descriptor */
		struct vehicle_status_s vehicle_status_raw;
		struct vehicle_gps_position_s vehicle_gps_position_raw;

		memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
		memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));

		bool new_data_vehicle_status;
		bool new_data_vehicle_gps_position;

		orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);

		if (new_data_vehicle_status) {
			orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw);
			no_data_vehicle_status = 0;
		} else {
			no_data_vehicle_status++;
			if(no_data_vehicle_status >= 3)
				no_data_vehicle_status = 3;
		}

		orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position);

		if (new_data_vehicle_gps_position) {
			orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw);
			no_data_vehicle_gps_position = 0;
		} else {
			no_data_vehicle_gps_position++;
			if(no_data_vehicle_gps_position >= 3)
				no_data_vehicle_gps_position = 3;
		}



		/* get number of used satellites in navigation */
		num_of_used_sats = 0;
		//for(int satloop=0; satloop<20; satloop++) {
		for(int satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) {
			if(vehicle_gps_position_raw.satellite_used[satloop] == 1) {
				num_of_used_sats++;
			}
		}

		if(new_data_vehicle_status || no_data_vehicle_status < 3){
			if(num_of_cells == 0) {
				/* looking for lipo cells that are connected */
				printf("<blinkm> checking cells\n");
				for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
					if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break;
				}
				printf("<blinkm> cells found:%u\n", num_of_cells);

			} else {
				if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
					/* LED Pattern for battery low warning */
					led_color_1 = LED_YELLOW;
					led_color_2 = LED_YELLOW;
					led_color_3 = LED_YELLOW;
					led_color_4 = LED_YELLOW;
					led_color_5 = LED_YELLOW;
					led_color_6 = LED_YELLOW;
					led_color_7 = LED_YELLOW;
					led_color_8 = LED_YELLOW;
					led_blink = LED_BLINK;

				} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
					/* LED Pattern for battery critical alerting */
					led_color_1 = LED_RED;
					led_color_2 = LED_RED;
					led_color_3 = LED_RED;
					led_color_4 = LED_RED;
					led_color_5 = LED_RED;
					led_color_6 = LED_RED;
					led_color_7 = LED_RED;
					led_color_8 = LED_RED;
					led_blink = LED_BLINK;

				} else {
					/* no battery warnings here */

					if(vehicle_status_raw.flag_system_armed == false) {
						/* system not armed */
						led_color_1 = LED_RED;
						led_color_2 = LED_RED;
						led_color_3 = LED_RED;
						led_color_4 = LED_RED;
						led_color_5 = LED_RED;
						led_color_6 = LED_RED;
						led_color_7 = LED_RED;
						led_color_8 = LED_RED;
						led_blink = LED_NOBLINK;

					} else {
						/* armed system - initial led pattern */
						led_color_1 = LED_RED;
						led_color_2 = LED_RED;
						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´s */
							if(num_of_used_sats >= 7) {
								led_color_1 = LED_OFF;
								led_color_2 = LED_OFF;
								led_color_3 = LED_OFF;
							} else if(num_of_used_sats == 6) {
								led_color_2 = LED_OFF;
								led_color_3 = LED_OFF;
							} else if(num_of_used_sats == 5) {
								led_color_3 = LED_OFF;
							}

						} else {
							/* no vehicle_gps_position data */
							led_color_1 = LED_WHITE;
							led_color_2 = LED_WHITE;
							led_color_3 = LED_WHITE;

						}

					}
				}
			}
		} else {
			/* LED Pattern for general Error - no vehicle_status can retrieved */
			led_color_1 = LED_WHITE;
			led_color_2 = LED_WHITE;
			led_color_3 = LED_WHITE;
			led_color_4 = LED_WHITE;
			led_color_5 = LED_WHITE;
			led_color_6 = LED_WHITE;
			led_color_7 = LED_WHITE;
			led_color_8 = LED_WHITE;
			led_blink = LED_BLINK;

		}

		/*
		printf( "<blinkm> Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tNDVS:%4u\tNDSAT:%4u\tSats:%4u\tFix:%4u\tVisible:%4u\n",
		vehicle_status_raw.voltage_battery,
		vehicle_status_raw.flag_system_armed,
		vehicle_status_raw.flight_mode,
		num_of_cells,
		no_data_vehicle_status,
		no_data_vehicle_gps_position,
		num_of_used_sats,
		vehicle_gps_position_raw.fix_type,
		vehicle_gps_position_raw.satellites_visible);
		*/

		led_thread_runcount=0;
		led_thread_ready = true;
		led_interval = LED_OFFTIME;

		if(detected_cells_runcount < 4){
			detected_cells_runcount++;
		} else {
			detected_cells_blinked = true;
		}

	} else {
		led_thread_runcount++;
	}

	if(systemstate_run == true) {
		/* re-queue ourselves to run again later */
		work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval);
	} else {
		stop_script();
		set_rgb(0,0,0);
	}
}
Exemple #22
0
void
Gimbal::start()
{
	/* schedule a cycle to start things */
	work_queue(LPWORK, &_work, (worker_t)&Gimbal::cycle_trampoline, this, 1);
}
Exemple #23
0
int ads7843e_register(FAR struct spi_dev_s *spi,
                      FAR struct ads7843e_config_s *config, int minor)
{
  FAR struct ads7843e_dev_s *priv;
  char devname[DEV_NAMELEN];
#ifdef CONFIG_ADS7843E_MULTIPLE
  irqstate_t flags;
#endif
  int ret;

  ivdbg("spi: %p minor: %d\n", spi, minor);

  /* Debug-only sanity checks */

  DEBUGASSERT(spi != NULL && config != NULL && minor >= 0 && minor < 100);

  /* Create and initialize a ADS7843E device driver instance */

#ifndef CONFIG_ADS7843E_MULTIPLE
  priv = &g_ads7843e;
#else
  priv = (FAR struct ads7843e_dev_s *)kmalloc(sizeof(struct ads7843e_dev_s));
  if (!priv)
    {
      idbg("kmalloc(%d) failed\n", sizeof(struct ads7843e_dev_s));
      return -ENOMEM;
    }
#endif

  /* Initialize the ADS7843E device driver instance */

  memset(priv, 0, sizeof(struct ads7843e_dev_s));
  priv->spi     = spi;               /* Save the SPI device handle */
  priv->config  = config;            /* Save the board configuration */
  priv->wdog    = wd_create();       /* Create a watchdog timer */
  priv->threshx = INVALID_THRESHOLD; /* Initialize thresholding logic */
  priv->threshy = INVALID_THRESHOLD; /* Initialize thresholding logic */

  sem_init(&priv->devsem,  0, 1);    /* Initialize device structure semaphore */
  sem_init(&priv->waitsem, 0, 0);    /* Initialize pen event wait semaphore */

  /* Make sure that interrupts are disabled */

  config->clear(config);
  config->enable(config, false);

  /* Attach the interrupt handler */

  ret = config->attach(config, ads7843e_interrupt);
  if (ret < 0)
    {
      idbg("Failed to attach interrupt\n");
      goto errout_with_priv;
    }

  idbg("Mode: %d Bits: 8 Frequency: %d\n",
       CONFIG_ADS7843E_SPIMODE, CONFIG_ADS7843E_FREQUENCY);

  /* Lock the SPI bus so that we have exclusive access */

  ads7843e_lock(spi);

  /* Configure the SPI interface */

  ads7843e_configspi(spi);

  /* Enable the PEN IRQ */

  ads7843e_sendcmd(priv, ADS7843_CMD_ENABPENIRQ);

  /* Unlock the bus */

  ads7843e_unlock(spi);

  /* Register the device as an input device */

  (void)snprintf(devname, DEV_NAMELEN, DEV_FORMAT, minor);
  ivdbg("Registering %s\n", devname);

  ret = register_driver(devname, &ads7843e_fops, 0666, priv);
  if (ret < 0)
    {
      idbg("register_driver() failed: %d\n", ret);
      goto errout_with_priv;
    }

  /* If multiple ADS7843E devices are supported, then we will need to add
   * this new instance to a list of device instances so that it can be
   * found by the interrupt handler based on the recieved IRQ number.
   */

#ifdef CONFIG_ADS7843E_MULTIPLE
  priv->flink    = g_ads7843elist;
  g_ads7843elist = priv;
  irqrestore(flags);
#endif

  /* Schedule work to perform the initial sampling and to set the data
   * availability conditions.
   */

  ret = work_queue(HPWORK, &priv->work, ads7843e_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_ADS7843E_MULTIPLE
  kfree(priv);
#endif
  return ret;
}
Exemple #24
0
int gpio_led_main(int argc, char *argv[])
{
	if (argc < 2) {
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
		errx(1, "usage: gpio_led {start|stop} [-p <1|2|a1|a2|r1|r2>]\n"
		     "\t-p\tUse pin:\n"
		     "\t\t1\tPX4FMU GPIO_EXT1 (default)\n"
		     "\t\t2\tPX4FMU GPIO_EXT2\n"
		     "\t\ta1\tPX4IO ACC1\n"
		     "\t\ta2\tPX4IO ACC2\n"
		     "\t\tr1\tPX4IO RELAY1\n"
		     "\t\tr2\tPX4IO RELAY2"
		    );
#endif
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
		errx(1, "usage: gpio_led {start|stop} [-p <n>]\n"
		     "\t-p <n>\tUse specified AUX OUT pin number (default: 1)"
		    );
#endif

	} else {

		if (!strcmp(argv[1], "start")) {
			if (gpio_led_started) {
				errx(1, "already running");
			}

			bool use_io = false;

			/* by default use GPIO_EXT_1 on FMUv1 and GPIO_SERVO_1 on FMUv2 */
			int pin = 1;

			/* pin name to display */
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
			char *pin_name = "PX4FMU GPIO_EXT1";
#endif
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
			char pin_name[] = "AUX OUT 1";
#endif

			if (argc > 2) {
				if (!strcmp(argv[2], "-p")) {
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1

					if (!strcmp(argv[3], "1")) {
						use_io = false;
						pin = GPIO_EXT_1;
						pin_name = "PX4FMU GPIO_EXT1";

					} else if (!strcmp(argv[3], "2")) {
						use_io = false;
						pin = GPIO_EXT_2;
						pin_name = "PX4FMU GPIO_EXT2";

					} else if (!strcmp(argv[3], "a1")) {
						use_io = true;
						pin = PX4IO_P_SETUP_RELAYS_ACC1;
						pin_name = "PX4IO ACC1";

					} else if (!strcmp(argv[3], "a2")) {
						use_io = true;
						pin = PX4IO_P_SETUP_RELAYS_ACC2;
						pin_name = "PX4IO ACC2";

					} else if (!strcmp(argv[3], "r1")) {
						use_io = true;
						pin = PX4IO_P_SETUP_RELAYS_POWER1;
						pin_name = "PX4IO RELAY1";

					} else if (!strcmp(argv[3], "r2")) {
						use_io = true;
						pin = PX4IO_P_SETUP_RELAYS_POWER2;
						pin_name = "PX4IO RELAY2";

					} else {
						errx(1, "unsupported pin: %s", argv[3]);
					}

#endif
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
					unsigned int n = strtoul(argv[3], NULL, 10);

					if (n >= 1 && n <= 6) {
						use_io = false;
						pin = 1 << (n - 1);
						snprintf(pin_name, sizeof(pin_name), "AUX OUT %d", n);

					} else {
						errx(1, "unsupported pin: %s", argv[3]);
					}

#endif
				}
			}

			gpio_led_data = malloc(sizeof(struct gpio_led_s));
			memset(gpio_led_data, 0, sizeof(struct gpio_led_s));
			gpio_led_data->use_io = use_io;
			gpio_led_data->pin = pin;
			int ret = work_queue(LPWORK, &(gpio_led_data->work), gpio_led_start, gpio_led_data, 0);

			if (ret != 0) {
				errx(1, "failed to queue work: %d", ret);

			} else {
				gpio_led_started = true;
				warnx("start, using pin: %s", pin_name);
				exit(0);
			}

		} else if (!strcmp(argv[1], "stop")) {
			if (gpio_led_started) {
				gpio_led_started = false;
				warnx("stop");
				exit(0);

			} else {
				errx(1, "not running");
			}

		} else {
			errx(1, "unrecognized command '%s', only supporting 'start' or 'stop'", argv[1]);
		}
	}
}
Exemple #25
0
/**
 * Main loop function
 */
void
PCA9685::i2cpwm()
{
	if (_mode == IOX_MODE_TEST_OUT) {
		setPin(0, PCA9685_PWMCENTER);
		_should_run = true;

	} else if (_mode == IOX_MODE_OFF) {
		_should_run = false;

	} else {
		if (!_mode_on_initialized) {
			// Init PWM limits
			pwm_limit_init(&_pwm_limit);

			// Get arming state
			_armed_sub = orb_subscribe(ORB_ID(actuator_armed));

			/* Subscribe to actuator groups */
			subscribe();

			/* set the uorb update interval lower than the driver pwm interval */
			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; ++i) {
				orb_set_interval(_control_subs[i], 1000.0f / PCA9685_PWMFREQ - 5);
			}

			_mode_on_initialized = true;
		}

		/* check if anything updated */
		int ret = ::poll(_poll_fds, _poll_fds_num, 0);

		if (ret < 0) {
			DEVICE_LOG("poll error %d", errno);

		} else if (ret == 0) {
	//			warnx("no PWM: failsafe");
		} else {
			/* get controls for required topics */
			unsigned poll_id = 0;

			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
				if (_control_subs[i] > 0) {
					if (_poll_fds[poll_id].revents & POLLIN) {
						orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
					}
				}
				poll_id++;
			}

			if (_mixers != nullptr) {
				size_t num_outputs = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS;

				// do mixing
				num_outputs = _mixers->mix(_outputs, num_outputs, NULL);

				// disable unused ports by setting their output to NaN
				for (size_t i = 0; i < sizeof(_outputs) / sizeof(_outputs[0]); i++) {
					if (i >= num_outputs) {
						_outputs[i] = NAN_VALUE;
					}
				}

				// Finally, write servo values to motors
				for (int i = 0; i < num_outputs; i++) {
					uint16_t new_value = PCA9685_PWMCENTER +
								 (_outputs[i] * M_PI_F * PCA9685_SCALE);
					// DEVICE_DEBUG("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value,
					//      (double)_controls[1].control[i]);

					if (isfinite(new_value) &&
						new_value >= PCA9685_PWMMIN &&
						new_value <= PCA9685_PWMMAX) {

						setPin(i, new_value);
						_rates[i] = new_value;
					}
				}
			}
		}

		bool updated;

		// Update Arming state
		orb_check(_armed_sub, &updated);
		if (updated) {
			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);

			bool set_armed = (_armed.armed || _armed.prearmed) && !_armed.lockdown;

			if (_servo_armed != set_armed) {
				_servo_armed = set_armed;
			}
		}

		// Update AUX controls update
		// orb_check(_actuator_controls_sub, &updated);
		// if (updated) {
		// 	size_t num_outputs = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS;
		//
		// 	// Get updated actuator
		// 	// Only update actuator 1 for now
		// 	orb_copy(ORB_ID(actuator_controls_1), _actuator_controls_sub, &_controls[1]);
		//
		//
		// }

		_should_run = true;
	}

	// check if any activity remains, else stop
	if (!_should_run) {
		_running = false;
		return;
	}

	// re-queue ourselves to run again later
	_running = true;
	work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, _i2cpwm_interval);
}
Exemple #26
0
void
InputPWM::cycle()
{

	int ret;

	perf_begin(_sample_perf);

	struct rc_input_values report;

	/* copy the current pwm values by reading them in */
	ret = read(0, (char *)&report, sizeof(report));
	if (ret < 0) {
		/* couldn't read data, make empty report */
		report.timestamp_publication = hrt_absolute_time();
		report.channel_count = 0;
		report.rssi = 0;
		report.input_source = RC_INPUT_SOURCE_UNKNOWN;
		if (_reports->force(&report)){
			perf_count(_buffer_overflows);
		}
		perf_end(_sample_perf);
		return;
	}

	if (_reports->force(&report)) {
		perf_count(_buffer_overflows);
	}

	perf_end(_sample_perf);

	// only _timer_index = 0 should publish a report of all rc inputs
	if (_timer_index == 0) {
		int decoded_channels = 0;
		report.timestamp_publication = hrt_absolute_time();
		report.timestamp_last_signal = report.timestamp_publication;
		report.rssi = 255;
		report.rc_failsafe = false;
		report.rc_lost = false;
		report.rc_lost_frame_count = 0;
		report.rc_total_frame_count = 0;
		report.rc_ppm_frame_length = 0;
		report.input_source = RC_INPUT_SOURCE_UNKNOWN;
		/* find out how many are actually active */
		for (int i=0;i < INPUT_PWM_MAX_CHANNELS; i++) {
			if (input_pwm_channel_decoded[i]) {
				report.values[i] = input_pwm_channel_values[i];
				decoded_channels++;
			}
			else
				report.values[i] = 0;
		}
		report.channel_count = decoded_channels;

		/* publish the data */
		orb_publish(ORB_ID(input_rc), _input_rc_topic, &report);
		/* notify anyone waiting for the data */
		poll_notify(POLLIN);
	}

	/* schedule a fresh cycle call */
	work_queue(HPWORK,
		   &_work,
		   (worker_t)&InputPWM::cycle_trampoline,
		   this,
		   _measure_ticks);

}
Exemple #27
0
int max11802_register(FAR struct spi_dev_s *spi,
                      FAR struct max11802_config_s *config, int minor)
{
  FAR struct max11802_dev_s *priv;
  char devname[DEV_NAMELEN];
#ifdef CONFIG_MAX11802_MULTIPLE
  irqstate_t flags;
#endif
  int ret;

  iinfo("spi: %p minor: %d\n", spi, minor);

  /* Debug-only sanity checks */

  DEBUGASSERT(spi != NULL && config != NULL && minor >= 0 && minor < 100);

  /* Create and initialize a MAX11802 device driver instance */

#ifndef CONFIG_MAX11802_MULTIPLE
  priv = &g_max11802;
#else
  priv = (FAR struct max11802_dev_s *)kmm_malloc(sizeof(struct max11802_dev_s));
  if (!priv)
    {
      ierr("ERROR: kmm_malloc(%d) failed\n", sizeof(struct max11802_dev_s));
      return -ENOMEM;
    }
#endif

  /* Initialize the MAX11802 device driver instance */

  memset(priv, 0, sizeof(struct max11802_dev_s));
  priv->spi     = spi;               /* Save the SPI device handle */
  priv->config  = config;            /* Save the board configuration */
  priv->wdog    = wd_create();       /* Create a watchdog timer */
  priv->threshx = INVALID_THRESHOLD; /* Initialize thresholding logic */
  priv->threshy = INVALID_THRESHOLD; /* Initialize thresholding logic */

  /* Initialize semaphores */

  sem_init(&priv->devsem,  0, 1);    /* Initialize device structure semaphore */
  sem_init(&priv->waitsem, 0, 0);    /* Initialize pen event wait semaphore */

  /* The pen event semaphore is used for signaling and, hence, should not
   * have priority inheritance enabled.
   */

  sem_setprotocol(&priv->waitsem, SEM_PRIO_NONE);

  /* Make sure that interrupts are disabled */

  config->clear(config);
  config->enable(config, false);

  /* Attach the interrupt handler */

  ret = config->attach(config, max11802_interrupt);
  if (ret < 0)
    {
      ierr("ERROR: Failed to attach interrupt\n");
      goto errout_with_priv;
    }

  iinfo("Mode: %d Bits: 8 Frequency: %d\n",
        CONFIG_MAX11802_SPIMODE, CONFIG_MAX11802_FREQUENCY);

  /* Lock the SPI bus so that we have exclusive access */

  max11802_lock(spi);

  /* Configure MAX11802 registers */

  SPI_SELECT(priv->spi, SPIDEV_TOUCHSCREEN, true);
  (void)SPI_SEND(priv->spi, MAX11802_CMD_MODE_WR);
  (void)SPI_SEND(priv->spi, MAX11802_MODE);
  SPI_SELECT(priv->spi, SPIDEV_TOUCHSCREEN, false);

  SPI_SELECT(priv->spi, SPIDEV_TOUCHSCREEN, true);
  (void)SPI_SEND(priv->spi, MAX11802_CMD_AVG_WR);
  (void)SPI_SEND(priv->spi, MAX11802_AVG);
  SPI_SELECT(priv->spi, SPIDEV_TOUCHSCREEN, false);

  SPI_SELECT(priv->spi, SPIDEV_TOUCHSCREEN, true);
  (void)SPI_SEND(priv->spi, MAX11802_CMD_TIMING_WR);
  (void)SPI_SEND(priv->spi, MAX11802_TIMING);
  SPI_SELECT(priv->spi, SPIDEV_TOUCHSCREEN, false);

  SPI_SELECT(priv->spi, SPIDEV_TOUCHSCREEN, true);
  (void)SPI_SEND(priv->spi, MAX11802_CMD_DELAY_WR);
  (void)SPI_SEND(priv->spi, MAX11802_DELAY);
  SPI_SELECT(priv->spi, SPIDEV_TOUCHSCREEN, false);

  /* Test that the device access was successful. */

  SPI_SELECT(priv->spi, SPIDEV_TOUCHSCREEN, true);
  (void)SPI_SEND(priv->spi, MAX11802_CMD_MODE_RD);
  ret = SPI_SEND(priv->spi, 0);
  SPI_SELECT(priv->spi, SPIDEV_TOUCHSCREEN, false);

  /* Unlock the bus */

  max11802_unlock(spi);

  if (ret != MAX11802_MODE)
  {
    ierr("ERROR: max11802 mode readback failed: %02x\n", ret);
    goto errout_with_priv;
  }

  /* Register the device as an input device */

  (void)snprintf(devname, DEV_NAMELEN, DEV_FORMAT, minor);
  iinfo("Registering %s\n", devname);

  ret = register_driver(devname, &max11802_fops, 0666, priv);
  if (ret < 0)
    {
      ierr("ERROR: register_driver() failed: %d\n", ret);
      goto errout_with_priv;
    }

  /* If multiple MAX11802 devices are supported, then we will need to add
   * this new instance to a list of device instances so that it can be
   * found by the interrupt handler based on the recieved IRQ number.
   */

#ifdef CONFIG_MAX11802_MULTIPLE
  flags          = enter_critical_section();
  priv->flink    = g_max11802list;
  g_max11802list = priv;
  leave_critical_section(flags);
#endif

  /* Schedule work to perform the initial sampling and to set the data
   * availability conditions.
   */

  ret = work_queue(HPWORK, &priv->work, max11802_worker, priv, 0);
  if (ret != 0)
    {
      ierr("ERROR: 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_MAX11802_MULTIPLE
  kmm_free(priv);
#endif
  return ret;
}
Exemple #28
0
/**
 * Set mode, if mode not changed has no any effect (doesn't reset blinks phase)
 */
void
RGBLED::set_mode(rgbled_mode_t mode)
{
	if (mode != _mode) {
		_mode = mode;

		switch (mode) {
		case RGBLED_MODE_OFF:
			_should_run = false;
			send_led_enable(false);
			break;

		case RGBLED_MODE_ON:
			_brightness = 1.0f;
			send_led_rgb();
			send_led_enable(true);
			break;

		case RGBLED_MODE_BLINK_SLOW:
			_should_run = true;
			_counter = 0;
			_led_interval = 2000;
			_brightness = 1.0f;
			send_led_rgb();
			break;

		case RGBLED_MODE_BLINK_NORMAL:
			_should_run = true;
			_counter = 0;
			_led_interval = 500;
			_brightness = 1.0f;
			send_led_rgb();
			break;

		case RGBLED_MODE_BLINK_FAST:
			_should_run = true;
			_counter = 0;
			_led_interval = 100;
			_brightness = 1.0f;
			send_led_rgb();
			break;

		case RGBLED_MODE_BREATHE:
			_should_run = true;
			_counter = 0;
			_led_interval = 25;
			send_led_enable(true);
			break;

		case RGBLED_MODE_PATTERN:
			_should_run = true;
			_counter = 0;
			_brightness = 1.0f;
			send_led_enable(true);
			break;

		default:
			warnx("mode unknown");
			break;
		}

		/* if it should run now, start the workq */
		if (_should_run && !_running) {
			_running = true;
			work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
		}

	}
}
Exemple #29
0
void
Gimbal::cycle()
{
	if (!_initialized) {
		/* get a subscription handle on the vehicle command topic */
		_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));

		/* get a publication handle on actuator output topic */
		struct actuator_controls_s zero_report;
		memset(&zero_report, 0, sizeof(zero_report));
		zero_report.timestamp = hrt_absolute_time();
		_actuator_controls_2_topic = orb_advertise(ORB_ID(actuator_controls_2), &zero_report);

		if (_actuator_controls_2_topic < 0) {
			warnx("advert err");
		}

		_initialized = true;
	}

	bool	updated = false;

	perf_begin(_sample_perf);

	float roll = 0.0f;
	float pitch = 0.0f;
	float yaw = 0.0f;


	if (_attitude_compensation) {
		if (_att_sub < 0) {
			_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
		}

		vehicle_attitude_s att;

		orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att);

		roll = -att.roll;
		pitch = -att.pitch;
		yaw = att.yaw;

		updated = true;
	}

	struct vehicle_command_s cmd;

	bool cmd_updated;

	orb_check(_vehicle_command_sub, &cmd_updated);

	if (cmd_updated) {

		orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);

		VEHICLE_MOUNT_MODE mountMode = (VEHICLE_MOUNT_MODE)cmd.param7;
		debug("cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", cmd.command, mountMode, (double)cmd.param1, (double)cmd.param2);

		if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL && mountMode == 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 * cmd.param1;
			pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param2;
			yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param3;
			
			updated = true;

		}

		if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
			float gimbalDirectionQuat[] = {cmd.param1, cmd.param2, cmd.param3, cmd.param4};
			math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler();

			roll += gimablDirectionEuler(0);
			pitch += gimablDirectionEuler(1);
			yaw += gimablDirectionEuler(2);

			updated = true;
		}
	}

	if (updated) {

		struct actuator_controls_s controls;

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

		/* 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));
}
Exemple #30
0
void
BATT_SMBUS::cycle()
{
	// get current time
	uint64_t now = hrt_absolute_time();

	// exit without rescheduling if we have failed to find a battery after 10 seconds
	if (!_enabled && (now - _start_time > BATT_SMBUS_TIMEOUT_MS)) {
		warnx("did not find smart battery");
		return;
	}

	// read data from sensor
	struct battery_status_s new_report;

	// set time of reading
	new_report.timestamp = now;

	// read voltage
	uint16_t tmp;

	if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) {
		// initialise new_report
		memset(&new_report, 0, sizeof(new_report));

		// convert millivolts to volts
		new_report.voltage_v = ((float)tmp) / 1000.0f;

		// read current
		uint8_t buff[4];

		if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) {
			new_report.current_a = -(float)((int32_t)((uint32_t)buff[3] << 24 | (uint32_t)buff[2] << 16 | (uint32_t)buff[1] << 8 |
							(uint32_t)buff[0])) / 1000.0f;
		}

		// read battery design capacity
		if (_batt_capacity == 0) {
			if (read_reg(BATT_SMBUS_FULL_CHARGE_CAPACITY, tmp) == OK) {
				_batt_capacity = tmp;
			}
		}

		// read remaining capacity
		if (_batt_capacity > 0) {
			if (read_reg(BATT_SMBUS_REMAINING_CAPACITY, tmp) == OK) {
				if (tmp < _batt_capacity) {
					new_report.discharged_mah = _batt_capacity - tmp;
				}
			}
		}

		// publish to orb
		if (_batt_topic != nullptr) {
			orb_publish(_batt_orb_id, _batt_topic, &new_report);

		} else {
			_batt_topic = orb_advertise(_batt_orb_id, &new_report);

			if (_batt_topic == nullptr) {
				errx(1, "ADVERT FAIL");
			}
		}

		// copy report for test()
		_last_report = new_report;

		// post a report to the ring
		_reports->force(&new_report);

		// notify anyone waiting for data
		poll_notify(POLLIN);

		// record we are working
		_enabled = true;
	}

	// schedule a fresh cycle call when the measurement is done
	work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this,
		   USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_MS));
}