int
HMC5883_I2C::probe()
{
uint8_t data[3] = {0, 0, 0};
_retries = 10;
if (read(ADDR_ID_A, &data[0], 1) ||
read(ADDR_ID_B, &data[1], 1) ||
read(ADDR_ID_C, &data[2], 1)) {
DEVICE_DEBUG("read_reg fail");
return -EIO;
}
_retries = 2;
if ((data[0] != ID_A_WHO_AM_I) ||
(data[1] != ID_B_WHO_AM_I) ||
(data[2] != ID_C_WHO_AM_I)) {
DEVICE_DEBUG("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]);
return -EIO;
}
return OK;
}
void
LIS3MDL::cycle()
{
/* _measure_ticks == 0 is used as _task_should_exit */
if (_measure_ticks == 0) {
return;
}
/* Collect last measurement at the start of every cycle */
if (collect() != OK) {
DEVICE_DEBUG("collection error");
/* restart the measurement state machine */
start();
return;
}
if (measure() != OK) {
DEVICE_DEBUG("measure error");
}
if (_measure_ticks > 0) {
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&LIS3MDL::cycle_trampoline,
this,
USEC2TICK(LIS3MDL_CONVERSION_INTERVAL));
}
}
int
LIS3MDL_SPI::init()
{
int ret;
ret = SPI::init();
if (ret != OK) {
DEVICE_DEBUG("SPI init failed");
return -EIO;
}
// read WHO_AM_I value
uint8_t data = 0;
if (read(ADDR_WHO_AM_I, &data, 1)) {
DEVICE_DEBUG("LIS3MDL read_reg fail");
}
if (data != ID_WHO_AM_I) {
DEVICE_DEBUG("LIS3MDL bad ID: %02x", data);
return -EIO;
}
return OK;
}
int
LPS25H::init()
{
int ret;
ret = CDev::init();
if (ret != OK) {
DEVICE_DEBUG("CDev init failed");
goto out;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_baro_s));
if (_reports == nullptr) {
DEVICE_DEBUG("can't get memory for reports");
ret = -ENOMEM;
goto out;
}
if (reset() != OK) {
goto out;
}
/* register alternate interfaces if we have to */
_class_instance = register_class_devname(BARO_BASE_DEVICE_PATH);
ret = OK;
out:
return ret;
}
int
HMC5883_SPI::init()
{
int ret;
ret = SPI::init();
if (ret != OK) {
DEVICE_DEBUG("SPI init failed");
return -EIO;
}
// read WHO_AM_I value
uint8_t data[3] = {0, 0, 0};
if (read(ADDR_ID_A, &data[0], 1) ||
read(ADDR_ID_B, &data[1], 1) ||
read(ADDR_ID_C, &data[2], 1)) {
DEVICE_DEBUG("read_reg fail");
}
if ((data[0] != ID_A_WHO_AM_I) ||
(data[1] != ID_B_WHO_AM_I) ||
(data[2] != ID_C_WHO_AM_I)) {
DEVICE_DEBUG("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]);
return -EIO;
}
return OK;
}
void
PCA9685::subscribe()
{
/* subscribe/unsubscribe to required actuator control groups */
uint32_t sub_groups = _groups_required & ~_groups_subscribed;
uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
_poll_fds_num = 0;
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (sub_groups & (1 << i)) {
DEVICE_DEBUG("subscribe to actuator_controls_%d", i);
_control_subs[i] = orb_subscribe(_control_topics[i]);
}
if (unsub_groups & (1 << i)) {
DEVICE_DEBUG("unsubscribe from actuator_controls_%d", i);
::close(_control_subs[i]);
_control_subs[i] = -1;
}
if (_control_subs[i] > 0) {
_poll_fds[_poll_fds_num].fd = _control_subs[i];
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num++;
}
}
}
int
LPS25H_SPI::init()
{
int ret;
ret = SPI::init();
if (ret != OK) {
DEVICE_DEBUG("SPI init failed");
return -EIO;
}
// read WHO_AM_I value
uint8_t id;
if (read(ADDR_ID, &id, 1)) {
DEVICE_DEBUG("read_reg fail");
return -EIO;
}
if (id != ID_WHO_AM_I) {
DEVICE_DEBUG("ID byte mismatch (%02x != %02x)", ID_WHO_AM_I, id);
return -EIO;
}
return OK;
}
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;
}
void LidarLiteI2C::cycle()
{
/* collection phase? */
if (_collect_phase) {
/* try a collection */
if (OK != collect()) {
DEVICE_DEBUG("collection error");
/* if we've been waiting more than 200ms then
send a new acquire */
if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT * 2) {
_collect_phase = false;
}
} else {
/* next phase is measurement */
_collect_phase = false;
/*
* Is there a collect->measure gap?
*/
if (getMeasureTicks() > USEC2TICK(LL40LS_CONVERSION_INTERVAL)) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&LidarLiteI2C::cycle_trampoline,
this,
getMeasureTicks() - USEC2TICK(LL40LS_CONVERSION_INTERVAL));
return;
}
}
}
if (_collect_phase == false) {
/* measurement phase */
if (OK != measure()) {
DEVICE_DEBUG("measure error");
} else {
/* next phase is collection. Don't switch to
collection phase until we have a successful
acquire request I2C transfer */
_collect_phase = true;
}
}
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&LidarLiteI2C::cycle_trampoline,
this,
USEC2TICK(LL40LS_CONVERSION_INTERVAL));
}
void
LIS3MDL::cycle()
{
if (_measure_ticks == 0) {
return;
}
/* collection phase? */
if (_collect_phase) {
/* perform collection */
if (OK != collect()) {
DEVICE_DEBUG("collection error");
/* restart the measurement state machine */
start();
return;
}
/* next phase is measurement */
_collect_phase = false;
/*
* Is there a collect->measure gap?
*/
if (_measure_ticks > USEC2TICK(LIS3MDL_CONVERSION_INTERVAL)) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&LIS3MDL::cycle_trampoline,
this,
_measure_ticks - USEC2TICK(LIS3MDL_CONVERSION_INTERVAL));
return;
}
}
/* measurement phase */
if (OK != measure()) {
DEVICE_DEBUG("measure error");
}
/* next phase is collection */
_collect_phase = true;
if (_measure_ticks > 0) {
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&LIS3MDL::cycle_trampoline,
this,
USEC2TICK(LIS3MDL_CONVERSION_INTERVAL));
}
}
int
ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
{
int result = OK;
DEVICE_DEBUG("ioctl %i %u", cmd, arg);
// irqstate_t flags = enter_critical_section();
/* decide whether to increase the alarm level to cmd or leave it alone */
switch (cmd) {
case TONE_SET_ALARM:
DEVICE_DEBUG("TONE_SET_ALARM %u", arg);
if (arg < TONE_NUMBER_OF_TUNES) {
if (arg == TONE_STOP_TUNE) {
// stop the tune
_tune = nullptr;
_next = nullptr;
_repeat = false;
_default_tune_number = 0;
} else {
/* always interrupt alarms, unless they are repeating and already playing */
if (!(_repeat && _default_tune_number == arg)) {
/* play the selected tune */
_default_tune_number = arg;
start_tune(_default_tunes[arg]);
}
}
} else {
result = -EINVAL;
}
break;
default:
result = -ENOTTY;
break;
}
// leave_critical_section(flags);
/* give it to the superclass if we didn't like it */
if (result == -ENOTTY) {
result = CDev::ioctl(filp, cmd, arg);
}
return result;
}
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
PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
{
int ret;
/* try it as a GPIO ioctl first */
ret = gpio_ioctl(filp, cmd, arg);
if (ret != -ENOTTY) {
return ret;
}
/* if we are in valid PWM mode, try it as a PWM ioctl as well */
switch (_mode) {
case MODE_2PWM:
case MODE_4PWM:
case MODE_6PWM:
#ifdef CONFIG_ARCH_BOARD_AEROCORE
case MODE_8PWM:
#endif
ret = pwm_ioctl(filp, cmd, arg);
break;
default:
DEVICE_DEBUG("not in a PWM mode");
break;
}
/* if nobody wants it, let CDev have it */
if (ret == -ENOTTY) {
ret = CDev::ioctl(filp, cmd, arg);
}
return ret;
}
int LidarLiteI2C::probe()
{
// cope with both old and new I2C bus address
const uint8_t addresses[2] = {LL40LS_BASEADDR, LL40LS_BASEADDR_OLD};
// more retries for detection
_retries = 10;
for (uint8_t i = 0; i < sizeof(addresses); i++) {
/*
check for hw and sw versions. It would be better if
we had a proper WHOAMI register
*/
if (read_reg(LL40LS_HW_VERSION, _hw_version) == OK && _hw_version > 0 &&
read_reg(LL40LS_SW_VERSION, _sw_version) == OK && _sw_version > 0) {
goto ok;
}
DEVICE_DEBUG("probe failed hw_version=0x%02x sw_version=0x%02x\n",
(unsigned)_hw_version,
(unsigned)_sw_version);
}
// not found on any address
return -EIO;
ok:
_retries = 3;
return reset_sensor();
}
int
LIS3MDL::init()
{
int ret = PX4_ERROR;
ret = CDev::init();
if (ret != OK) {
DEVICE_DEBUG("CDev init failed");
return ret;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
if (_reports == nullptr) {
return PX4_ERROR;
}
/* reset the device configuration */
reset();
_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
return PX4_OK;
}
int
QMC5883::init()
{
int ret = PX4_ERROR;
ret = CDev::init();
if (ret != OK) {
DEVICE_DEBUG("CDev init failed");
goto out;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
if (_reports == nullptr) {
goto out;
}
/* reset the device configuration */
reset();
_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
ret = OK;
/* sensor is ok */
_sensor_ok = true;
out:
return ret;
}
int LidarLitePWM::init()
{
/* do regular cdev init */
int ret = CDev::init();
if (ret != PX4_OK) {
return PX4_ERROR;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s));
if (_reports == nullptr) {
return PX4_ERROR;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
/* 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) {
DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?");
}
return PX4_OK;
}
int
SRF02_I2C::measure()
{
int ret;
/*
* Send the command to begin a measurement.
*/
uint8_t cmd[2];
cmd[0] = 0x00;
cmd[1] = SRF02_TAKE_RANGE_REG;
ret = transfer(cmd, 2, nullptr, 0);
if (OK != ret) {
perf_count(_comms_errors);
DEVICE_DEBUG("i2c::transfer returned %d", ret);
return ret;
}
ret = OK;
return ret;
}
/**
* Main loop function
*/
void
PCA9685::i2cpwm()
{
if (_mode == IOX_MODE_TEST_OUT) {
setPin(0, PCA9685_PWMCENTER);
_should_run = true;
} else if (_mode == IOX_MODE_OFF) {
_should_run = false;
} else {
if (!_mode_on_initialized) {
/* Subscribe to actuator control 2 (payload group for gimbal) */
_actuator_controls_sub = orb_subscribe(ORB_ID(actuator_controls_2));
/* set the uorb update interval lower than the driver pwm interval */
orb_set_interval(_actuator_controls_sub, 1000.0f / PCA9685_PWMFREQ - 5);
_mode_on_initialized = true;
}
/* Read the servo setpoints from the actuator control topics (gimbal) */
bool updated;
orb_check(_actuator_controls_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls);
for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
/* Scale the controls to PWM, first multiply by pi to get rad,
* the control[i] values are on the range -1 ... 1 */
uint16_t new_value = PCA9685_PWMCENTER +
(_actuator_controls.control[i] * M_PI_F * PCA9685_SCALE);
DEVICE_DEBUG("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value,
(double)_actuator_controls.control[i]);
if (new_value != _current_values[i] &&
isfinite(new_value) &&
new_value >= PCA9685_PWMMIN &&
new_value <= PCA9685_PWMMAX) {
/* This value was updated, send the command to adjust the PWM value */
setPin(i, new_value);
_current_values[i] = new_value;
}
}
}
_should_run = true;
}
// check if any activity remains, else stop
if (!_should_run) {
_running = false;
return;
}
// re-queue ourselves to run again later
_running = true;
work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, _i2cpwm_interval);
}
int
ADCSIM::init()
{
DEVICE_DEBUG("init done");
/* create the device node */
return VDev::init();
}
int
SPI::init()
{
int ret = OK;
/* attach to the spi bus */
if (_dev == nullptr) {
_dev = up_spiinitialize(_bus);
}
if (_dev == nullptr) {
DEVICE_DEBUG("failed to init SPI");
ret = -ENOENT;
goto out;
}
// tell other SPI users that we may be doing transfers in
// interrupt context
up_spi_set_need_irq_save(_dev);
/* deselect device to ensure high to low transition of pin select */
SPI_SELECT(_dev, _device, false);
/* call the probe function to check whether the device is present */
ret = probe();
if (ret != OK) {
DEVICE_DEBUG("probe failed");
goto out;
}
/* do base class init, which will create the device node, etc. */
ret = CDev::init();
if (ret != OK) {
DEVICE_DEBUG("cdev init failed");
goto out;
}
/* tell the workd where we are */
DEVICE_LOG("on SPI bus %d at %d (%u KHz)", _bus, _device, _frequency / 1000);
out:
return ret;
}
int
TAP_ESC::ioctl(file *filp, int cmd, unsigned long arg)
{
int ret = OK;
switch (cmd) {
case MIXERIOCRESET:
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
_groups_required = 0;
}
break;
case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg;
unsigned buflen = strnlen(buf, 1024);
if (_mixers == nullptr) {
_mixers = new MixerGroup(control_callback, (uintptr_t)_controls);
}
if (_mixers == nullptr) {
_groups_required = 0;
ret = -ENOMEM;
} else {
ret = _mixers->load_from_buf(buf, buflen);
if (ret != 0) {
DEVICE_DEBUG("mixer load failed with %d", ret);
delete _mixers;
_mixers = nullptr;
_groups_required = 0;
ret = -EINVAL;
} else {
_mixers->groups_required(_groups_required);
}
}
break;
}
default:
ret = -ENOTTY;
break;
}
return ret;
}
void
ETSAirspeed::cycle()
{
int ret;
/* collection phase? */
if (_collect_phase) {
/* perform collection */
ret = collect();
if (OK != ret) {
perf_count(_comms_errors);
/* restart the measurement state machine */
start();
_sensor_ok = false;
return;
}
/* next phase is measurement */
_collect_phase = false;
/*
* Is there a collect->measure gap?
*/
if (_measure_ticks > USEC2TICK(CONVERSION_INTERVAL)) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&Airspeed::cycle_trampoline,
this,
_measure_ticks - USEC2TICK(CONVERSION_INTERVAL));
return;
}
}
/* measurement phase */
ret = measure();
if (OK != ret) {
DEVICE_DEBUG("measure error");
}
_sensor_ok = (ret == OK);
/* next phase is collection */
_collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&Airspeed::cycle_trampoline,
this,
USEC2TICK(CONVERSION_INTERVAL));
}
void
PX4FLOW::cycle()
{
if (OK != measure()) {
DEVICE_DEBUG("measure error");
}
/* perform collection */
if (OK != collect()) {
DEVICE_DEBUG("collection error");
/* restart the measurement state machine */
start();
return;
}
work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this,
_measure_ticks);
}
int
I2C::init()
{
int ret = PX4_OK;
// Assume the driver set the desired bus frequency. There is no standard
// way to set it from user space.
// do base class init, which will create device node, etc
ret = VDev::init();
if (ret != PX4_OK) {
DEVICE_DEBUG("VDev::init failed");
return ret;
}
_fd = px4_open(get_devname(), PX4_F_RDONLY | PX4_F_WRONLY);
if (_fd < 0) {
DEVICE_DEBUG("px4_open failed of device %s", get_devname());
return PX4_ERROR;
}
#ifdef __PX4_QURT
simulate = true;
#endif
if (simulate) {
_fd = 10000;
}
else {
#ifndef __PX4_QURT
// Open the actual I2C device and map to the virtual dev name
_fd = ::open(get_devname(), O_RDWR);
if (_fd < 0) {
warnx("could not open %s", get_devname());
px4_errno = errno;
return PX4_ERROR;
}
#endif
}
return ret;
}
int
RM3100_I2C::probe()
{
uint8_t data = 0;
_retries = 10;
if (read(ADDR_REVID, &data, 1)) {
DEVICE_DEBUG("RM3100 read_reg fail");
return -EIO;
}
_retries = 2;
if (data != RM3100_REVID) {
DEVICE_DEBUG("RM3100 bad ID: %02x", data);
return -EIO;
}
return OK;
}
int
LPS22HB_I2C::probe()
{
uint8_t id;
_retries = 10;
if (read(WHO_AM_I, &id, 1)) {
DEVICE_DEBUG("read_reg fail");
return -EIO;
}
_retries = 2;
if (id != LPS22HB_ID_WHO_AM_I) {
DEVICE_DEBUG("ID byte mismatch (%02x != %02x)", LPS22HB_ID_WHO_AM_I, id);
return -EIO;
}
return OK;
}
int
IST8310::probe()
{
uint8_t data[1] = {0};
_retries = 10;
if (read(ADDR_WAI, &data[0], 1)) {
DEVICE_DEBUG("read_reg fail");
return -EIO;
}
_retries = 2;
if ((data[0] != WAI_EXPECTED_VALUE)) {
DEVICE_DEBUG("ID byte mismatch (%02x) expected %02x", data[0], WAI_EXPECTED_VALUE);
return -EIO;
}
return OK;
}
int
SRF02_I2C::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[2] = {0, 0};
uint8_t cmd = 0x02;
perf_begin(_sample_perf);
ret = transfer(&cmd, 1, nullptr, 0);
ret = transfer(nullptr, 0, &val[0], 2);
if (ret < 0) {
DEVICE_DEBUG("error reading from sensor: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
uint16_t distance_cm = val[0] << 8 | val[1];
float distance_m = float(distance_cm) * 1e-2f;
struct distance_sensor_s report;
report.timestamp = hrt_absolute_time();
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
report.orientation = 8;
report.current_distance = distance_m;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0f;
/* TODO: set proper ID */
report.id = 0;
/* publish it, if we are the primary */
if (_distance_sensor_topic != nullptr) {
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
}
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
int
MPU6000_SPI::init()
{
int ret;
ret = SPI::init();
if (ret != OK) {
DEVICE_DEBUG("SPI init failed");
return -EIO;
}
return OK;
}
