void
FXOS8700CQ::stop()
{
hrt_cancel(&_accel_call);
hrt_cancel(&_mag_call);
/* reset internal states */
memset(_last_accel, 0, sizeof(_last_accel));
/* discard unread data in the buffers */
_accel_reports->flush();
_mag_reports->flush();
}
void
PMW3901::start()
{
/* reset the report ring and state machine */
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&PMW3901::cycle_trampoline, this, USEC2TICK(PMW3901_US));
/* notify about state change */
struct subsystem_info_s info = {};
info.timestamp = hrt_absolute_time();
info.present = true;
info.enabled = true;
info.ok = true;
info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW;
if (_subsystem_pub != nullptr) {
orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &info);
} else {
_subsystem_pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
}
ssize_t
MB12XX::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct distance_sensor_s);
struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(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(rbuf)) {
ret += sizeof(*rbuf);
rbuf++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement - run one conversion */
do {
_reports->flush();
/* trigger a measurement */
if (OK != measure()) {
ret = -EIO;
break;
}
/* wait for it to complete */
usleep(_cycling_rate * 2);
/* run the collection phase */
if (OK != collect()) {
ret = -EIO;
break;
}
/* state machine will have generated a report, copy it out */
if (_reports->get(rbuf)) {
ret = sizeof(*rbuf);
}
} while (0);
return ret;
}
void
MB12XX::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 5);
/* notify about state change */
struct subsystem_info_s info = {};
info.present = true;
info.enabled = true;
info.ok = true;
info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
static orb_advert_t pub = nullptr;
if (pub != nullptr) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
}
void
SF0X::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&SF0X::cycle_trampoline, this, 1);
// /* notify about state change */
// struct subsystem_info_s info = {
// true,
// true,
// true,
// SUBSYSTEM_TYPE_RANGEFINDER
// };
// static orb_advert_t pub = -1;
// if (pub > 0) {
// orb_publish(ORB_ID(subsystem_info), pub, &info);
// } else {
// pub = orb_advertise(ORB_ID(subsystem_info), &info);
// }
}
void
PX4FLOW::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1);
/* notify about state change */
struct subsystem_info_s info = {
true,
true,
true,
subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW
};
static orb_advert_t pub = nullptr;
if (pub != nullptr) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
}
void
FXOS8700CQ::start()
{
/* make sure we are stopped first */
stop();
/* reset the report ring */
_accel_reports->flush();
_mag_reports->flush();
/* start polling at the specified rate */
hrt_call_every(&_accel_call,
1000,
_call_accel_interval - FXOS8700C_TIMER_REDUCTION,
(hrt_callout)&FXOS8700CQ::measure_trampoline, this);
hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&FXOS8700CQ::mag_measure_trampoline, this);
}
ssize_t
LPS25H::read(struct file *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 */
/* XXX really it'd be nice to lock against other readers here */
do {
_reports->flush();
/* trigger a measurement */
if (OK != measure()) {
ret = -EIO;
break;
}
/* wait for it to complete */
usleep(LPS25H_CONVERSION_INTERVAL);
/* run the collection phase */
if (OK != collect()) {
ret = -EIO;
break;
}
if (_reports->get(brp)) {
ret = sizeof(struct baro_report);
}
} while (0);
return ret;
}
void
BATT_SMBUS::start()
{
// reset the report ring and state machine
_reports->flush();
// schedule a cycle to start things
work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, 1);
}
void
FXAS21002C::stop()
{
hrt_cancel(&_gyro_call);
/* reset internal states */
/* discard unread data in the buffers */
_reports->flush();
}
void
QMC5883::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&QMC5883::cycle_trampoline, this, 1);
}
int
ACCELSIM::start()
{
//PX4_INFO("ACCELSIM::start");
/* make sure we are stopped first */
stop();
/* reset the report ring */
_accel_reports->flush();
_mag_reports->flush();
int ret2 = VirtDevObj::start();
if (ret2 != 0) {
PX4_ERR("ACCELSIM::start base class start failed");
}
return (ret2 != 0) ? -1 : 0;
}
void
CM8JL65::start()
{
PX4_INFO("driver started");
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&CM8JL65::cycle_trampoline, this, 1);
}
void
SF1XX::start()
{
/* reset the report ring and state machine */
_reports->flush();
/* set register to '0' */
measure();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&SF1XX::cycle_trampoline, this, USEC2TICK(_conversion_interval));
}
void
BAROSIM::start_cycle()
{
/* reset the report ring and state machine */
_collect_phase = false;
_measure_phase = 0;
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&BAROSIM::cycle_trampoline, this, 1);
}
void
BMA180::start()
{
/* make sure we are stopped first */
stop();
/* reset the report ring */
_reports->flush();
/* start polling at the specified rate */
hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&BMA180::measure_trampoline, this);
}
void
BAROSIM::start_cycle()
{
/* reset the report ring and state machine */
_collect_phase = false;
_measure_phase = 0;
_reports->flush();
/* schedule a cycle to start things */
setSampleInterval(1000);
start();
}
void
FXAS21002C::start()
{
/* make sure we are stopped first */
stop();
/* reset the report ring */
_reports->flush();
/* start polling at the specified rate */
hrt_call_every(&_gyro_call,
1000,
_call_interval - FXAS21002C_TIMER_REDUCTION,
(hrt_callout)&FXAS21002C::measure_trampoline, this);
}
ssize_t
FXAS21002C::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct gyro_report);
struct gyro_report *gbuf = reinterpret_cast<struct gyro_report *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (_call_interval > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the measurement code while we are doing this;
* we are careful to avoid racing with it.
*/
while (count--) {
if (_reports->get(gbuf)) {
ret += sizeof(*gbuf);
gbuf++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement */
_reports->flush();
measure();
/* measurement will have generated a report, copy it out */
if (_reports->get(gbuf)) {
ret = sizeof(*gbuf);
}
return ret;
}
ssize_t
ACCELSIM::mag_read(void *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct mag_report);
mag_report *mrb = reinterpret_cast<mag_report *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (_mag->m_sample_interval_usecs > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
*/
while (count--) {
if (_mag_reports->get(mrb)) {
ret += sizeof(*mrb);
mrb++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement */
_mag_reports->flush();
_mag->_measure();
/* measurement will have generated a report, copy it out */
if (_mag_reports->get(mrb)) {
ret = sizeof(*mrb);
}
return ret;
}
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;
}