void FXAS21002C::reset() { /* write 0 0 0 000 00 = 0x00 to CTRL_REG1 to place FXOS21002 in Standby * [6]: RST=0 * [5]: ST=0 self test disabled * [4-2]: DR[2-0]=000 for 200Hz ODR * [1-0]: Active=0, Ready=0 for Standby mode */ write_reg(FXAS21002C_CTRL_REG1, 0); /* write 0000 0000 = 0x00 to CTRL_REG0 to configure range and filters * [7-6]: BW[1-0]=00, LPF 64 @ 800Hz ODR * [5]: SPIW=0 4 wire SPI * [4-3]: SEL[1-0]=00 for 10Hz HPF at 200Hz ODR * [2]: HPF_EN=0 disable HPF * [1-0]: FS[1-0]=00 for 1600dps (TBD CHANGE TO 2000dps when final trimmed parts available) */ write_checked_reg(FXAS21002C_CTRL_REG0, CTRL_REG0_BW_LOW | CTRL_REG0_FS_2000_DPS); /* write CTRL_REG1 to configure 800Hz ODR and enter Active mode */ write_checked_reg(FXAS21002C_CTRL_REG1, CTRL_REG1_DR_800HZ | CTRL_REG1_ACTIVE); /* Set the default */ set_samplerate(0); set_range(FXAS21002C_DEFAULT_RANGE_DPS); set_onchip_lowpass_filter(FXAS21002C_DEFAULT_ONCHIP_FILTER_FREQ); _read = 0; }
static int hw_dev_config_set(int dev_index, int hwcap, void *value) { struct sr_dev_inst *sdi; struct context *ctx; if (!(sdi = sr_dev_inst_get(dev_insts, dev_index))) { sr_err("zp: %s: sdi was NULL", __func__); return SR_ERR; } if (!(ctx = sdi->priv)) { sr_err("zp: %s: sdi->priv was NULL", __func__); return SR_ERR_ARG; } switch (hwcap) { case SR_HWCAP_SAMPLERATE: return set_samplerate(sdi, *(uint64_t *)value); case SR_HWCAP_PROBECONFIG: return configure_probes(sdi, (GSList *)value); case SR_HWCAP_LIMIT_SAMPLES: ctx->limit_samples = *(uint64_t *)value; return SR_OK; default: return SR_ERR; } }
void L3GD20::reset() { // ensure the chip doesn't interpret any other bus traffic as I2C disable_i2c(); /* set default configuration */ write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ write_reg(ADDR_CTRL_REG4, REG4_BDU); write_reg(ADDR_CTRL_REG5, 0); write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ /* disable FIFO. This makes things simpler and ensures we * aren't getting stale data. It means we must run the hrt * callback fast enough to not miss data. */ write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); set_samplerate(0); // 760Hz set_range(L3GD20_DEFAULT_RANGE_DPS); set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ); _read = 0; }
void EngineControl::init(unsigned int samplerate_, unsigned int buffersize_, int policy_, int priority_) { if (policy_ != policy || priority_ != priority) { policy = policy_; priority = priority_; set_buffersize(buffersize_); set_samplerate(samplerate_); return; } if (buffersize_ != buffersize) { set_buffersize(buffersize_); } if (samplerate_ != samplerate) { set_samplerate(samplerate_); } }
void IIS328DQ::reset() { /* set default configuration */ write_checked_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_checked_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ write_checked_reg(ADDR_CTRL_REG3, 0x02); /* DRDY enable */ write_checked_reg(ADDR_CTRL_REG4, REG4_BDU); set_samplerate(0, _accel_onchip_filter_bandwidth); //1000Hz set_range(IIS328DQ_ACCEL_DEFAULT_RANGE_G); //设置软件滤波器截止频率 set_driver_lowpass_filter(IIS328DQ_DEFAULT_RATE, IIS328DQ_DEFAULT_DRIVER_FILTER_FREQ); _read = 0; }
int L3GD20::init() { int ret = ERROR; /* do SPI init (and probe) first */ if (SPI::init() != OK) goto out; /* allocate basic report buffers */ _num_reports = 2; _oldest_report = _next_report = 0; _reports = new struct gyro_report[_num_reports]; if (_reports == nullptr) goto out; /* advertise sensor topic */ memset(&_reports[0], 0, sizeof(_reports[0])); _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]); /* set default configuration */ write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ write_reg(ADDR_CTRL_REG4, REG4_BDU); write_reg(ADDR_CTRL_REG5, 0); write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ /* disable FIFO. This makes things simpler and ensures we * aren't getting stale data. It means we must run the hrt * callback fast enough to not miss data. */ write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); set_range(500); /* default to 500dps */ set_samplerate(0); /* max sample rate */ ret = OK; out: return ret; }
static int config_set(int id, GVariant *data, const struct sr_dev_inst *sdi) { struct dev_context *devc; if (sdi->status != SR_ST_ACTIVE) return SR_ERR_DEV_CLOSED; if (!(devc = sdi->priv)) { sr_err("%s: sdi->priv was NULL.", __func__); return SR_ERR_BUG; } switch (id) { case SR_CONF_SAMPLERATE: if (set_samplerate(sdi, g_variant_get_uint64(data)) == SR_ERR) { sr_err("%s: setting samplerate failed.", __func__); return SR_ERR; } sr_dbg("SAMPLERATE = %" PRIu64, devc->cur_samplerate); break; case SR_CONF_LIMIT_MSEC: if (g_variant_get_uint64(data) == 0) { sr_err("%s: LIMIT_MSEC can't be 0.", __func__); return SR_ERR; } devc->limit_msec = g_variant_get_uint64(data); sr_dbg("LIMIT_MSEC = %" PRIu64, devc->limit_msec); break; case SR_CONF_LIMIT_SAMPLES: if (g_variant_get_uint64(data) < MIN_NUM_SAMPLES) { sr_err("%s: LIMIT_SAMPLES too small.", __func__); return SR_ERR; } devc->limit_samples = g_variant_get_uint64(data); sr_dbg("LIMIT_SAMPLES = %" PRIu64, devc->limit_samples); break; default: return SR_ERR_NA; } return SR_OK; }
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); } }
bool AP_InertialSensor_L3GD20::_hardware_init(Sample_rate sample_rate) { if (!_spi_sem->take(100)) { hal.scheduler->panic(PSTR("L3GD20: Unable to get semaphore")); } // initially run the bus at low speed _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); // ensure the chip doesn't interpret any other bus traffic as I2C disable_i2c(); // Chip reset /* set default configuration */ _register_write(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); hal.scheduler->delay(1); _register_write(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ hal.scheduler->delay(1); _register_write(ADDR_CTRL_REG3, 0x08); /* DRDY enable */ hal.scheduler->delay(1); _register_write(ADDR_CTRL_REG4, REG4_BDU); hal.scheduler->delay(1); _register_write(ADDR_CTRL_REG5, 0); hal.scheduler->delay(1); _register_write(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ hal.scheduler->delay(1); /* disable FIFO. This makes things simpler and ensures we * aren't getting stale data. It means we must run the hrt * callback fast enough to not miss data. */ _register_write(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); hal.scheduler->delay(1); set_samplerate(0); // 760Hz hal.scheduler->delay(1); set_range(L3GD20_DEFAULT_RANGE_DPS); hal.scheduler->delay(1); // //TODO: Filtering // uint8_t default_filter; // // sample rate and filtering // // to minimise the effects of aliasing we choose a filter // // that is less than half of the sample rate // switch (sample_rate) { // case RATE_50HZ: // // this is used for plane and rover, where noise resistance is // // more important than update rate. Tests on an aerobatic plane // // show that 10Hz is fine, and makes it very noise resistant // default_filter = BITS_DLPF_CFG_10HZ; // _sample_shift = 2; // break; // case RATE_100HZ: // default_filter = BITS_DLPF_CFG_20HZ; // _sample_shift = 1; // break; // case RATE_200HZ: // default: // default_filter = BITS_DLPF_CFG_20HZ; // _sample_shift = 0; // break; // } // _set_filter_register(_L3GD20_filter, default_filter); // now that we have initialised, we set the SPI bus speed to high _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); _spi_sem->give(); return true; }
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 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: /* With internal low pass filters enabled, 250 Hz is sufficient */ /* XXX for vibration tests with 800 Hz */ return ioctl(filp, SENSORIOCSPOLLRATE, 800); /* 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 < 1000) return -EINVAL; /* adjust sample rate of sensor */ set_samplerate(arg); /* 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: { /* account for 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 accel_report *buf = new struct accel_report[arg]; if (nullptr == buf) return -ENOMEM; /* reset the measurement state machine with the new buffer, free the old */ stop(); delete[] _accel_reports; _num_accel_reports = arg; _accel_reports = buf; start(); return OK; } case SENSORIOCGQUEUEDEPTH: return _num_accel_reports - 1; case SENSORIOCRESET: /* XXX implement */ return -EINVAL; // case ACCELIOCSLOWPASS: case ACCELIOCGLOWPASS: unsigned bandwidth; if (OK == get_antialias_filter_bandwidth(bandwidth)) return bandwidth; else return -EINVAL; default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); } }
int LSM303D::init() { int ret = ERROR; int mag_ret; int fd_mag; /* do SPI init (and probe) first */ if (SPI::init() != OK) goto out; /* allocate basic report buffers */ _num_accel_reports = 2; _oldest_accel_report = _next_accel_report = 0; _accel_reports = new struct accel_report[_num_accel_reports]; if (_accel_reports == nullptr) goto out; /* advertise accel topic */ memset(&_accel_reports[0], 0, sizeof(_accel_reports[0])); _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]); _num_mag_reports = 2; _oldest_mag_report = _next_mag_report = 0; _mag_reports = new struct mag_report[_num_mag_reports]; if (_mag_reports == nullptr) goto out; /* advertise mag topic */ memset(&_mag_reports[0], 0, sizeof(_mag_reports[0])); _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); /* enable accel, XXX do this with an ioctl? */ write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A); /* enable mag, XXX do this with an ioctl? */ write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); /* XXX should we enable FIFO? */ set_range(8); /* XXX 16G mode seems wrong (shows 6 instead of 9.8m/s^2, therefore use 8G for now */ set_antialias_filter_bandwidth(50); /* available bandwidths: 50, 194, 362 or 773 Hz */ set_samplerate(400); /* max sample rate */ mag_set_range(4); /* XXX: take highest sensor range of 12GA? */ mag_set_samplerate(100); /* XXX test this when another mag is used */ /* do CDev init for the mag device node, keep it optional */ mag_ret = _mag->init(); if (mag_ret != OK) { _mag_topic = -1; } ret = OK; out: return ret; }
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: /* 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: { /* account for 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 gyro_report *buf = new struct gyro_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: /* XXX implement */ return -EINVAL; case GYROIOCSSAMPLERATE: return set_samplerate(arg); case GYROIOCGSAMPLERATE: return _current_rate; case GYROIOCSLOWPASS: case GYROIOCGLOWPASS: /* XXX not implemented due to wacky interaction with samplerate */ return -EINVAL; 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: return set_range(arg); case GYROIOCGRANGE: return _current_range; case GYROIOCSELFTEST: return self_test(); default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); } }