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 InputPWM::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(INPUT_PWM_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(INPUT_PWM_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 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 = irqsave(); if (!_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } irqrestore(flags); return OK; } case SENSORIOCGQUEUEDEPTH: return _reports->size(); case SENSORIOCRESET: return OK; case RC_INPUT_GET: { /* fetch R/C input values into (rc_input_values *)arg */ struct rc_input_values *report = (rc_input_values *)arg; int ret; ret = read(0, (char *)report, sizeof(*report)); if (ret > 0) return OK; else return ret; } default: /* give it to the superclass */ return CDev::ioctl(filp, cmd, arg); } }
int HMC5883::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(HMC5883_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(HMC5883_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 1000000/TICK2USEC(_measure_ticks); case SENSORIOCSQUEUEDEPTH: { /* add one to account for the sentinel in the ring */ arg++; /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 2) || (arg > 100)) return -EINVAL; /* allocate new buffer */ struct mag_report *buf = new struct mag_report[arg]; if (nullptr == buf) return -ENOMEM; /* reset the measurement state machine with the new buffer, free the old */ stop(); delete[] _reports; _num_reports = arg; _reports = buf; start(); return OK; } case SENSORIOCGQUEUEDEPTH: return _num_reports - 1; 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 MAGIOCSLOWPASS: case MAGIOCGLOWPASS: /* not supported, no internal filtering */ return -EINVAL; case MAGIOCSSCALE: /* set new scale factors */ memcpy(&_scale, (mag_scale *)arg, sizeof(_scale)); /* check calibration, but not actually return an error */ (void)check_calibration(); return 0; case MAGIOCGSCALE: /* copy out scale factors */ memcpy((mag_scale *)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: if (_bus == PX4_I2C_BUS_EXPANSION) return 1; else return 0; default: /* give it to the superclass */ return I2C::ioctl(filp, cmd, arg); } }
int AK8975::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(AK8975_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(AK8975_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 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 = irqsave(); if (!_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } irqrestore(flags); return OK; } case SENSORIOCGQUEUEDEPTH: return _reports->size(); 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: // not allowed on this sensor return -EINVAL; case MAGIOCGRANGE: return _range_ga; case MAGIOCSLOWPASS: case MAGIOCGLOWPASS: /* not supported, no internal filtering */ return -EINVAL; case MAGIOCSSCALE: /* not supported on this sensor */ return -EINVAL; case MAGIOCGSCALE: /* copy out scale factors */ memcpy((mag_scale *)arg, &_scale, sizeof(_scale)); return 0; case MAGIOCCALIBRATE: /* not supported */ return -EINVAL; case MAGIOCEXSTRAP: /* not supported */ return -EINVAL; case MAGIOCSELFTEST: return check_calibration(); case MAGIOCGEXTERNAL: if (_bus == PX4_I2C_BUS_EXPANSION) return 1; else return 0; default: /* give it to the superclass */ return I2C::ioctl(filp, cmd, arg); } }