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 LSM303D::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, LSM303D_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... */ _accel_call.period = _call_accel_interval = ticks; /* 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 = irqsave(); if (!_accel_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } irqrestore(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 _accel_filter_x.get_cutoff_freq(); case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ struct accel_scale *s = (struct accel_scale *) 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)/LSM303D_ONE_G + 0.5f); case ACCELIOCGSCALE: /* copy scale out */ memcpy((struct accel_scale *) 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 L3GD20::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: if (_is_l3g4200d) { return ioctl(filp, SENSORIOCSPOLLRATE, L3G4200D_DEFAULT_RATE); } return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_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.period = _call_interval = ticks; /* adjust filters */ float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq(); float sample_rate = 1.0e6f/ticks; set_driver_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 = irqsave(); if (!_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } irqrestore(flags); return OK; } case SENSORIOCGQUEUEDEPTH: return _reports->size(); case SENSORIOCRESET: reset(); return OK; case GYROIOCSSAMPLERATE: return set_samplerate(arg, _current_bandwidth); case GYROIOCGSAMPLERATE: return _current_rate; case GYROIOCSLOWPASS: { // set the software lowpass cut-off in Hz float cutoff_freq_hz = arg; float sample_rate = 1.0e6f / _call_interval; set_driver_lowpass_filter(sample_rate, cutoff_freq_hz); return OK; } case GYROIOCGLOWPASS: return _gyro_filter_x.get_cutoff_freq(); case GYROIOCSSCALE: /* copy scale in */ memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale)); return OK; case GYROIOCGSCALE: /* copy scale out */ memcpy((struct gyro_scale *) 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); case GYROIOCSELFTEST: return self_test(); case GYROIOCSHWLOWPASS: return set_samplerate(_current_rate, arg); case GYROIOCGHWLOWPASS: return _current_bandwidth; default: /* give it to the superclass */ return SPI::ioctl(filp, 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 FXAS21002C::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { case SENSORIOCSPOLLRATE: { switch (arg) { /* zero would be bad */ case 0: return -EINVAL; /* set default polling rate */ 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 SENSORIOCRESET: reset(); return OK; case GYROIOCSSCALE: /* copy scale in */ memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale)); return OK; default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); } }