ssize_t
ACCELSIM::devRead(void *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct accel_report);
accel_report *arb = reinterpret_cast<accel_report *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (m_sample_interval_usecs > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
*/
while (count--) {
if (_accel_reports->get(arb)) {
ret += sizeof(*arb);
arb++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement */
_measure();
/* measurement will have generated a report, copy it out */
if (_accel_reports->get(arb)) {
ret = sizeof(*arb);
}
return ret;
}
int
FXAS21002C::init()
{
/* do SPI init (and probe) first */
if (SPI::init() != OK) {
PX4_ERR("SPI init failed");
return PX4_ERROR;
}
param_t gyro_cut_ph = param_find("IMU_GYRO_CUTOFF");
float gyro_cut = FXAS21002C_DEFAULT_FILTER_FREQ;
if (gyro_cut_ph != PARAM_INVALID && param_get(gyro_cut_ph, &gyro_cut) == PX4_OK) {
PX4_INFO("gyro cutoff set to %.2f Hz", double(gyro_cut));
set_sw_lowpass_filter(FXAS21002C_DEFAULT_RATE, gyro_cut);
} else {
PX4_ERR("IMU_GYRO_CUTOFF param invalid");
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report));
if (_reports == nullptr) {
return PX4_ERROR;
}
reset();
/* fill report structures */
measure();
_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_reports->get(&grp);
/* measurement will have generated a report, publish */
_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
&_orb_class_instance, (external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
if (_gyro_topic == nullptr) {
PX4_ERR("ADVERT ERR");
return PX4_ERROR;
}
return PX4_OK;
}
int
FXAS21002C::init()
{
int ret = PX4_ERROR;
/* do SPI init (and probe) first */
if (SPI::init() != OK) {
PX4_ERR("SPI init failed");
goto out;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report));
if (_reports == nullptr) {
goto out;
}
reset();
/* fill report structures */
measure();
_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_reports->get(&grp);
/* measurement will have generated a report, publish */
_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
&_orb_class_instance, (external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
if (_gyro_topic == nullptr) {
PX4_ERR("ADVERT ERR");
}
ret = OK;
out:
return ret;
}
int
TRONE::init()
{
int ret = ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK) {
goto out;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
if (_reports == nullptr) {
goto out;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report;
measure();
_reports->get(&ds_report);
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_LOW);
if (_distance_sensor_topic == nullptr) {
DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
}
}
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
out:
return ret;
}
int
L3GD20::init()
{
int ret = ERROR;
/* do SPI init (and probe) first */
if (SPI::init() != OK) {
goto out;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report));
if (_reports == nullptr) {
goto out;
}
_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
reset();
measure();
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_reports->get(&grp);
_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
&_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
if (_gyro_topic == nullptr) {
DEVICE_DEBUG("failed to create sensor_gyro publication");
}
ret = OK;
out:
return ret;
}
/*
* read some samples from the device
*/
ssize_t
PWMIN::read(struct file *filp, char *buffer, size_t buflen)
{
_last_read_time = hrt_absolute_time();
unsigned count = buflen / sizeof(struct pwm_input_s);
struct pwm_input_s *buf = reinterpret_cast<struct pwm_input_s *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
while (count--) {
if (_reports->get(buf)) {
ret += sizeof(struct pwm_input_s);
buf++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
ssize_t
BAROSIM::read(device::file_t *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct baro_report);
struct baro_report *brp = reinterpret_cast<struct baro_report *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (_measure_ticks > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the workq thread while we are doing this;
* we are careful to avoid racing with them.
*/
while (count--) {
if (_reports->get(brp)) {
ret += sizeof(*brp);
brp++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement - run one conversion */
do {
_measure_phase = 0;
_reports->flush();
/* do temperature first */
if (OK != measure()) {
ret = -EIO;
break;
}
usleep(BAROSIM_CONVERSION_INTERVAL);
if (OK != collect()) {
ret = -EIO;
break;
}
/* now do a pressure measurement */
if (OK != measure()) {
ret = -EIO;
break;
}
usleep(BAROSIM_CONVERSION_INTERVAL);
if (OK != collect()) {
ret = -EIO;
break;
}
/* state machine will have generated a report, copy it out */
if (_reports->get(brp)) {
ret = sizeof(*brp);
}
} while (0);
return ret;
}
int
BAROSIM::init()
{
int ret;
DEVICE_DEBUG("BAROSIM::init");
ret = VDev::init();
if (ret != OK) {
DEVICE_DEBUG("VDev init failed");
goto out;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(baro_report));
if (_reports == nullptr) {
DEVICE_DEBUG("can't get memory for reports");
ret = -ENOMEM;
goto out;
}
/* register alternate interfaces if we have to */
_class_instance = register_class_devname(BARO_BASE_DEVICE_PATH);
struct baro_report brp;
/* do a first measurement cycle to populate reports with valid data */
_measure_phase = 0;
_reports->flush();
_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
if (_baro_topic == nullptr) {
PX4_ERR("failed to create sensor_baro publication");
}
/* this do..while is goto without goto */
do {
/* do temperature first */
if (OK != measure()) {
ret = -EIO;
PX4_ERR("temp measure failed");
break;
}
usleep(BAROSIM_CONVERSION_INTERVAL);
if (OK != collect()) {
ret = -EIO;
PX4_ERR("temp collect failed");
break;
}
/* now do a pressure measurement */
if (OK != measure()) {
ret = -EIO;
PX4_ERR("pressure collect failed");
break;
}
usleep(BAROSIM_CONVERSION_INTERVAL);
if (OK != collect()) {
ret = -EIO;
PX4_ERR("pressure collect failed");
break;
}
/* state machine will have generated a report, copy it out */
_reports->get(&brp);
ret = OK;
//PX4_WARN("sensor_baro publication %ld", _baro_topic);
} while (0);
out:
return ret;
}
int
BMA180::init()
{
int ret = PX4_ERROR;
/* do SPI init (and probe) first */
if (SPI::init() != OK) {
goto out;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
if (_reports == nullptr) {
goto out;
}
/* perform soft reset (p48) */
write_reg(ADDR_RESET, SOFT_RESET);
/* wait 10 ms (datasheet incorrectly lists 10 us on page 49) */
usleep(10000);
/* enable writing to chip config */
modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE);
/* disable I2C interface */
modify_reg(ADDR_HIGH_DUR, HIGH_DUR_DIS_I2C, 0);
/* switch to low-noise mode */
modify_reg(ADDR_TCO_Z, TCO_Z_MODE_MASK, 0);
/* disable 12-bit mode */
modify_reg(ADDR_OFFSET_T, OFFSET_T_READOUT_12BIT, 0);
/* disable shadow-disable mode */
modify_reg(ADDR_GAIN_Y, GAIN_Y_SHADOW_DIS, 0);
/* disable writing to chip config */
modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
if (set_range(4)) { warnx("Failed setting range"); }
if (set_lowpass(75)) { warnx("Failed setting lowpass"); }
if (read_reg(ADDR_CHIP_ID) == CHIP_ID) {
ret = OK;
} else {
ret = PX4_ERROR;
}
_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
/* advertise sensor topic, measure manually to initialize valid report */
measure();
if (_class_instance == CLASS_DEVICE_PRIMARY) {
struct accel_report arp;
_reports->get(&arp);
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
}
out:
return ret;
}
int
ACCELSIM::init()
{
int ret = -1;
struct mag_report mrp = {};
struct accel_report arp = {};
/* do SIM init first */
if (VirtDevObj::init() != 0) {
PX4_WARN("SIM init failed");
goto out;
}
/* allocate basic report buffers */
_accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
if (_accel_reports == nullptr) {
goto out;
}
_mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
if (_mag_reports == nullptr) {
goto out;
}
/* do init for the mag device node */
ret = _mag->init();
if (ret != OK) {
PX4_WARN("MAG init failed");
goto out;
}
/* fill report structures */
_measure();
/* advertise sensor topic, measure manually to initialize valid report */
_mag_reports->get(&mrp);
/* measurement will have generated a report, publish */
_mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp,
&_mag->_mag_orb_class_instance, ORB_PRIO_LOW);
if (_mag->_mag_topic == nullptr) {
PX4_WARN("ADVERT ERR");
}
/* advertise sensor topic, measure manually to initialize valid report */
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
&_accel_orb_class_instance, ORB_PRIO_DEFAULT);
if (_accel_topic == nullptr) {
PX4_WARN("ADVERT ERR");
}
out:
return ret;
}
int
FXOS8700CQ::init()
{
int ret = PX4_ERROR;
/* do SPI init (and probe) first */
if (SPI::init() != OK) {
PX4_ERR("SPI init failed");
goto out;
}
/* allocate basic report buffers */
_accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
if (_accel_reports == nullptr) {
goto out;
}
_mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
if (_mag_reports == nullptr) {
goto out;
}
reset();
/* do CDev init for the mag device node */
ret = _mag->init();
if (ret != OK) {
PX4_ERR("MAG init failed");
goto out;
}
/* fill report structures */
measure();
/* advertise sensor topic, measure manually to initialize valid report */
struct mag_report mrp;
_mag_reports->get(&mrp);
/* measurement will have generated a report, publish */
_mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp,
&_mag->_mag_orb_class_instance, ORB_PRIO_LOW);
if (_mag->_mag_topic == nullptr) {
PX4_ERR("ADVERT ERR");
}
_accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
if (_accel_topic == nullptr) {
PX4_ERR("ADVERT ERR");
}
out:
return ret;
}