bool create_pubs()
{
// initialize the reports
memset(&_gyro, 0, sizeof(struct gyro_report));
memset(&_accel, 0, sizeof(struct accel_report));
memset(&_mag, 0, sizeof(struct mag_report));
_gyro_pub = orb_advertise_multi(ORB_ID(sensor_gyro), &_gyro,
&_gyro_orb_class_instance, ORB_PRIO_HIGH - 1);
if (_gyro_pub == nullptr) {
PX4_ERR("sensor_gyro advert fail");
return false;
}
_accel_pub = orb_advertise_multi(ORB_ID(sensor_accel), &_accel,
&_accel_orb_class_instance, ORB_PRIO_HIGH - 1);
if (_accel_pub == nullptr) {
PX4_ERR("sensor_accel advert fail");
return false;
}
_mag_pub = orb_advertise_multi(ORB_ID(sensor_mag), &_mag,
&_mag_orb_class_instance, ORB_PRIO_HIGH - 1);
if (_mag_pub == nullptr) {
PX4_ERR("sensor_mag advert fail");
return false;
}
return true;
}
void PublicationBase::update(void *data)
{
if (_handle != nullptr) {
int ret = orb_publish(getMeta(), getHandle(), data);
if (ret != PX4_OK) { warnx("publish fail"); }
} else {
orb_advert_t handle;
if (_priority > 0) {
handle = orb_advertise_multi(
getMeta(), data,
&_instance, _priority);
} else {
handle = orb_advertise(getMeta(), data);
}
if (int64_t(handle) != PX4_ERROR) {
setHandle(handle);
} else {
warnx("advert fail");
}
}
}
int LidarLitePWM::init()
{
/* do regular cdev init */
int ret = CDev::init();
if (ret != OK) {
return ERROR;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s));
if (_reports == nullptr) {
return ERROR;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the distance_sensor 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) {
debug("failed to create distance_sensor object. Did you start uORB?");
}
}
return OK;
}
int uORBTest::UnitTest::test_unadvertise()
{
test_note("Testing unadvertise");
//we still have the advertisements from the previous test_multi calls.
for (int i = 0; i < 4; ++i) {
int ret = orb_unadvertise(_pfd[i]);
if (ret != PX4_OK) {
return test_fail("orb_unadvertise failed (%i)", ret);
}
}
//try to advertise and see whether we get the right instance
int instance_test[4];
struct orb_test t;
for (int i = 0; i < 4; ++i) {
_pfd[i] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance_test[i], ORB_PRIO_MAX);
if (instance_test[i] != i) {
return test_fail("got wrong instance (should be %i, is %i)", i, instance_test[i]);
}
}
for (int i = 0; i < 4; ++i) {
orb_unadvertise(_pfd[i]);
}
return test_note("PASS unadvertise");
}
int LidarLiteI2C::init()
{
int ret = PX4_ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK) {
return ret;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s));
if (_reports == nullptr) {
return ret;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
/* 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_DEBUG("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;
return ret;
}
int
BMI055_gyro::init()
{
int ret;
/* do SPI init (and probe) first */
ret = SPI::init();
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("SPI setup failed");
return ret;
}
/* allocate basic report buffers */
_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
if (_gyro_reports == nullptr) {
goto out;
}
if (reset() != OK) {
goto out;
}
/* Initialize offsets and scales */
_gyro_scale.x_offset = 0;
_gyro_scale.x_scale = 1.0f;
_gyro_scale.y_offset = 0;
_gyro_scale.y_scale = 1.0f;
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("gyro init failed");
return ret;
}
_gyro_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
measure();
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_gyro_reports->get(&grp);
_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
&_gyro_orb_class_instance, (external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
if (_gyro_topic == nullptr) {
warnx("ADVERT FAIL");
}
out:
return ret;
}
int
PX4FLOW::init()
{
int ret = PX4_ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK) {
return ret;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(optical_flow_s));
if (_reports == nullptr) {
return ret;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
/* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {};
if (_class_instance == CLASS_DEVICE_PRIMARY) {
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_HIGH);
if (_distance_sensor_topic == nullptr) {
DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
}
} else {
DEVICE_LOG("not primary range device, not advertising");
}
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
/* get yaw rotation from sensor frame to body frame */
param_t rot = param_find("SENS_FLOW_ROT");
/* only set it if the parameter exists */
if (rot != PARAM_INVALID) {
int32_t val = 6; // the recommended installation for the flow sensor is with the Y sensor axis forward
param_get(rot, &val);
_sensor_rotation = (enum Rotation)val;
}
return ret;
}
int uORBTest::UnitTest::pub_test_multi2_main()
{
int data_next_idx = 0;
const int num_instances = 3;
orb_advert_t orb_pub[num_instances];
struct orb_test_medium data_topic;
for (int i = 0; i < num_instances; ++i) {
orb_advert_t &pub = orb_pub[i];
int idx = i;
// PX4_WARN("advertise %i, t=%" PRIu64, i, hrt_absolute_time());
pub = orb_advertise_multi(ORB_ID(orb_test_medium_multi), &data_topic, &idx, ORB_PRIO_DEFAULT);
if (idx != i) {
_thread_should_exit = true;
PX4_ERR("Got wrong instance! should be: %i, but is %i", i, idx);
return -1;
}
}
usleep(100 * 1000);
int message_counter = 0, num_messages = 50 * num_instances;
while (message_counter++ < num_messages) {
usleep(2); //make sure the timestamps are different
orb_advert_t &pub = orb_pub[data_next_idx];
data_topic.time = hrt_absolute_time();
data_topic.val = data_next_idx;
orb_publish(ORB_ID(orb_test_medium_multi), pub, &data_topic);
// PX4_WARN("publishing msg (idx=%i, t=%" PRIu64 ")", data_next_idx, data_topic.time);
data_next_idx = (data_next_idx + 1) % num_instances;
if (data_next_idx == 0) {
usleep(50 * 1000);
}
}
usleep(100 * 1000);
_thread_should_exit = true;
for (int i = 0; i < num_instances; ++i) {
orb_unadvertise(orb_pub[i]);
}
return 0;
}
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 PX4Magnetometer::init()
{
mag_report report{};
report.device_id = _device_id.devid;
if (_topic == nullptr) {
_topic = orb_advertise_multi(ORB_ID(sensor_mag), &report, &_orb_class_instance, ORB_PRIO_HIGH - 1);
if (_topic == nullptr) {
PX4_ERR("Advertise failed.");
return PX4_ERROR;
}
}
return PX4_OK;
}
int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, const void *data, int *instance,
int priority)
{
if (*handle == nullptr) {
*handle = orb_advertise_multi(meta, data, instance, priority);
if (*handle != nullptr) {
return 0;
}
} else {
return orb_publish(meta, *handle, data);
}
return -1;
}
int
IIS328DQ::init()
{
int ret = ERROR;
if (probe() != OK)
goto out;
/* do SPI init (and probe) first */
if (CDev::init() != OK)
goto out;
/* allocate basic report buffers */
if (_reports != NULL) {
ringbuf_deinit(_reports);
vPortFree(_reports);
_reports = NULL;
}
/* allocate basic report buffers */
_reports = (ringbuf_t *) pvPortMalloc (sizeof(ringbuf_t));
ringbuf_init(_reports, 2, sizeof(accel_report));
if (_reports == NULL)
goto out;
_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
reset();
measure();
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
ringbuf_get(_reports, &arp, sizeof(arp));
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
&_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
if (_accel_topic == NULL) {
DEVICE_DEBUG("failed to create sensor_accel publication");
}
ret = OK;
out:
return ret;
}
void
PX4FMU::publish_pwm_outputs(uint16_t *values, size_t numvalues) {
actuator_outputs_s outputs;
outputs.noutputs = numvalues;
outputs.timestamp = hrt_absolute_time();
for (size_t i = 0; i < _max_actuators; ++i) {
outputs.output[i] = i < numvalues ? (float)values[i] : 0;
}
if (_outputs_pub == nullptr) {
int instance = -1;
_outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &instance, ORB_PRIO_DEFAULT);
} else {
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
}
}
int
SF0X::init()
{
/* status */
int ret = 0;
do { /* create a scope to handle exit conditions using break */
/* do regular cdev init */
ret = CDev::init();
if (ret != OK) { break; }
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
if (_reports == nullptr) {
warnx("mem err");
ret = -1;
break;
}
_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 = {};
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_HIGH);
if (_distance_sensor_topic == nullptr) {
DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
}
}
} while (0);
/* close the fd */
::close(_fd);
_fd = -1;
return 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
MPU9250_mag::init()
{
int ret;
ret = CDev::init();
/* if setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("MPU9250 mag init failed");
return ret;
}
_mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
if (_mag_reports == nullptr) {
goto out;
}
_mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
ak8963_setup();
/* advertise sensor topic, measure manually to initialize valid report */
struct mag_report mrp;
_mag_reports->get(&mrp);
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp,
&_mag_orb_class_instance, ORB_PRIO_LOW);
// &_mag_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
if (_mag_topic == nullptr) {
warnx("ADVERT FAIL");
}
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;
}
int
CM8JL65::init()
{
/* status */
int ret = 0;
do { /* create a scope to handle exit conditions using break */
/* do regular cdev init */
ret = CDev::init();
if (ret != OK) { break; }
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
if (_reports == nullptr) {
PX4_ERR("alloc failed");
ret = -1;
break;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
/* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {};
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_HIGH);
if (_distance_sensor_topic == nullptr) {
PX4_ERR("failed to create distance_sensor object");
}
} while (0);
return ret;
}
int
PX4FLOW::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(optical_flow_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 = {};
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_HIGH);
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 uORBTest::UnitTest::test_multi_reversed()
{
test_note("try multi-topic support subscribing before publishing");
/* For these tests 0 and 1 instances are taken from before, therefore continue with 2 and 3. */
/* Subscribe first and advertise afterwards. */
int sfd2 = orb_subscribe_multi(ORB_ID(orb_multitest), 2);
if (sfd2 < 0) {
return test_fail("sub. id2: ret: %d", sfd2);
}
struct orb_test t {}, u {};
t.val = 0;
int instance2;
_pfd[2] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance2, ORB_PRIO_MAX);
int instance3;
_pfd[3] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance3, ORB_PRIO_MIN);
test_note("advertised");
if (instance2 != 2) {
return test_fail("mult. id2: %d", instance2);
}
if (instance3 != 3) {
return test_fail("mult. id3: %d", instance3);
}
t.val = 204;
if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[2], &t)) {
return test_fail("mult. pub0 fail");
}
t.val = 304;
if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[3], &t)) {
return test_fail("mult. pub1 fail");
}
test_note("published");
if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd2, &u)) {
return test_fail("sub #2 copy failed: %d", errno);
}
if (u.val != 204) {
return test_fail("sub #3 val. mismatch: %d", u.val);
}
int sfd3 = orb_subscribe_multi(ORB_ID(orb_multitest), 3);
if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd3, &u)) {
return test_fail("sub #3 copy failed: %d", errno);
}
if (u.val != 304) {
return test_fail("sub #3 val. mismatch: %d", u.val);
}
return test_note("PASS multi-topic reversed");
}
int
QMC5883::collect()
{
#pragma pack(push, 1)
struct { /* status register and data as read back from the device */
uint8_t x[2];
uint8_t y[2];
uint8_t z[2];
} qmc_report;
#pragma pack(pop)
struct {
int16_t x, y, z;
} report;
int ret;
uint8_t check_counter;
perf_begin(_sample_perf);
struct mag_report new_report;
bool sensor_is_onboard = false;
float xraw_f;
float yraw_f;
float zraw_f;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
new_report.timestamp = hrt_absolute_time();
new_report.error_count = perf_event_count(_comms_errors);
new_report.scaling = _range_scale;
new_report.device_id = _device_id.devid;
/*
* @note We could read the status register here, which could tell us that
* we were too early and that the output registers are still being
* written. In the common case that would just slow us down, and
* we're better off just never being early.
*/
/* get measurements from the device */
ret = _interface->read(QMC5883_ADDR_DATA_OUT_X_LSB, (uint8_t *)&qmc_report, sizeof(qmc_report));
if (ret != OK) {
perf_count(_comms_errors);
DEVICE_DEBUG("data/status read error");
goto out;
}
/* map data we just received LSB, MSB */
report.x = (((int16_t)qmc_report.x[1]) << 8) + qmc_report.x[0];
report.y = (((int16_t)qmc_report.y[1]) << 8) + qmc_report.y[0];
report.z = (((int16_t)qmc_report.z[1]) << 8) + qmc_report.z[0];
/*
* If any of the values are -4096, there was an internal math error in the sensor.
* Generalise this to a simple range check that will also catch some bit errors.
*/
if ((abs(report.x) > QMC5883_MAX_COUNT) ||
(abs(report.y) > QMC5883_MAX_COUNT) ||
(abs(report.z) > QMC5883_MAX_COUNT)) {
perf_count(_comms_errors);
goto out;
}
/* get temperature measurements from the device */
new_report.temperature = 0;
if (_temperature_counter++ == 100) {
uint8_t raw_temperature[2];
_temperature_counter = 0;
ret = _interface->read(QMC5883_ADDR_TEMP_OUT_LSB,
raw_temperature, sizeof(raw_temperature));
if (ret == OK) {
int16_t temp16 = (((int16_t)raw_temperature[1]) << 8) +
raw_temperature[0];
new_report.temperature = QMC5883_TEMP_OFFSET + temp16 * 1.0f / 100.0f;
}
} else {
new_report.temperature = _last_report.temperature;
}
/*
* RAW outputs
*
* to align the sensor axes with the board, x and y need to be flipped
* and y needs to be negated
*/
new_report.x_raw = -report.y;
new_report.y_raw = report.x;
/* z remains z */
new_report.z_raw = report.z;
/* scale values for output */
// XXX revisit for SPI part, might require a bus type IOCTL
unsigned dummy;
sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
new_report.is_external = !sensor_is_onboard;
if (sensor_is_onboard) {
// convert onboard so it matches offboard for the
// scaling below
report.y = -report.y;
report.x = -report.x;
}
/* the standard external mag by 3DR has x pointing to the
* right, y pointing backwards, and z down, therefore switch x
* and y and invert y */
//TODO: sort out axes mapping
xraw_f = -report.y;
yraw_f = report.x;
zraw_f = report.z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */
new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale;
if (!(_pub_blocked)) {
if (_mag_topic != nullptr) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
} else {
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report,
&_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);
if (_mag_topic == nullptr) {
DEVICE_DEBUG("ADVERT FAIL");
}
}
}
_last_report = new_report;
/* post a report to the ring */
_reports->force(&new_report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
/*
periodically check the configuration
registers. With a bad I2C cable it is possible for the
registers to become corrupt, leading to bad readings. It
doesn't happen often, but given the poor cables some
vehicles have it is worth checking for.
*/
check_counter = perf_event_count(_sample_perf) % 256;
if (check_counter == 0) {
check_conf();
}
ret = OK;
out:
perf_end(_sample_perf);
return ret;
}
int
SF1XX::init()
{
int ret = PX4_ERROR;
int hw_model;
param_get(param_find("SENS_EN_SF1XX"), &hw_model);
switch (hw_model) {
case 0:
DEVICE_LOG("disabled.");
return ret;
case 1: /* SF10/a (25m 32Hz) */
_min_distance = 0.01f;
_max_distance = 25.0f;
_conversion_interval = 31250;
break;
case 2: /* SF10/b (50m 32Hz) */
_min_distance = 0.01f;
_max_distance = 50.0f;
_conversion_interval = 31250;
break;
case 3: /* SF10/c (100m 16Hz) */
_min_distance = 0.01f;
_max_distance = 100.0f;
_conversion_interval = 62500;
break;
case 4:
/* SF11/c (120m 20Hz) */
_min_distance = 0.01f;
_max_distance = 120.0f;
_conversion_interval = 50000;
break;
case 5:
/* SF20/LW20 (100m 48-388Hz) */
_min_distance = 0.001f;
_max_distance = 100.0f;
_conversion_interval = 20834;
break;
default:
DEVICE_LOG("invalid HW model %d.", hw_model);
return ret;
}
/* do I2C init (and probe) first */
if (I2C::init() != OK) {
return ret;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
set_address(SF1XX_BASEADDR);
if (_reports == nullptr) {
return ret;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
/* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {};
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_HIGH);
if (_distance_sensor_topic == nullptr) {
DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
}
// Select altitude register
int ret2 = measure();
if (ret2 == 0) {
ret = OK;
_sensor_ok = true;
DEVICE_LOG("(%dm %dHz) with address %d found", (int)_max_distance,
(int)(1e6f / _conversion_interval), SF1XX_BASEADDR);
}
return ret;
}
int
LIS3MDL::collect()
{
#pragma pack(push, 1)
struct { /* status register and data as read back from the device */
uint8_t x[2];
uint8_t y[2];
uint8_t z[2];
uint8_t t[2];
} lis_report;
struct {
int16_t x;
int16_t y;
int16_t z;
int16_t t;
} report;
#pragma pack(pop)
int ret;
// uint8_t check_counter;
perf_begin(_sample_perf);
struct mag_report new_report;
bool sensor_is_onboard = false;
float xraw_f;
float yraw_f;
float zraw_f;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
new_report.timestamp = hrt_absolute_time();
new_report.error_count = perf_event_count(_comms_errors);
new_report.range_ga = _range_ga;
new_report.scaling = _range_scale;
new_report.device_id = _device_id.devid;
/*
* @note We could read the status register here, which could tell us that
* we were too early and that the output registers are still being
* written. In the common case that would just slow us down, and
* we're better off just never being early.
*/
/* get measurements from the device */
ret = _interface->read(ADDR_OUT_X_L, (uint8_t *)&lis_report, sizeof(lis_report));
if (ret != OK) {
perf_count(_comms_errors);
DEVICE_DEBUG("data/status read error");
goto out;
}
/* convert the data we just received */
report.x = (((int16_t)lis_report.x[1]) << 8) + lis_report.x[0];
report.y = (((int16_t)lis_report.y[1]) << 8) + lis_report.y[0];
report.z = (((int16_t)lis_report.z[1]) << 8) + lis_report.z[0];
report.t = (((int16_t)lis_report.t[1]) << 8) + lis_report.t[0];
/* get measurements from the device */
new_report.temperature = report.t;
new_report.temperature = 25 + (report.t / (16 * 8.0f));
// XXX revisit for SPI part, might require a bus type IOCTL
unsigned dummy;
sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
new_report.is_external = !sensor_is_onboard;
/*
* RAW outputs
*
*/
new_report.x_raw = report.x;
new_report.y_raw = report.y;
new_report.z_raw = report.z;
/* the LIS3MDL mag on Pixhawk Pro by Drotek has x pointing towards,
* y pointing to the right, and z down, therefore no switch needed,
* it is better to have no artificial rotation inside the
* driver and then use the startup script with -R command with the
* real rotation between the sensor and body frame */
xraw_f = report.x;
yraw_f = report.y;
zraw_f = report.z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */
new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale;
if (!(_pub_blocked)) {
if (_mag_topic != nullptr) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
} else {
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report,
&_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);
if (_mag_topic == nullptr) {
DEVICE_DEBUG("ADVERT FAIL");
}
}
}
_last_report = new_report;
/* post a report to the ring */
_reports->force(&new_report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
check_conf();
ret = OK;
out:
perf_end(_sample_perf);
return ret;
}
int
MPU9250::init()
{
#if defined(USE_I2C)
unsigned dummy;
use_i2c(_interface->ioctl(MPUIOCGIS_I2C, dummy));
#endif
int ret = probe();
if (ret != OK) {
DEVICE_DEBUG("MPU9250 probe failed");
return ret;
}
/* do init */
ret = CDev::init();
/* if init failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("CDev init failed");
return ret;
}
/* allocate basic report buffers */
_accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
if (_accel_reports == nullptr) {
goto out;
}
_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report));
if (_gyro_reports == nullptr) {
goto out;
}
if (reset() != OK) {
goto out;
}
/* Initialize offsets and scales */
_accel_scale.x_offset = 0;
_accel_scale.x_scale = 1.0f;
_accel_scale.y_offset = 0;
_accel_scale.y_scale = 1.0f;
_accel_scale.z_offset = 0;
_accel_scale.z_scale = 1.0f;
_gyro_scale.x_offset = 0;
_gyro_scale.x_scale = 1.0f;
_gyro_scale.y_offset = 0;
_gyro_scale.y_scale = 1.0f;
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
/* do CDev init for the gyro device node, keep it optional */
ret = _gyro->init();
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("gyro init failed");
return ret;
}
#ifdef USE_I2C
if (!_mag->is_passthrough() && _mag->_interface->init() != OK) {
warnx("failed to setup ak8963 interface");
}
#endif
/* do CDev init for the mag device node, keep it optional */
ret = _mag->init();
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("mag init failed");
return ret;
}
_accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
measure();
/* 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_MAX - 1 : ORB_PRIO_HIGH - 1);
if (_accel_topic == nullptr) {
warnx("ADVERT FAIL");
}
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_gyro_reports->get(&grp);
_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
&_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
if (_gyro->_gyro_topic == nullptr) {
warnx("ADVERT FAIL");
}
out:
return ret;
}
/**
* Advertise as the publisher of a topic.
*
* This performs the initial advertisement of a topic; it creates the topic
* node in /obj if required and publishes the initial data.
*
* Any number of advertisers may publish to a topic; publications are atomic
* but co-ordination between publishers is not provided by the ORB.
*
* Internally this will call orb_advertise_multi with an instance of 0 and
* default priority.
*
* @param meta The uORB metadata (usually from the ORB_ID() macro)
* for the topic.
* @param data A pointer to the initial data to be published.
* For topics updated by interrupt handlers, the advertisement
* must be performed from non-interrupt context.
* @param queue_size Maximum number of buffered elements. If this is 1, no queuing is
* used.
* @return nullptr on error, otherwise returns an object pointer
* that can be used to publish to the topic.
* If the topic in question is not known (due to an
* ORB_DEFINE with no corresponding ORB_DECLARE)
* this function will return nullptr and set errno to ENOENT.
*/
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data, unsigned int queue_size = 1)
{
return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT, queue_size);
}
int
MB12XX::init()
{
int ret = PX4_ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK) {
return ret;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
_index_counter = MB12XX_BASEADDR; /* set temp sonar i2c address to base adress */
set_device_address(_index_counter); /* set I2c port to temp sonar i2c adress */
if (_reports == nullptr) {
return ret;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
/* get a publish handle on the range finder topic */
struct distance_sensor_s 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?");
}
// XXX we should find out why we need to wait 200 ms here
usleep(200000);
/* check for connected rangefinders on each i2c port:
We start from i2c base address (0x70 = 112) and count downwards
So second iteration it uses i2c address 111, third iteration 110 and so on*/
for (unsigned counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) {
_index_counter = MB12XX_BASEADDR - counter; /* set temp sonar i2c address to base adress - counter */
set_device_address(_index_counter); /* set I2c port to temp sonar i2c adress */
int ret2 = measure();
if (ret2 == 0) { /* sonar is present -> store address_index in array */
addr_ind.push_back(_index_counter);
DEVICE_DEBUG("sonar added");
_latest_sonar_measurements.push_back(200);
}
}
_index_counter = MB12XX_BASEADDR;
set_device_address(_index_counter); /* set i2c port back to base adress for rest of driver */
/* if only one sonar detected, no special timing is required between firing, so use default */
if (addr_ind.size() == 1) {
_cycling_rate = MB12XX_CONVERSION_INTERVAL;
} else {
_cycling_rate = TICKS_BETWEEN_SUCCESIVE_FIRES;
}
/* show the connected sonars in terminal */
for (unsigned i = 0; i < addr_ind.size(); i++) {
DEVICE_LOG("sonar %d with address %d added", (i + 1), addr_ind[i]);
}
DEVICE_DEBUG("Number of sonars connected: %d", addr_ind.size());
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
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
BMI160::init()
{
int ret;
/* do SPI init (and probe) first */
ret = SPI::init();
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("SPI setup failed");
return ret;
}
/* allocate basic report buffers */
_accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
if (_accel_reports == nullptr) {
goto out;
}
_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report));
if (_gyro_reports == nullptr) {
goto out;
}
if (reset() != OK) {
goto out;
}
/* Initialize offsets and scales */
_accel_scale.x_offset = 0;
_accel_scale.x_scale = 1.0f;
_accel_scale.y_offset = 0;
_accel_scale.y_scale = 1.0f;
_accel_scale.z_offset = 0;
_accel_scale.z_scale = 1.0f;
_gyro_scale.x_offset = 0;
_gyro_scale.x_scale = 1.0f;
_gyro_scale.y_offset = 0;
_gyro_scale.y_scale = 1.0f;
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
/* do CDev init for the gyro device node, keep it optional */
ret = _gyro->init();
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("gyro init failed");
return ret;
}
_accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
measure();
/* 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_MAX - 1 : ORB_PRIO_HIGH - 1);
if (_accel_topic == nullptr) {
warnx("ADVERT FAIL");
}
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_gyro_reports->get(&grp);
_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
&_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
if (_gyro->_gyro_topic == nullptr) {
warnx("ADVERT FAIL");
}
out:
return ret;
}
int uORBTest::UnitTest::test_multi()
{
/* this routine tests the multi-topic support */
test_note("try multi-topic support");
struct orb_test t {}, u {};
t.val = 0;
int instance0;
_pfd[0] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX);
test_note("advertised");
int instance1;
_pfd[1] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN);
if (instance0 != 0) {
return test_fail("mult. id0: %d", instance0);
}
if (instance1 != 1) {
return test_fail("mult. id1: %d", instance1);
}
t.val = 103;
if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[0], &t)) {
return test_fail("mult. pub0 fail");
}
test_note("published");
t.val = 203;
if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[1], &t)) {
return test_fail("mult. pub1 fail");
}
/* subscribe to both topics and ensure valid data is received */
int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0);
if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd0, &u)) {
return test_fail("sub #0 copy failed: %d", errno);
}
if (u.val != 103) {
return test_fail("sub #0 val. mismatch: %d", u.val);
}
int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1);
if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd1, &u)) {
return test_fail("sub #1 copy failed: %d", errno);
}
if (u.val != 203) {
return test_fail("sub #1 val. mismatch: %d", u.val);
}
/* test priorities */
int prio;
if (PX4_OK != orb_priority(sfd0, &prio)) {
return test_fail("prio #0");
}
if (prio != ORB_PRIO_MAX) {
return test_fail("prio: %d", prio);
}
if (PX4_OK != orb_priority(sfd1, &prio)) {
return test_fail("prio #1");
}
if (prio != ORB_PRIO_MIN) {
return test_fail("prio: %d", prio);
}
if (PX4_OK != latency_test<struct orb_test>(ORB_ID(orb_test), false)) {
return test_fail("latency test failed");
}
orb_unsubscribe(sfd0);
orb_unsubscribe(sfd1);
return test_note("PASS multi-topic test");
}
int
IST8310::collect()
{
#pragma pack(push, 1)
struct { /* status register and data as read back from the device */
uint8_t x[2];
uint8_t y[2];
uint8_t z[2];
} report_buffer;
#pragma pack(pop)
struct {
int16_t x, y, z;
} report;
int ret;
uint8_t check_counter;
perf_begin(_sample_perf);
struct mag_report new_report;
const bool sensor_is_external = external();
float xraw_f;
float yraw_f;
float zraw_f;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
new_report.timestamp = hrt_absolute_time();
new_report.is_external = sensor_is_external;
new_report.error_count = perf_event_count(_comms_errors);
new_report.scaling = _range_scale;
new_report.device_id = _device_id.devid;
/*
* @note We could read the status register here, which could tell us that
* we were too early and that the output registers are still being
* written. In the common case that would just slow us down, and
* we're better off just never being early.
*/
/* get measurements from the device */
ret = read(ADDR_DATA_OUT_X_LSB, (uint8_t *)&report_buffer, sizeof(report_buffer));
if (ret != OK) {
perf_count(_comms_errors);
DEVICE_DEBUG("I2C read error");
goto out;
}
/* swap the data we just received */
report.x = (((int16_t)report_buffer.x[1]) << 8) | (int16_t)report_buffer.x[0];
report.y = (((int16_t)report_buffer.y[1]) << 8) | (int16_t)report_buffer.y[0];
report.z = (((int16_t)report_buffer.z[1]) << 8) | (int16_t)report_buffer.z[0];
/*
* Check if value makes sense according to the FSR and Resolution of
* this sensor, discarding outliers
*/
if (report.x > IST8310_MAX_VAL_XY || report.x < IST8310_MIN_VAL_XY ||
report.y > IST8310_MAX_VAL_XY || report.y < IST8310_MIN_VAL_XY ||
report.z > IST8310_MAX_VAL_Z || report.z < IST8310_MIN_VAL_Z) {
perf_count(_range_errors);
DEVICE_DEBUG("data/status read error");
goto out;
}
/* temperature measurement is not available on IST8310 */
new_report.temperature = 0;
/*
* raw outputs
*
* Sensor doesn't follow right hand rule, swap x and y to make it obey
* it.
*/
new_report.x_raw = report.y;
new_report.y_raw = report.x;
new_report.z_raw = report.z;
/* scale values for output */
xraw_f = report.y;
yraw_f = report.x;
zraw_f = report.z;
/* apply user specified rotation */
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale;
new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale;
new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale;
if (!(_pub_blocked)) {
if (_mag_topic != nullptr) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
} else {
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report,
&_orb_class_instance, sensor_is_external ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
if (_mag_topic == nullptr) {
DEVICE_DEBUG("ADVERT FAIL");
}
}
}
_last_report = new_report;
/* post a report to the ring */
_reports->force(&new_report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
/*
periodically check the range register and configuration
registers. With a bad I2C cable it is possible for the
registers to become corrupt, leading to bad readings. It
doesn't happen often, but given the poor cables some
vehicles have it is worth checking for.
*/
check_counter = perf_event_count(_sample_perf) % 256;
if (check_counter == 128) {
check_conf();
}
ret = OK;
out:
perf_end(_sample_perf);
return ret;
}
