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; }
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)); } }
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); } }
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)); } }
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); } }
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; }
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)); }
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); } }
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); } }
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)); }
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; }
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)); }
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; }
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); }
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); }
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); }
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; }
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; }
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, ¶ms_updated); if (params_updated) { struct parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_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)); }
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); } }
void Gimbal::start() { /* schedule a cycle to start things */ work_queue(LPWORK, &_work, (worker_t)&Gimbal::cycle_trampoline, this, 1); }
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; }
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]); } } }
/** * 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); }
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); }
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; }
/** * 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); } } }
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)); }
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)); }