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);
}
}
