int MPU9250::reset() { irqstate_t state; /* When the mpu9250 starts from 0V the internal power on circuit * per the data sheet will require: * * Start-up time for register read/write From power-up Typ:11 max:100 ms * */ px4_usleep(110000); // Hold off sampling until done (100 MS will be shortened) state = px4_enter_critical_section(); _reset_wait = hrt_absolute_time() + 100000; px4_leave_critical_section(state); int ret; ret = reset_mpu(); if (ret == OK && (_whoami == MPU_WHOAMI_9250)) { ret = _mag->ak8963_reset(); } state = px4_enter_critical_section(); _reset_wait = hrt_absolute_time() + 10; px4_leave_critical_section(state); return ret; }
int test_time(int argc, char *argv[]) { hrt_abstime h, c; int64_t lowdelta, maxdelta = 0; int64_t delta, deltadelta; /* enable the cycle counter */ (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ /* get an average delta between the two clocks - this should stay roughly the same */ delta = 0; for (unsigned i = 0; i < 100; i++) { uint32_t flags = px4_enter_critical_section(); h = hrt_absolute_time(); c = cycletime(); px4_leave_critical_section(flags); delta += h - c; } lowdelta = abs(delta / 100); /* loop checking the time */ for (unsigned i = 0; i < 100; i++) { usleep(rand()); uint32_t flags = px4_enter_critical_section(); c = cycletime(); h = hrt_absolute_time(); px4_leave_critical_section(flags); delta = abs(h - c); deltadelta = abs(delta - lowdelta); if (deltadelta > maxdelta) { maxdelta = deltadelta; } if (deltadelta > 1000) { fprintf(stderr, "h %" PRIu64 " c %" PRIu64 " d %" PRId64 "\n", h, c, delta - lowdelta); } } printf("Maximum jitter %" PRId64 "us\n", maxdelta); return 0; }
static void param_bus_lock(bool lock) { #if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) // FMUv4 has baro and FRAM on the same bus, // as this offers on average a 100% silent // bus for the baro operation // XXX this would be the preferred locking method // if (dev == nullptr) { // dev = px4_spibus_initialize(PX4_SPI_BUS_BARO); // } // SPI_LOCK(dev, lock); // we lock like this for Pixracer for now if (lock) { irq_state = px4_enter_critical_section(); } else { px4_leave_critical_section(irq_state); } #endif }
/* * Initialise the timer we are going to use. */ void PWMIN::_timer_init(void) { /* run with interrupts disabled in case the timer is already * setup. We don't want it firing while we are doing the setup */ irqstate_t flags = px4_enter_critical_section(); /* configure input pin */ px4_arch_configgpio(GPIO_PWM_IN); // XXX refactor this out of this driver /* configure reset pin */ px4_arch_configgpio(GPIO_VDD_RANGEFINDER_EN); /* claim our interrupt vector */ irq_attach(PWMIN_TIMER_VECTOR, pwmin_tim_isr, NULL); /* Clear no bits, set timer enable bit.*/ modifyreg32(PWMIN_TIMER_POWER_REG, 0, PWMIN_TIMER_POWER_BIT); /* disable and configure the timer */ rCR1 = 0; rCR2 = 0; rSMCR = 0; rDIER = DIER_PWMIN_A; rCCER = 0; /* unlock CCMR* registers */ rCCMR1 = CCMR1_PWMIN; rCCMR2 = CCMR2_PWMIN; rSMCR = SMCR_PWMIN_1; /* Set up mode */ rSMCR = SMCR_PWMIN_2; /* Enable slave mode controller */ rCCER = CCER_PWMIN; rDCR = 0; /* for simplicity scale by the clock in MHz. This gives us * readings in microseconds which is typically what is needed * for a PWM input driver */ uint32_t prescaler = PWMIN_TIMER_CLOCK / 1000000UL; /* * define the clock speed. We want the highest possible clock * speed that avoids overflows. */ rPSC = prescaler - 1; /* run the full span of the counter. All timers can handle * uint16 */ rARR = UINT16_MAX; /* generate an update event; reloads the counter, all registers */ rEGR = GTIM_EGR_UG; /* enable the timer */ rCR1 = GTIM_CR1_CEN; /* enable interrupts */ up_enable_irq(PWMIN_TIMER_VECTOR); px4_leave_critical_section(flags); _timer_started = true; }
int LidarLiteI2C::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) { return -EINVAL; } irqstate_t flags = px4_enter_critical_section(); if (!_reports->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } px4_leave_critical_section(flags); return OK; } case SENSORIOCGQUEUEDEPTH: return _reports->size(); default: { int result = LidarLite::ioctl(filp, cmd, arg); if (result == -EINVAL) { result = I2C::ioctl(filp, cmd, arg); } return result; } } }
static void hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg) { irqstate_t flags = px4_enter_critical_section(); /* if the entry is currently queued, remove it */ /* note that we are using a potentially uninitialised entry->link here, but it is safe as sq_rem() doesn't dereference the passed node unless it is found in the list. So we potentially waste a bit of time searching the queue for the uninitialised entry->link but we don't do anything actually unsafe. */ if (entry->deadline != 0) { sq_rem(&entry->link, &callout_queue); } entry->deadline = deadline; entry->period = interval; entry->callout = callout; entry->arg = arg; hrt_call_enter(entry); px4_leave_critical_section(flags); }
int BMI160::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { /* these are shared with the accel side */ case SENSORIOCSPOLLRATE: case SENSORIOCGPOLLRATE: case SENSORIOCRESET: return ioctl(filp, cmd, arg); case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) { return -EINVAL; } irqstate_t flags = px4_enter_critical_section(); if (!_gyro_reports->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } px4_leave_critical_section(flags); return OK; } case GYROIOCGSAMPLERATE: return _gyro_sample_rate; case GYROIOCSSAMPLERATE: return gyro_set_sample_rate(arg); case GYROIOCSSCALE: /* copy scale in */ memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale)); return OK; case GYROIOCGSCALE: /* copy scale out */ memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); return OK; case GYROIOCSRANGE: return set_gyro_range(arg); case GYROIOCGRANGE: return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); } }
ssize_t uORB::DeviceNode::read(struct file *filp, char *buffer, size_t buflen) { SubscriberData *sd = (SubscriberData *)filp_to_sd(filp); /* if the object has not been written yet, return zero */ if (_data == nullptr) { return 0; } /* if the caller's buffer is the wrong size, that's an error */ if (buflen != _meta->o_size) { return -EIO; } /* * Perform an atomic copy & state update */ irqstate_t flags = px4_enter_critical_section(); if (_generation > sd->generation + _queue_size) { /* Reader is too far behind: some messages are lost */ _lost_messages += _generation - (sd->generation + _queue_size); sd->generation = _generation - _queue_size; } if (_generation == sd->generation && sd->generation > 0) { /* The subscriber already read the latest message, but nothing new was published yet. * Return the previous message */ --sd->generation; } /* if the caller doesn't want the data, don't give it to them */ if (nullptr != buffer) { memcpy(buffer, _data + (_meta->o_size * (sd->generation % _queue_size)), _meta->o_size); } if (sd->generation < _generation) { ++sd->generation; } /* set priority */ sd->set_priority(_priority); /* * Clear the flag that indicates that an update has been reported, as * we have just collected it. */ sd->set_update_reported(false); px4_leave_critical_section(flags); return _meta->o_size; }
/** * Store the absolute time in an interrupt-safe fashion */ hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now) { irqstate_t flags = px4_enter_critical_section(); hrt_abstime ts = hrt_absolute_time(); px4_leave_critical_section(flags); return ts; }
/** * Compare a time value with the current time as atomic operation */ hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then) { irqstate_t flags = px4_enter_critical_section(); hrt_abstime delta = hrt_absolute_time() - *then; px4_leave_critical_section(flags); return delta; }
ssize_t uORB::DeviceNode::write(struct file *filp, const char *buffer, size_t buflen) { /* * Writes are legal from interrupt context as long as the * object has already been initialised from thread context. * * Writes outside interrupt context will allocate the object * if it has not yet been allocated. * * Note that filp will usually be NULL. */ if (nullptr == _data) { if (!up_interrupt_context()) { lock(); /* re-check size */ if (nullptr == _data) { _data = new uint8_t[_meta->o_size * _queue_size]; } unlock(); } /* failed or could not allocate */ if (nullptr == _data) { return -ENOMEM; } } /* If write size does not match, that is an error */ if (_meta->o_size != buflen) { return -EIO; } /* Perform an atomic copy. */ irqstate_t flags = px4_enter_critical_section(); memcpy(_data + (_meta->o_size * (_generation % _queue_size)), buffer, _meta->o_size); /* update the timestamp and generation count */ _last_update = hrt_absolute_time(); /* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */ _generation++; _published = true; px4_leave_critical_section(flags); /* notify any poll waiters */ poll_notify(POLLIN); return _meta->o_size; }
void CDev::poll_notify(pollevent_t events) { /* lock against poll() as well as other wakeups */ irqstate_t state = px4_enter_critical_section(); for (unsigned i = 0; i < _max_pollwaiters; i++) if (nullptr != _pollset[i]) { poll_notify_one(_pollset[i], events); } px4_leave_critical_section(state); }
/** * Remove the entry from the callout list. */ void hrt_cancel(struct hrt_call *entry) { irqstate_t flags = px4_enter_critical_section(); sq_rem(&entry->link, &callout_queue); entry->deadline = 0; /* if this is a periodic call being removed by the callout, prevent it from * being re-entered when the callout returns. */ entry->period = 0; px4_leave_critical_section(flags); }
ssize_t ADC::read(file *filp, char *buffer, size_t len) { const size_t maxsize = sizeof(px4_adc_msg_t) * _channel_count; if (len > maxsize) { len = maxsize; } /* block interrupts while copying samples to avoid racing with an update */ irqstate_t flags = px4_enter_critical_section(); memcpy(buffer, _samples, len); px4_leave_critical_section(flags); return len; }
static void led_pwm_timer_init_timer(unsigned timer) { irqstate_t flags = px4_enter_critical_section(); /* enable the timer clock before we try to talk to it */ modifyreg32(led_pwm_timers[timer].clock_register, 0, led_pwm_timers[timer].clock_bit); /* disable and configure the timer */ rCR1(timer) = 0; rCR2(timer) = 0; rSMCR(timer) = 0; rDIER(timer) = 0; rCCER(timer) = 0; rCCMR1(timer) = 0; rCCMR2(timer) = 0; rCCR1(timer) = 0; rCCR2(timer) = 0; rCCR3(timer) = 0; rCCR4(timer) = 0; rCCER(timer) = 0; rDCR(timer) = 0; if ((led_pwm_timers[timer].base == STM32_TIM1_BASE) || (led_pwm_timers[timer].base == STM32_TIM8_BASE)) { /* master output enable = on */ rBDTR(timer) = ATIM_BDTR_MOE; } /* If the timer clock source provided as clock_freq is the STM32_APBx_TIMx_CLKIN * then configure the timer to free-run at 1MHz. * Otherwise, other frequencies are attainable by adjusting .clock_freq accordingly. */ rPSC(timer) = (led_pwm_timers[timer].clock_freq / 1000000) - 1; /* configure the timer to update at the desired rate */ rARR(timer) = (1000000 / LED_PWM_RATE) - 1; /* generate an update event; reloads the counter and all registers */ rEGR(timer) = GTIM_EGR_UG; px4_leave_critical_section(flags); }
int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { case BAROIOCSMSLPRESSURE: { if ((arg < 80000) || (arg > 120000)) { return -EINVAL; } else { DEVICE_LOG("new msl pressure %u", _msl_pressure); _msl_pressure = arg; return OK; } } case BAROIOCGMSLPRESSURE: { return _msl_pressure; } case SENSORIOCSPOLLRATE: { // not supported yet, pretend that everything is ok return OK; } case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) { return -EINVAL; } irqstate_t flags = px4_enter_critical_section(); if (!_reports.resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } px4_leave_critical_section(flags); return OK; } default: { return CDev::ioctl(filp, cmd, arg); } } }
/** * Fetch a never-wrapping absolute time value in microseconds from * some arbitrary epoch shortly after system start. */ hrt_abstime hrt_absolute_time(void) { hrt_abstime abstime; uint32_t count; irqstate_t flags; /* * Counter state. Marked volatile as they may change * inside this routine but outside the irqsave/restore * pair. Discourage the compiler from moving loads/stores * to these outside of the protected range. */ static volatile hrt_abstime base_time; static volatile uint32_t last_count; /* prevent re-entry */ flags = px4_enter_critical_section(); /* get the current counter value */ count = rCNT; /* * Determine whether the counter has wrapped since the * last time we're called. * * This simple test is sufficient due to the guarantee that * we are always called at least once per counter period. */ if (count < last_count) { base_time += HRT_COUNTER_PERIOD; } /* save the count for next time */ last_count = count; /* compute the current time */ abstime = HRT_COUNTER_SCALE(base_time + count); px4_leave_critical_section(flags); return abstime; }
__END_DECLS /************************************************************************************ * Name: board_rc_input * * Description: * All boards my optionally provide this API to invert the Serial RC input. * This is needed on SoCs that support the notion RXINV or TXINV as apposed to * and external XOR controlled by a GPIO * ************************************************************************************/ __EXPORT void board_rc_input(bool invert_on) { irqstate_t irqstate = px4_enter_critical_section(); uint32_t cr1 = getreg32(STM32_USART_CR1_OFFSET + RC_UXART_BASE); uint32_t cr2 = getreg32(STM32_USART_CR2_OFFSET + RC_UXART_BASE); uint32_t regval = cr1; /* {R|T}XINV bit fields can only be written when the USART is disabled (UE=0). */ regval &= ~USART_CR1_UE; putreg32(regval, STM32_USART_CR1_OFFSET + RC_UXART_BASE); if (invert_on) { cr2 |= (USART_CR2_RXINV | USART_CR2_TXINV); } else { cr2 &= ~(USART_CR2_RXINV | USART_CR2_TXINV); } putreg32(cr2, STM32_USART_CR2_OFFSET + RC_UXART_BASE); putreg32(cr1, STM32_USART_CR1_OFFSET + RC_UXART_BASE); leave_critical_section(irqstate); }
/* * handle ioctl requests */ int PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 500)) { return -EINVAL; } irqstate_t flags = px4_enter_critical_section(); if (!_reports->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } px4_leave_critical_section(flags); return OK; } case SENSORIOCGQUEUEDEPTH: return _reports->size(); case SENSORIOCRESET: /* user has asked for the timer to be reset. This may * be needed if the pin was used for a different * purpose (such as PWM output) */ _timer_init(); /* also reset the sensor */ hard_reset(); return OK; default: /* give it to the superclass */ return CDev::ioctl(filp, cmd, arg); } }
int LPS25H::ioctl(struct file *filp, int cmd, unsigned long arg) { unsigned dummy = arg; switch (cmd) { case SENSORIOCSPOLLRATE: { switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _measure_ticks = 0; return OK; /* external signalling (DRDY) not supported */ case SENSOR_POLLRATE_EXTERNAL: /* zero would be bad */ case 0: return -EINVAL; /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); /* set interval for next measurement to minimum legal value */ _measure_ticks = USEC2TICK(LPS25H_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ if (want_start) { start(); } return OK; } /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); /* convert hz to tick interval via microseconds */ unsigned ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ if (ticks < USEC2TICK(LPS25H_CONVERSION_INTERVAL)) { return -EINVAL; } /* update interval for next measurement */ _measure_ticks = ticks; /* if we need to start the poll state machine, do it */ if (want_start) { start(); } return OK; } } } case SENSORIOCGPOLLRATE: if (_measure_ticks == 0) { return SENSOR_POLLRATE_MANUAL; } return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) { return -EINVAL; } irqstate_t flags = px4_enter_critical_section(); if (!_reports->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } px4_leave_critical_section(flags); return OK; } case SENSORIOCGQUEUEDEPTH: return _reports->size(); case SENSORIOCRESET: return reset(); case BAROIOCSMSLPRESSURE: /* range-check for sanity */ if ((arg < 80000) || (arg > 120000)) { return -EINVAL; } _msl_pressure = arg; return OK; case BAROIOCGMSLPRESSURE: return _msl_pressure; case DEVIOCGDEVICEID: return _interface->ioctl(cmd, dummy); default: /* give it to the superclass */ return CDev::ioctl(filp, cmd, arg); } }
int FXAS21002C::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { case SENSORIOCSPOLLRATE: { switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_interval = 0; return OK; /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: /* zero would be bad */ case 0: return -EINVAL; /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: return ioctl(filp, SENSORIOCSPOLLRATE, FXAS21002C_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_call_interval == 0); /* convert hz to hrt interval via microseconds */ unsigned ticks = 1000000 / arg; /* check against maximum sane rate */ if (ticks < 1000) { return -EINVAL; } /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _call_interval = ticks; _gyro_call.period = _call_interval - FXAS21002C_TIMER_REDUCTION; /* adjust filters */ float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq(); float sample_rate = 1.0e6f / ticks; set_sw_lowpass_filter(sample_rate, cutoff_freq_hz); /* if we need to start the poll state machine, do it */ if (want_start) { start(); } return OK; } } } case SENSORIOCGPOLLRATE: if (_call_interval == 0) { return SENSOR_POLLRATE_MANUAL; } return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) { return -EINVAL; } irqstate_t flags = px4_enter_critical_section(); if (!_reports->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } px4_leave_critical_section(flags); return OK; } case SENSORIOCRESET: reset(); return OK; case GYROIOCSSAMPLERATE: return set_samplerate(arg); case GYROIOCGSAMPLERATE: return _current_rate; case GYROIOCSSCALE: /* copy scale in */ memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale)); return OK; case GYROIOCGSCALE: /* copy scale out */ memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); return OK; case GYROIOCSRANGE: /* arg should be in dps */ return set_range(arg); case GYROIOCGRANGE: /* convert to dps and round */ return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); } }
int SF1XX::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { case SENSORIOCSPOLLRATE: { switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _measure_ticks = 0; return OK; /* external signalling (DRDY) not supported */ case SENSOR_POLLRATE_EXTERNAL: /* zero would be bad */ case 0: return -EINVAL; /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); /* set interval for next measurement to minimum legal value */ _measure_ticks = USEC2TICK(_conversion_interval); /* if we need to start the poll state machine, do it */ if (want_start) { start(); } return OK; } /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); /* convert hz to tick interval via microseconds */ int ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ if (ticks < USEC2TICK(_conversion_interval)) { return -EINVAL; } /* update interval for next measurement */ _measure_ticks = ticks; /* if we need to start the poll state machine, do it */ if (want_start) { start(); } return OK; } } } case SENSORIOCGPOLLRATE: if (_measure_ticks == 0) { return SENSOR_POLLRATE_MANUAL; } return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) { return -EINVAL; } irqstate_t flags = px4_enter_critical_section(); if (!_reports->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } px4_leave_critical_section(flags); return OK; } case SENSORIOCGQUEUEDEPTH: return _reports->size(); case SENSORIOCRESET: /* XXX implement this */ return -EINVAL; case RANGEFINDERIOCSETMINIUMDISTANCE: { set_minimum_distance(*(float *)arg); return 0; } break; case RANGEFINDERIOCSETMAXIUMDISTANCE: { set_maximum_distance(*(float *)arg); return 0; } break; default: /* give it to the superclass */ return I2C::ioctl(filp, cmd, arg); } }
int LIS3MDL::ioctl(struct file *filp, int cmd, unsigned long arg) { unsigned dummy = arg; switch (cmd) { case SENSORIOCSPOLLRATE: { switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _measure_ticks = 0; return OK; /* external signalling (DRDY) not supported */ case SENSOR_POLLRATE_EXTERNAL: /* zero would be bad */ case 0: return -EINVAL; /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); /* set interval for next measurement to minimum legal value */ _measure_ticks = USEC2TICK(LIS3MDL_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ if (want_start) { start(); } return OK; } /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); /* convert hz to tick interval via microseconds */ unsigned ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ if (ticks < USEC2TICK(LIS3MDL_CONVERSION_INTERVAL)) { // RobD: quick fix for Phil's testing return -EINVAL; } /* update interval for next measurement */ _measure_ticks = ticks; /* if we need to start the poll state machine, do it */ if (want_start) { start(); } return OK; } } } case SENSORIOCGPOLLRATE: if (_measure_ticks == 0) { return SENSOR_POLLRATE_MANUAL; } return 1000000 / TICK2USEC(_measure_ticks); case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) { return -EINVAL; } irqstate_t flags = px4_enter_critical_section(); if (!_reports->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } px4_leave_critical_section(flags); return OK; } case SENSORIOCRESET: return reset(); case MAGIOCSSAMPLERATE: /* same as pollrate because device is in single measurement mode*/ return ioctl(filp, SENSORIOCSPOLLRATE, arg); case MAGIOCGSAMPLERATE: /* same as pollrate because device is in single measurement mode*/ return 1000000 / TICK2USEC(_measure_ticks); case MAGIOCSRANGE: return set_range(arg); case MAGIOCGRANGE: return _range_ga; case MAGIOCSSCALE: /* set new scale factors */ memcpy(&_scale, (struct mag_calibration_s *)arg, sizeof(_scale)); /* check calibration, but not actually return an error */ (void)check_calibration(); return 0; case MAGIOCGSCALE: /* copy out scale factors */ memcpy((struct mag_calibration_s *)arg, &_scale, sizeof(_scale)); return 0; case MAGIOCCALIBRATE: return calibrate(filp, arg); case MAGIOCEXSTRAP: return set_excitement(arg); case MAGIOCSELFTEST: return check_calibration(); case MAGIOCGEXTERNAL: DEVICE_DEBUG("MAGIOCGEXTERNAL in main driver"); return _interface->ioctl(cmd, dummy); case DEVIOCGDEVICEID: return _interface->ioctl(cmd, dummy); default: /* give it to the superclass */ return CDev::ioctl(filp, cmd, arg); } }
int MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { /* these are shared with the accel side */ case SENSORIOCSPOLLRATE: case SENSORIOCGPOLLRATE: case SENSORIOCRESET: return ioctl(filp, cmd, arg); case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) { return -EINVAL; } irqstate_t flags = px4_enter_critical_section(); if (!_gyro_reports->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } px4_leave_critical_section(flags); return OK; } case SENSORIOCGQUEUEDEPTH: return _gyro_reports->size(); case GYROIOCGSAMPLERATE: return _sample_rate; case GYROIOCSSAMPLERATE: _set_sample_rate(arg); return OK; case GYROIOCGLOWPASS: return _gyro_filter_x.get_cutoff_freq(); case GYROIOCSLOWPASS: // set software filtering _gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); return OK; case GYROIOCSSCALE: /* copy scale in */ memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale)); return OK; case GYROIOCGSCALE: /* copy scale out */ memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); return OK; case GYROIOCSRANGE: /* XXX not implemented */ // XXX change these two values on set: // _gyro_range_scale = xx // _gyro_range_rad_s = xx return -EINVAL; case GYROIOCGRANGE: return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); case GYROIOCSELFTEST: return gyro_self_test(); #ifdef GYROIOCSHWLOWPASS case GYROIOCSHWLOWPASS: _set_dlpf_filter(arg); return OK; #endif #ifdef GYROIOCGHWLOWPASS case GYROIOCGHWLOWPASS: return _dlpf_freq; #endif default: /* give it to the superclass */ return CDev::ioctl(filp, cmd, arg); } }
int MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { case SENSORIOCRESET: return reset(); case SENSORIOCSPOLLRATE: { switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_interval = 0; return OK; /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: /* zero would be bad */ case 0: return -EINVAL; /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: return ioctl(filp, SENSORIOCSPOLLRATE, 1000); case SENSOR_POLLRATE_DEFAULT: return ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_call_interval == 0); /* convert hz to hrt interval via microseconds */ unsigned ticks = 1000000 / arg; /* check against maximum sane rate */ if (ticks < 1000) { return -EINVAL; } // adjust filters float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); float sample_rate = 1.0e6f / ticks; _set_dlpf_filter(cutoff_freq_hz); _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq(); _set_dlpf_filter(cutoff_freq_hz_gyro); _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _call_interval = ticks; /* set call interval faster than the sample time. We then detect when we have duplicate samples and reject them. This prevents aliasing due to a beat between the stm32 clock and the mpu9250 clock */ _call.period = _call_interval - MPU9250_TIMER_REDUCTION; /* if we need to start the poll state machine, do it */ if (want_start) { start(); } return OK; } } } case SENSORIOCGPOLLRATE: if (_call_interval == 0) { return SENSOR_POLLRATE_MANUAL; } return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) { return -EINVAL; } irqstate_t flags = px4_enter_critical_section(); if (!_accel_reports->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } px4_leave_critical_section(flags); return OK; } case SENSORIOCGQUEUEDEPTH: return _accel_reports->size(); case ACCELIOCGSAMPLERATE: return _sample_rate; case ACCELIOCSSAMPLERATE: _set_sample_rate(arg); return OK; case ACCELIOCGLOWPASS: return _accel_filter_x.get_cutoff_freq(); case ACCELIOCSLOWPASS: // set software filtering _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); return OK; case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ struct accel_calibration_s *s = (struct accel_calibration_s *) arg; float sum = s->x_scale + s->y_scale + s->z_scale; if (sum > 2.0f && sum < 4.0f) { memcpy(&_accel_scale, s, sizeof(_accel_scale)); return OK; } else { return -EINVAL; } } case ACCELIOCGSCALE: /* copy scale out */ memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; case ACCELIOCSRANGE: return set_accel_range(arg); case ACCELIOCGRANGE: return (unsigned long)((_accel_range_m_s2) / MPU9250_ONE_G + 0.5f); case ACCELIOCSELFTEST: return accel_self_test(); #ifdef ACCELIOCSHWLOWPASS case ACCELIOCSHWLOWPASS: _set_dlpf_filter(arg); return OK; #endif #ifdef ACCELIOCGHWLOWPASS case ACCELIOCGHWLOWPASS: return _dlpf_freq; #endif default: /* give it to the superclass */ return CDev::ioctl(filp, cmd, arg); } }
int FXOS8700CQ::mag_ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { case SENSORIOCSPOLLRATE: { switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_mag_interval = 0; return OK; /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: /* zero would be bad */ case 0: return -EINVAL; /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: /* 100 Hz is max for mag */ return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100); /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_call_mag_interval == 0); /* convert hz to hrt interval via microseconds */ unsigned ticks = 1000000 / arg; /* check against maximum sane rate */ if (ticks < 1000) { return -EINVAL; } /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _mag_call.period = _call_mag_interval = ticks; /* if we need to start the poll state machine, do it */ if (want_start) { start(); } return OK; } } } case SENSORIOCGPOLLRATE: if (_call_mag_interval == 0) { return SENSOR_POLLRATE_MANUAL; } return 1000000 / _call_mag_interval; case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) { return -EINVAL; } irqstate_t flags = px4_enter_critical_section(); if (!_mag_reports->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } px4_leave_critical_section(flags); return OK; } case SENSORIOCGQUEUEDEPTH: return _mag_reports->size(); case SENSORIOCRESET: reset(); return OK; case MAGIOCSSAMPLERATE: return mag_set_samplerate(arg); case MAGIOCGSAMPLERATE: return _mag_samplerate; case MAGIOCSLOWPASS: case MAGIOCGLOWPASS: /* not supported, no internal filtering */ return -EINVAL; case MAGIOCSSCALE: /* copy scale in */ memcpy(&_mag_scale, (struct mag_calibration_s *) arg, sizeof(_mag_scale)); return OK; case MAGIOCGSCALE: /* copy scale out */ memcpy((struct mag_calibration_s *) arg, &_mag_scale, sizeof(_mag_scale)); return OK; case MAGIOCSRANGE: return mag_set_range(arg); case MAGIOCGRANGE: return _mag_range_ga; case MAGIOCSELFTEST: return mag_self_test(); case MAGIOCGEXTERNAL: /* Even if this sensor is on the "external" SPI bus * it is still fixed to the autopilot assembly, * so always return 0. */ return 0; default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); } }
int uORB::DeviceNode::ioctl(struct file *filp, int cmd, unsigned long arg) { SubscriberData *sd = filp_to_sd(filp); switch (cmd) { case ORBIOCLASTUPDATE: { irqstate_t state = px4_enter_critical_section(); *(hrt_abstime *)arg = _last_update; px4_leave_critical_section(state); return OK; } case ORBIOCUPDATED: *(bool *)arg = appears_updated(sd); return OK; case ORBIOCSETINTERVAL: { int ret = PX4_OK; lock(); if (arg == 0) { if (sd->update_interval) { delete(sd->update_interval); sd->update_interval = nullptr; } } else { if (sd->update_interval) { sd->update_interval->interval = arg; } else { sd->update_interval = new UpdateIntervalData(); if (sd->update_interval) { memset(&sd->update_interval->update_call, 0, sizeof(hrt_call)); sd->update_interval->interval = arg; } else { ret = -ENOMEM; } } } unlock(); return ret; } case ORBIOCGADVERTISER: *(uintptr_t *)arg = (uintptr_t)this; return OK; case ORBIOCGPRIORITY: *(int *)arg = sd->priority(); return OK; case ORBIOCSETQUEUESIZE: //no need for locking here, since this is used only during the advertisement call, //and only one advertiser is allowed to open the DeviceNode at the same time. return update_queue_size(arg); case ORBIOCGETINTERVAL: if (sd->update_interval) { *(unsigned *)arg = sd->update_interval->interval; } else { *(unsigned *)arg = 0; } return OK; default: /* give it to the superclass */ return CDev::ioctl(filp, cmd, arg); } }
int MPU9250_mag::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { case SENSORIOCRESET: /* * TODO: we could implement a reset of the AK8963 registers */ //return reset(); return _parent->ioctl(filp, cmd, arg); case SENSORIOCSPOLLRATE: { switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: /* * TODO: investigate being able to stop * the continuous sampling */ //stop(); return OK; /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: /* zero would be bad */ case 0: return -EINVAL; /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: return ioctl(filp, SENSORIOCSPOLLRATE, 100); case SENSOR_POLLRATE_DEFAULT: return ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_AK8963_SAMPLE_RATE); /* adjust to a legal polling interval in Hz */ default: { if (MPU9250_AK8963_SAMPLE_RATE != arg) { return -EINVAL; } return OK; } } } case SENSORIOCGPOLLRATE: return MPU9250_AK8963_SAMPLE_RATE; case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) { return -EINVAL; } irqstate_t flags = px4_enter_critical_section(); if (!_mag_reports->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } px4_leave_critical_section(flags); return OK; } case SENSORIOCGQUEUEDEPTH: return _mag_reports->size(); case MAGIOCGSAMPLERATE: return MPU9250_AK8963_SAMPLE_RATE; case MAGIOCSSAMPLERATE: /* * We don't currently support any means of changing * the sampling rate of the mag */ if (MPU9250_AK8963_SAMPLE_RATE != arg) { return -EINVAL; } return OK; case MAGIOCSSCALE: /* copy scale in */ memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale)); return OK; case MAGIOCGSCALE: /* copy scale out */ memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale)); return OK; case MAGIOCSRANGE: return -EINVAL; case MAGIOCGRANGE: return 48; // fixed full scale measurement range of +/- 4800 uT == 48 Gauss case MAGIOCSELFTEST: return self_test(); #ifdef MAGIOCSHWLOWPASS case MAGIOCSHWLOWPASS: return -EINVAL; #endif #ifdef MAGIOCGHWLOWPASS case MAGIOCGHWLOWPASS: return -EINVAL; #endif default: return (int)CDev::ioctl(filp, cmd, arg); } }
bool uORB::DeviceNode::appears_updated(SubscriberData *sd) { /* assume it doesn't look updated */ bool ret = false; /* avoid racing between interrupt and non-interrupt context calls */ irqstate_t state = px4_enter_critical_section(); /* check if this topic has been published yet, if not bail out */ if (_data == nullptr) { ret = false; goto out; } /* * If the subscriber's generation count matches the update generation * count, there has been no update from their perspective; if they * don't match then we might have a visible update. */ while (sd->generation != _generation) { /* * Handle non-rate-limited subscribers. */ if (sd->update_interval == nullptr) { ret = true; break; } /* * If we have previously told the subscriber that there is data, * and they have not yet collected it, continue to tell them * that there has been an update. This mimics the non-rate-limited * behaviour where checking / polling continues to report an update * until the topic is read. */ if (sd->update_reported()) { ret = true; break; } /* * If the interval timer is still running, the topic should not * appear updated, even though at this point we know that it has. * We have previously been through here, so the subscriber * must have collected the update we reported, otherwise * update_reported would still be true. */ if (!hrt_called(&sd->update_interval->update_call)) { break; } /* * Make sure that we don't consider the topic to be updated again * until the interval has passed once more by restarting the interval * timer and thereby re-scheduling a poll notification at that time. */ hrt_call_after(&sd->update_interval->update_call, sd->update_interval->interval, &uORB::DeviceNode::update_deferred_trampoline, (void *)this); /* * Remember that we have told the subscriber that there is data. */ sd->set_update_reported(true); ret = true; break; } out: px4_leave_critical_section(state); /* consider it updated */ return ret; }
int BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { case SENSORIOCSPOLLRATE: { switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_interval = 0; return OK; /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: /* zero would be bad */ case 0: return -EINVAL; /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: /* With internal low pass filters enabled, 250 Hz is sufficient */ return ioctl(filp, SENSORIOCSPOLLRATE, 250); /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_call_interval == 0); /* convert hz to hrt interval via microseconds */ unsigned ticks = 1000000 / arg; /* check against maximum sane rate */ if (ticks < 1000) { return -EINVAL; } /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _call.period = _call_interval = ticks; /* if we need to start the poll state machine, do it */ if (want_start) { start(); } return OK; } } } case SENSORIOCGPOLLRATE: if (_call_interval == 0) { return SENSOR_POLLRATE_MANUAL; } return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 2) || (arg > 100)) { return -EINVAL; } irqstate_t flags = px4_enter_critical_section(); if (!_reports->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } px4_leave_critical_section(flags); return OK; } case SENSORIOCGQUEUEDEPTH: return _reports->size(); case SENSORIOCRESET: /* XXX implement */ return -EINVAL; case ACCELIOCSSAMPLERATE: /* sensor sample rate is not (really) adjustable */ return -EINVAL; case ACCELIOCGSAMPLERATE: return 1200; /* always operating in low-noise mode */ case ACCELIOCSLOWPASS: return set_lowpass(arg); case ACCELIOCGLOWPASS: return _current_lowpass; case ACCELIOCSSCALE: /* copy scale in */ memcpy(&_accel_scale, (struct accel_calibration_s *) arg, sizeof(_accel_scale)); return OK; case ACCELIOCGSCALE: /* copy scale out */ memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; case ACCELIOCSRANGE: return set_range(arg); case ACCELIOCGRANGE: return _current_range; default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); } }