/*
* 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
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
SF1XX::ioctl(device::file_t *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;
}
ATOMIC_ENTER;
if (!_reports->resize(arg)) {
ATOMIC_LEAVE;
return -ENOMEM;
}
ATOMIC_LEAVE;
return OK;
}
case SENSORIOCRESET:
/* XXX implement this */
return -EINVAL;
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
}
}
int
BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE:
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop_cycle();
_measure_ticks = 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: {
/* 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(BAROSIM_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start) {
start_cycle();
}
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 long ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(BAROSIM_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_cycle();
}
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;
}
if (!_reports->resize(arg)) {
return -ENOMEM;
}
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
case SENSORIOCRESET:
/*
* Since we are initialized, we do not need to do anything, since the
* PROM is correctly read and the part does not need to be configured.
*/
return OK;
case BAROIOCSMSLPRESSURE:
/* range-check for sanity */
if ((arg < 80000) || (arg > 120000)) {
return -EINVAL;
}
_msl_pressure = arg;
return OK;
case BAROIOCGMSLPRESSURE:
return _msl_pressure;
default:
break;
}
/* give it to the bus-specific superclass */
// return bus_ioctl(filp, cmd, arg);
return VDev::ioctl(filp, cmd, arg);
}
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);
}
}
int
ACCELSIM::mag_ioctl(unsigned long cmd, unsigned long arg)
{
unsigned long ul_arg = (unsigned long)arg;
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
_mag->stop();
_mag->m_sample_interval_usecs = 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(SENSORIOCSPOLLRATE, 100);
/* adjust to a legal polling interval in Hz */
default: {
/* convert hz to hrt interval via microseconds */
unsigned interval = 1000000 / ul_arg;
/* check against maximum sane rate (1ms) */
if (interval < 10000) {
return -EINVAL;
}
bool want_start = (_mag->m_sample_interval_usecs == 0);
/* update interval for next measurement */
_mag->setSampleInterval(interval);
if (want_start) {
_mag->start();
}
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_mag->m_sample_interval_usecs == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return 1000000 / _mag->m_sample_interval_usecs;
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
if (!_mag_reports->resize(arg)) {
return -ENOMEM;
}
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _mag_reports->size();
case SENSORIOCRESET:
// Nothing to do for simulator
return OK;
case MAGIOCSSAMPLERATE:
// No need to set internal sampling rate for simulator
return OK;
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 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;
case MAGIOCSELFTEST:
return OK;
default:
/* give it to the superclass */
return VirtDevObj::devIOCTL(cmd, arg);
}
}
int
BAROSIM::devIOCTL(unsigned long cmd, unsigned long arg)
{
PX4_WARN("baro IOCTL %llu", hrt_absolute_time());
switch (cmd) {
case SENSORIOCSPOLLRATE:
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop_cycle();
setSampleInterval(0);
m_sample_interval_usecs = 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: {
setSampleInterval(BAROSIM_CONVERSION_INTERVAL);
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* convert hz to tick interval via microseconds */
unsigned long interval = 1000000 / arg;
/* check against maximum rate */
if (interval < BAROSIM_CONVERSION_INTERVAL) {
return -EINVAL;
}
bool want_start = (m_sample_interval_usecs == 0);
/* update interval for next measurement */
setSampleInterval(interval);
if (want_start) {
start();
}
return OK;
}
}
case SENSORIOCGPOLLRATE:
if (m_sample_interval_usecs == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return (1000000 / m_sample_interval_usecs);
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
if (!_reports->resize(arg)) {
return -ENOMEM;
}
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
case SENSORIOCRESET:
/*
* Since we are initialized, we do not need to do anything, since the
* PROM is correctly read and the part does not need to be configured.
*/
return OK;
case BAROIOCSMSLPRESSURE:
/* range-check for sanity */
if ((arg < 80000) || (arg > 120000)) {
return -EINVAL;
}
_msl_pressure = arg;
return OK;
case BAROIOCGMSLPRESSURE:
return _msl_pressure;
default:
break;
}
/* give it to the bus-specific superclass */
// return bus_ioctl(filp, cmd, arg);
return VirtDevObj::devIOCTL(cmd, arg);
}
int
ACCELSIM::devIOCTL(unsigned long cmd, unsigned long arg)
{
unsigned long ul_arg = (unsigned long)arg;
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (ul_arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
m_sample_interval_usecs = 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 devIOCTL(SENSORIOCSPOLLRATE, 1600);
case SENSOR_POLLRATE_DEFAULT:
return devIOCTL(SENSORIOCSPOLLRATE, ACCELSIM_ACCEL_DEFAULT_RATE);
/* adjust to a legal polling interval in Hz */
default: {
/* convert hz to hrt interval via microseconds */
unsigned interval = 1000000 / ul_arg;
/* check against maximum sane rate */
if (interval < 500) {
return -EINVAL;
}
/* adjust filters */
accel_set_driver_lowpass_filter((float)ul_arg, _accel_filter_x.get_cutoff_freq());
bool want_start = (m_sample_interval_usecs == 0);
/* update interval for next measurement */
setSampleInterval(interval);
if (want_start) {
start();
}
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (m_sample_interval_usecs == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return 1000000 / m_sample_interval_usecs;
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((ul_arg < 1) || (ul_arg > 100)) {
return -EINVAL;
}
if (!_accel_reports->resize(ul_arg)) {
return -ENOMEM;
}
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _accel_reports->size();
case SENSORIOCRESET:
// Nothing to do for simulator
return OK;
case ACCELIOCSSAMPLERATE:
// No need to set internal sampling rate for simulator
return OK;
case ACCELIOCGSAMPLERATE:
return _accel_samplerate;
case ACCELIOCSLOWPASS: {
return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)ul_arg);
}
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 ACCELIOCSRANGE:
/* arg needs to be in G */
return accel_set_range(ul_arg);
case ACCELIOCGRANGE:
/* convert to m/s^2 and return rounded in G */
return (unsigned long)((_accel_range_m_s2) / ACCELSIM_ONE_G + 0.5f);
case ACCELIOCGSCALE:
/* copy scale out */
memcpy((struct accel_calibration_s *) arg, &(_accel_scale), sizeof(_accel_scale));
return OK;
case ACCELIOCSELFTEST:
return OK;
default:
/* give it to the superclass */
return VirtDevObj::devIOCTL(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
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
FXOS8700CQ::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_accel_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, 1600);
case SENSOR_POLLRATE_DEFAULT:
return ioctl(filp, SENSORIOCSPOLLRATE, FXOS8700C_ACCEL_DEFAULT_RATE);
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_call_accel_interval == 0);
/* convert hz to hrt interval via microseconds */
unsigned ticks = 1000000 / arg;
/* check against maximum sane rate */
if (ticks < 500) {
return -EINVAL;
}
/* adjust filters */
accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq());
/* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */
_call_accel_interval = ticks;
_accel_call.period = _call_accel_interval - FXOS8700C_TIMER_REDUCTION;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_call_accel_interval == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return 1000000 / _call_accel_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 SENSORIOCRESET:
reset();
return OK;
case ACCELIOCSSAMPLERATE:
return accel_set_samplerate(arg);
case ACCELIOCGSAMPLERATE:
return _accel_samplerate;
case ACCELIOCSLOWPASS: {
return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg);
}
case ACCELIOCGLOWPASS:
return static_cast<int>(_accel_filter_x.get_cutoff_freq());
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 ACCELIOCSRANGE:
/* arg needs to be in G */
return accel_set_range(arg);
case ACCELIOCGRANGE:
/* convert to m/s^2 and return rounded in G */
return (unsigned long)((_accel_range_m_s2) / FXOS8700C_ONE_G + 0.5f);
case ACCELIOCGSCALE:
/* copy scale out */
memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale));
return OK;
case ACCELIOCSELFTEST:
return accel_self_test();
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
}
}
int
PX4FLOW::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(PX4FLOW_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(PX4FLOW_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 = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
case SENSORIOCSROTATION:
_sensor_rotation = (enum Rotation)arg;
return OK;
case SENSORIOCGROTATION:
return _sensor_rotation;
case SENSORIOCRESET:
/* XXX implement this */
return -EINVAL;
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
}
}