mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Ported more simulated sensors to DriverFramework
Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois
parent
40c079efa8
commit
efd9995a10
@ -45,6 +45,10 @@
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "DevMgr.hpp"
|
||||
|
||||
using namespace DriverFramework;
|
||||
|
||||
namespace device
|
||||
{
|
||||
|
||||
@ -512,13 +516,21 @@ VDev *VDev::getDev(const char *path)
|
||||
void VDev::showDevices()
|
||||
{
|
||||
int i = 0;
|
||||
PX4_INFO("Devices:");
|
||||
PX4_INFO("PX4 Devices:");
|
||||
|
||||
for (; i < PX4_MAX_DEV; ++i) {
|
||||
if (devmap[i] && strncmp(devmap[i]->name, "/dev/", 5) == 0) {
|
||||
PX4_INFO(" %s", devmap[i]->name);
|
||||
}
|
||||
}
|
||||
PX4_INFO("DF Devices:");
|
||||
|
||||
std::string devname;
|
||||
for (unsigned int index=0; i == 0; ++i) {
|
||||
if (DevMgr::getNextDeviceName(index, devname) == 0) {
|
||||
PX4_INFO(" %s", devname.c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void VDev::showTopics()
|
||||
@ -566,4 +578,4 @@ const char *VDev::devList(unsigned int *next)
|
||||
return NULL;
|
||||
}
|
||||
|
||||
} // namespace device
|
||||
} // namespace device
|
||||
|
||||
@ -46,6 +46,7 @@
|
||||
#include "drv_sensor.h"
|
||||
#include "drv_orb_dev.h"
|
||||
|
||||
#ifdef __PX4_NUTTX
|
||||
/*
|
||||
* ioctl() definitions
|
||||
*/
|
||||
@ -65,6 +66,11 @@
|
||||
*/
|
||||
#define DEVIOCGDEVICEID _DEVICEIOC(2)
|
||||
|
||||
#else
|
||||
#include "DevObj.hpp"
|
||||
|
||||
#endif
|
||||
|
||||
#ifdef __PX4_POSIX
|
||||
|
||||
#ifndef SIOCDEVPRIVATE
|
||||
|
||||
@ -1 +1 @@
|
||||
Subproject commit c4be5e70b36c86868520472f93fd945d7f0d8986
|
||||
Subproject commit 0d91f67f2e8a1ae91a5c853d5cae8b30e67a635e
|
||||
@ -65,6 +65,10 @@
|
||||
#include "state_machine_helper.h"
|
||||
#include "commander_helper.h"
|
||||
#include "PreflightCheck.h"
|
||||
#ifndef __PX4_NUTTX
|
||||
#include "DevMgr.hpp"
|
||||
using namespace DriverFramework;
|
||||
#endif
|
||||
|
||||
// This array defines the arming state transitions. The rows are the new state, and the columns
|
||||
// are the current state. Using new state and current state you can index into the array which
|
||||
@ -463,30 +467,30 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
|
||||
|
||||
#else
|
||||
|
||||
const char *devname;
|
||||
unsigned int handle = 0;
|
||||
std::string devname;
|
||||
unsigned int index = 0;
|
||||
for(;;) {
|
||||
devname = px4_get_device_names(&handle);
|
||||
if (devname == NULL)
|
||||
if (DevMgr::getNextDeviceName(index, devname) < 0) {
|
||||
break;
|
||||
}
|
||||
|
||||
/* skip mavlink */
|
||||
if (!strcmp("/dev/mavlink", devname)) {
|
||||
if (!strcmp("/dev/mavlink", devname.c_str())) {
|
||||
continue;
|
||||
}
|
||||
|
||||
DevHandle h;
|
||||
DevMgr::getHandle(devname.c_str(), h);
|
||||
|
||||
int sensfd = px4_open(devname, 0);
|
||||
|
||||
if (sensfd < 0) {
|
||||
warn("failed opening device %s", devname);
|
||||
if (!h.isValid()) {
|
||||
warn("failed opening device %s", devname.c_str());
|
||||
continue;
|
||||
}
|
||||
|
||||
int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
|
||||
px4_close(sensfd);
|
||||
int block_ret = h.ioctl(DEVIOCSPUBBLOCK, (void *)1);
|
||||
DevMgr::releaseHandle(h);
|
||||
|
||||
printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
|
||||
printf("Disabling %s: %s\n", devname.c_str(), (block_ret == OK) ? "OK" : "ERROR");
|
||||
}
|
||||
|
||||
ret = TRANSITION_CHANGED;
|
||||
|
||||
@ -53,7 +53,6 @@
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#include <drivers/device/device.h>
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
@ -244,10 +243,8 @@ public:
|
||||
ACCELSIM_mag(ACCELSIM *parent);
|
||||
~ACCELSIM_mag();
|
||||
|
||||
virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen);
|
||||
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
|
||||
|
||||
virtual int init();
|
||||
virtual ssize_t devRead(void *buffer, size_t buflen);
|
||||
virtual int devIOCTL(unsigned long cmd, void *arg);
|
||||
|
||||
protected:
|
||||
friend class ACCELSIM;
|
||||
@ -913,7 +910,7 @@ ACCELSIM::_measure()
|
||||
/* notify anyone waiting for data */
|
||||
DevMgr::updateNotify(*this);
|
||||
|
||||
if (!(_pub_blocked)) {
|
||||
if (!(m_pub_blocked)) {
|
||||
/* publish it */
|
||||
|
||||
// The first call to measure() is from init() and _accel_topic is not
|
||||
@ -1000,9 +997,9 @@ ACCELSIM::mag_measure()
|
||||
_mag_reports->force(&mag_report);
|
||||
|
||||
/* notify anyone waiting for data */
|
||||
DevMgr::updateNotify(*this)
|
||||
DevMgr::updateNotify(*this);
|
||||
|
||||
if (!(_pub_blocked)) {
|
||||
if (!(m_pub_blocked)) {
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
|
||||
}
|
||||
@ -1014,7 +1011,7 @@ ACCELSIM::mag_measure()
|
||||
}
|
||||
|
||||
ACCELSIM_mag::ACCELSIM_mag(ACCELSIM *parent) :
|
||||
VirtDev("ACCELSIM_mag", ACCELSIM_DEVICE_PATH_MAG, MAG_BASE_DEVICE_PATH, 10000),
|
||||
VirtDevObj("ACCELSIM_mag", ACCELSIM_DEVICE_PATH_MAG, MAG_BASE_DEVICE_PATH, 10000),
|
||||
_parent(parent),
|
||||
_mag_topic(nullptr),
|
||||
_mag_orb_class_instance(-1),
|
||||
@ -1027,7 +1024,7 @@ ACCELSIM_mag::~ACCELSIM_mag()
|
||||
}
|
||||
|
||||
ssize_t
|
||||
ACCELSIM_mag::devRead(char *buffer, size_t buflen)
|
||||
ACCELSIM_mag::devRead(void *buffer, size_t buflen)
|
||||
{
|
||||
return _parent->mag_read(buffer, buflen);
|
||||
}
|
||||
@ -1037,7 +1034,7 @@ ACCELSIM_mag::devIOCTL(unsigned long cmd, void *arg)
|
||||
{
|
||||
switch (cmd) {
|
||||
case DEVIOCGDEVICEID:
|
||||
return (int)VirtDevObj::ioctl(cmd, arg);
|
||||
return (int)VirtDevObj::devIOCTL(cmd, arg);
|
||||
break;
|
||||
|
||||
default:
|
||||
@ -1077,6 +1074,9 @@ start(enum Rotation rotation)
|
||||
return 0;
|
||||
}
|
||||
|
||||
DevHandle h;
|
||||
DevHandle h_mag;
|
||||
|
||||
/* create the driver */
|
||||
g_dev = new ACCELSIM(ACCELSIM_DEVICE_PATH_ACCEL, rotation);
|
||||
|
||||
@ -1091,7 +1091,6 @@ start(enum Rotation rotation)
|
||||
}
|
||||
|
||||
/* set the poll rate to default, starts automatic data collection */
|
||||
DevHandle h;
|
||||
DevMgr::getHandle(ACCELSIM_DEVICE_PATH_ACCEL, h);
|
||||
|
||||
if (!h.isValid()) {
|
||||
@ -1099,18 +1098,17 @@ start(enum Rotation rotation)
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
|
||||
if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) {
|
||||
PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL);
|
||||
DevMgr::releaseHandle(h);
|
||||
goto fail;
|
||||
}
|
||||
|
||||
DevHandle h_mag;
|
||||
DevMgr::getHandle(ACCELSIM_DEVICE_PATH_MAG, h_mag);
|
||||
|
||||
/* don't fail if mag dev cannot be opened */
|
||||
if (!h_mag.isValid()) {
|
||||
if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
|
||||
if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) {
|
||||
PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL);
|
||||
}
|
||||
|
||||
|
||||
@ -48,7 +48,6 @@
|
||||
#include <semaphore.h>
|
||||
#include <string.h>
|
||||
#include <fcntl.h>
|
||||
#include <poll.h>
|
||||
#include <errno.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
@ -67,6 +66,7 @@
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#include "barosim.h"
|
||||
#include "VirtDevObj.hpp"
|
||||
|
||||
enum BAROSIM_BUS {
|
||||
BAROSIM_BUS_SIM_EXTERNAL
|
||||
@ -86,16 +86,16 @@ enum BAROSIM_BUS {
|
||||
#define BAROSIM_CONVERSION_INTERVAL 10000 /* microseconds */
|
||||
#define BAROSIM_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */
|
||||
|
||||
class BAROSIM : public device::VDev
|
||||
class BAROSIM : public VirtDevObj
|
||||
{
|
||||
public:
|
||||
BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char *path);
|
||||
BAROSIM(DevObj *interface, barosim::prom_u &prom_buf, const char *path);
|
||||
~BAROSIM();
|
||||
|
||||
virtual int init();
|
||||
|
||||
virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen);
|
||||
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
|
||||
virtual ssize_t devRead(void *buffer, size_t buflen);
|
||||
virtual int devIOCTL(unsigned long cmd, void *arg);
|
||||
|
||||
/**
|
||||
* Diagnostics - print some basic information about the driver.
|
||||
@ -103,7 +103,7 @@ public:
|
||||
void print_info();
|
||||
|
||||
protected:
|
||||
Device *_interface;
|
||||
DevObj *_interface;
|
||||
|
||||
barosim::prom_s _prom;
|
||||
|
||||
@ -182,7 +182,10 @@ protected:
|
||||
*
|
||||
* @return OK if the measurement command was successful.
|
||||
*/
|
||||
virtual int measure();
|
||||
int measure();
|
||||
|
||||
// Unused
|
||||
virtual void _measure() {}
|
||||
|
||||
/**
|
||||
* Collect the result of the most recent measurement.
|
||||
@ -195,8 +198,8 @@ protected:
|
||||
*/
|
||||
extern "C" __EXPORT int barosim_main(int argc, char *argv[]);
|
||||
|
||||
BAROSIM::BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char *path) :
|
||||
VDev("BAROSIM", path),
|
||||
BAROSIM::BAROSIM(DevObj *interface, barosim::prom_u &prom_buf, const char *path) :
|
||||
VirtDevObj("BAROSIM", path, BARO_BASE_DEVICE_PATH, 0),
|
||||
_interface(interface),
|
||||
_prom(prom_buf.s),
|
||||
_measure_ticks(0),
|
||||
@ -224,10 +227,6 @@ BAROSIM::~BAROSIM()
|
||||
/* make sure we are truly inactive */
|
||||
stop_cycle();
|
||||
|
||||
if (_class_instance != -1) {
|
||||
unregister_class_devname(get_devname(), _class_instance);
|
||||
}
|
||||
|
||||
/* free any existing reports */
|
||||
if (_reports != nullptr) {
|
||||
delete _reports;
|
||||
@ -246,12 +245,12 @@ int
|
||||
BAROSIM::init()
|
||||
{
|
||||
int ret;
|
||||
DEVICE_DEBUG("BAROSIM::init");
|
||||
//PX4_DEBUG("BAROSIM::init");
|
||||
|
||||
ret = VDev::init();
|
||||
ret = VirtDevObj::init();
|
||||
|
||||
if (ret != OK) {
|
||||
DEVICE_DEBUG("VDev init failed");
|
||||
PX4_ERR("VirtDevObj init failed");
|
||||
goto out;
|
||||
}
|
||||
|
||||
@ -259,14 +258,11 @@ BAROSIM::init()
|
||||
_reports = new ringbuffer::RingBuffer(2, sizeof(baro_report));
|
||||
|
||||
if (_reports == nullptr) {
|
||||
DEVICE_DEBUG("can't get memory for reports");
|
||||
PX4_ERR("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;
|
||||
@ -325,7 +321,7 @@ out:
|
||||
}
|
||||
|
||||
ssize_t
|
||||
BAROSIM::read(device::file_t *filp, char *buffer, size_t buflen)
|
||||
BAROSIM::devRead(void *buffer, size_t buflen)
|
||||
{
|
||||
unsigned count = buflen / sizeof(struct baro_report);
|
||||
struct baro_report *brp = reinterpret_cast<struct baro_report *>(buffer);
|
||||
@ -397,12 +393,14 @@ BAROSIM::read(device::file_t *filp, char *buffer, size_t buflen)
|
||||
}
|
||||
|
||||
int
|
||||
BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
||||
BAROSIM::devIOCTL(unsigned long cmd, void *arg)
|
||||
{
|
||||
unsigned long ul_arg = (unsigned long)arg;
|
||||
|
||||
switch (cmd) {
|
||||
|
||||
case SENSORIOCSPOLLRATE:
|
||||
switch (arg) {
|
||||
switch (ul_arg) {
|
||||
|
||||
/* switching to manual polling */
|
||||
case SENSOR_POLLRATE_MANUAL:
|
||||
@ -440,7 +438,7 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
||||
bool want_start = (_measure_ticks == 0);
|
||||
|
||||
/* convert hz to tick interval via microseconds */
|
||||
unsigned long ticks = USEC2TICK(1000000 / arg);
|
||||
unsigned long ticks = USEC2TICK(1000000 / ul_arg);
|
||||
|
||||
/* check against maximum rate */
|
||||
if (ticks < USEC2TICK(BAROSIM_CONVERSION_INTERVAL)) {
|
||||
@ -468,11 +466,11 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
||||
|
||||
case SENSORIOCSQUEUEDEPTH: {
|
||||
/* lower bound is mandatory, upper bound is a sanity check */
|
||||
if ((arg < 1) || (arg > 100)) {
|
||||
if ((ul_arg < 1) || (ul_arg > 100)) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
if (!_reports->resize(arg)) {
|
||||
if (!_reports->resize(ul_arg)) {
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
@ -492,11 +490,11 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
||||
case BAROIOCSMSLPRESSURE:
|
||||
|
||||
/* range-check for sanity */
|
||||
if ((arg < 80000) || (arg > 120000)) {
|
||||
if ((ul_arg < 80000) || (ul_arg > 120000)) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
_msl_pressure = arg;
|
||||
_msl_pressure = ul_arg;
|
||||
return OK;
|
||||
|
||||
case BAROIOCGMSLPRESSURE:
|
||||
@ -508,7 +506,7 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
||||
|
||||
/* give it to the bus-specific superclass */
|
||||
// return bus_ioctl(filp, cmd, arg);
|
||||
return VDev::ioctl(filp, cmd, arg);
|
||||
return VirtDevObj::devIOCTL(cmd, arg);
|
||||
}
|
||||
|
||||
void
|
||||
@ -542,7 +540,7 @@ void
|
||||
BAROSIM::cycle()
|
||||
{
|
||||
int ret;
|
||||
unsigned dummy;
|
||||
void *dummy = nullptr;
|
||||
|
||||
/* collection phase? */
|
||||
if (_collect_phase) {
|
||||
@ -552,7 +550,7 @@ BAROSIM::cycle()
|
||||
|
||||
if (ret != OK) {
|
||||
/* issue a reset command to the sensor */
|
||||
_interface->dev_ioctl(IOCTL_RESET, dummy);
|
||||
_interface->devIOCTL(IOCTL_RESET, (void *)dummy);
|
||||
/* reset the collection state machine and try again */
|
||||
start_cycle();
|
||||
return;
|
||||
@ -586,7 +584,7 @@ BAROSIM::cycle()
|
||||
if (ret != OK) {
|
||||
//DEVICE_LOG("measure error %d", ret);
|
||||
/* issue a reset command to the sensor */
|
||||
_interface->dev_ioctl(IOCTL_RESET, dummy);
|
||||
_interface->devIOCTL(IOCTL_RESET, dummy);
|
||||
/* reset the collection state machine and try again */
|
||||
start_cycle();
|
||||
return;
|
||||
@ -613,12 +611,12 @@ BAROSIM::measure()
|
||||
/*
|
||||
* In phase zero, request temperature; in other phases, request pressure.
|
||||
*/
|
||||
unsigned addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1;
|
||||
unsigned long addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1;
|
||||
|
||||
/*
|
||||
* Send the command to begin measuring.
|
||||
*/
|
||||
ret = _interface->dev_ioctl(IOCTL_MEASURE, addr);
|
||||
ret = _interface->devIOCTL(IOCTL_MEASURE, (void *)addr);
|
||||
|
||||
if (OK != ret) {
|
||||
perf_count(_comms_errors);
|
||||
@ -650,7 +648,7 @@ BAROSIM::collect()
|
||||
report.error_count = perf_event_count(_comms_errors);
|
||||
|
||||
/* read the most recent measurement - read offset/size are hardcoded in the interface */
|
||||
ret = _interface->dev_read(0, (void *)&baro_report, sizeof(baro_report));
|
||||
ret = _interface->devRead((void *)&baro_report, sizeof(baro_report));
|
||||
|
||||
if (ret < 0) {
|
||||
perf_count(_comms_errors);
|
||||
@ -672,7 +670,7 @@ BAROSIM::collect()
|
||||
report.timestamp = hrt_absolute_time();
|
||||
|
||||
/* publish it */
|
||||
if (!(_pub_blocked)) {
|
||||
if (!(m_pub_blocked)) {
|
||||
if (_baro_topic != nullptr) {
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
|
||||
@ -687,7 +685,7 @@ BAROSIM::collect()
|
||||
}
|
||||
|
||||
/* notify anyone waiting for data */
|
||||
poll_notify(POLLIN);
|
||||
DevMgr::updateNotify(*this);
|
||||
}
|
||||
|
||||
/* update the measurement state machine */
|
||||
@ -810,7 +808,7 @@ start_bus(struct barosim_bus_option &bus)
|
||||
}
|
||||
|
||||
prom_u prom_buf;
|
||||
device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum);
|
||||
DevObj *interface = bus.interface_constructor(prom_buf, bus.busnum);
|
||||
|
||||
if (interface->init() != OK) {
|
||||
delete interface;
|
||||
@ -827,21 +825,22 @@ start_bus(struct barosim_bus_option &bus)
|
||||
return false;
|
||||
}
|
||||
|
||||
int fd = px4_open(bus.devpath, O_RDONLY);
|
||||
DevHandle h;
|
||||
DevMgr::getHandle(bus.devpath, h);
|
||||
|
||||
/* set the poll rate to default, starts automatic data collection */
|
||||
if (fd == -1) {
|
||||
if (!h.isValid()) {
|
||||
PX4_ERR("can't open baro device");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
|
||||
px4_close(fd);
|
||||
if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) {
|
||||
DevMgr::releaseHandle(h);
|
||||
PX4_ERR("failed setting default poll rate");
|
||||
return false;
|
||||
}
|
||||
|
||||
px4_close(fd);
|
||||
DevMgr::releaseHandle(h);
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -882,17 +881,16 @@ test()
|
||||
ssize_t sz;
|
||||
int ret;
|
||||
|
||||
int fd;
|
||||
DevHandle h;
|
||||
DevMgr::getHandle(bus.devpath, h);
|
||||
|
||||
fd = px4_open(bus.devpath, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
PX4_ERR("open failed (try 'barosim start' if the driver is not running)");
|
||||
if (!h.isValid()) {
|
||||
PX4_ERR("getHandle failed (try 'barosim start' if the driver is not running)");
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* do a simple demand read */
|
||||
sz = px4_read(fd, &report, sizeof(report));
|
||||
sz = h.read(&report, sizeof(report));
|
||||
|
||||
if (sz != sizeof(report)) {
|
||||
PX4_ERR("immediate read failed");
|
||||
@ -906,38 +904,37 @@ test()
|
||||
PX4_INFO("time: %lld", (long long)report.timestamp);
|
||||
|
||||
/* set the queue depth to 10 */
|
||||
if (OK != px4_ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) {
|
||||
if (OK != h.ioctl(SENSORIOCSQUEUEDEPTH, (void *)10UL)) {
|
||||
PX4_ERR("failed to set queue depth");
|
||||
px4_close(fd);
|
||||
DevMgr::releaseHandle(h);
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* start the sensor polling at 2Hz */
|
||||
if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
|
||||
if (OK != h.ioctl(SENSORIOCSPOLLRATE, (void *)2UL)) {
|
||||
PX4_ERR("failed to set 2Hz poll rate");
|
||||
px4_close(fd);
|
||||
DevMgr::releaseHandle(h);
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* read the sensor 5x and report each value */
|
||||
for (unsigned i = 0; i < 5; i++) {
|
||||
px4_pollfd_struct_t fds;
|
||||
UpdateList in_set, out_set;
|
||||
in_set.push_back(&h);
|
||||
|
||||
/* wait for data to be ready */
|
||||
fds.fd = fd;
|
||||
fds.events = POLLIN;
|
||||
ret = px4_poll(&fds, 1, 2000);
|
||||
ret = DevMgr::waitForUpdate(in_set, out_set, 1000);
|
||||
|
||||
if (ret != 1) {
|
||||
PX4_WARN("timed out waiting for sensor data");
|
||||
}
|
||||
|
||||
/* now go get it */
|
||||
sz = px4_read(fd, &report, sizeof(report));
|
||||
sz = h.read(&report, sizeof(report));
|
||||
|
||||
if (sz != sizeof(report)) {
|
||||
PX4_ERR("periodic read failed");
|
||||
px4_close(fd);
|
||||
DevMgr::releaseHandle(h);
|
||||
return 1;
|
||||
}
|
||||
|
||||
@ -948,7 +945,7 @@ test()
|
||||
PX4_INFO("time: %lld", (long long)report.timestamp);
|
||||
}
|
||||
|
||||
px4_close(fd);
|
||||
DevMgr::releaseHandle(h);
|
||||
PX4_INFO("PASS");
|
||||
return 0;
|
||||
}
|
||||
@ -960,21 +957,21 @@ int
|
||||
reset()
|
||||
{
|
||||
struct barosim_bus_option &bus = bus_options[0];
|
||||
int fd;
|
||||
|
||||
fd = px4_open(bus.devpath, O_RDONLY);
|
||||
DevHandle h;
|
||||
DevMgr::getHandle(bus.devpath, h);
|
||||
|
||||
if (fd < 0) {
|
||||
if (!h.isValid()) {
|
||||
PX4_ERR("failed ");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) {
|
||||
if (h.ioctl(SENSORIOCRESET, 0) < 0) {
|
||||
PX4_ERR("driver reset failed");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
|
||||
if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) {
|
||||
PX4_ERR("driver poll restart failed");
|
||||
return 1;
|
||||
}
|
||||
@ -1008,20 +1005,19 @@ calibrate(unsigned altitude)
|
||||
{
|
||||
struct barosim_bus_option &bus = bus_options[0];
|
||||
struct baro_report report;
|
||||
float pressure;
|
||||
float p1;
|
||||
float pressure;
|
||||
float p1;
|
||||
|
||||
int fd;
|
||||
DevHandle h;
|
||||
DevMgr::getHandle(bus.devpath, h);
|
||||
|
||||
fd = px4_open(bus.devpath, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
if (!h.isValid()) {
|
||||
PX4_ERR("open failed (try 'barosim start' if the driver is not running)");
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* start the sensor polling at max */
|
||||
if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) {
|
||||
if (OK != h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_MAX)) {
|
||||
PX4_ERR("failed to set poll rate");
|
||||
return 1;
|
||||
}
|
||||
@ -1030,14 +1026,13 @@ calibrate(unsigned altitude)
|
||||
pressure = 0.0f;
|
||||
|
||||
for (unsigned i = 0; i < 20; i++) {
|
||||
px4_pollfd_struct_t fds;
|
||||
int ret;
|
||||
ssize_t sz;
|
||||
UpdateList in_set, out_set;
|
||||
in_set.push_back(&h);
|
||||
|
||||
/* wait for data to be ready */
|
||||
fds.fd = fd;
|
||||
fds.events = POLLIN;
|
||||
ret = px4_poll(&fds, 1, 1000);
|
||||
ret = DevMgr::waitForUpdate(in_set, out_set, 1000);
|
||||
|
||||
if (ret != 1) {
|
||||
PX4_ERR("timed out waiting for sensor data");
|
||||
@ -1045,7 +1040,7 @@ calibrate(unsigned altitude)
|
||||
}
|
||||
|
||||
/* now go get it */
|
||||
sz = px4_read(fd, &report, sizeof(report));
|
||||
sz = h.read(&report, sizeof(report));
|
||||
|
||||
if (sz != sizeof(report)) {
|
||||
PX4_ERR("sensor read failed");
|
||||
@ -1073,12 +1068,12 @@ calibrate(unsigned altitude)
|
||||
/* save as integer Pa */
|
||||
p1 *= 1000.0f;
|
||||
|
||||
if (px4_ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) {
|
||||
if (h.ioctl(BAROIOCSMSLPRESSURE, (void *)((unsigned long)(p1))) != OK) {
|
||||
PX4_WARN("BAROIOCSMSLPRESSURE");
|
||||
return 1;
|
||||
}
|
||||
|
||||
px4_close(fd);
|
||||
DevMgr::releaseHandle(h);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
@ -54,7 +54,7 @@
|
||||
#include "barosim.h"
|
||||
#include "board_config.h"
|
||||
|
||||
device::Device *BAROSIM_sim_interface(barosim::prom_u &prom_buf);
|
||||
DevObj *BAROSIM_sim_interface(barosim::prom_u &prom_buf);
|
||||
|
||||
class BARO_SIM : public device::SIM
|
||||
{
|
||||
@ -94,10 +94,10 @@ private:
|
||||
|
||||
};
|
||||
|
||||
device::Device *
|
||||
DevObj *
|
||||
BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum)
|
||||
{
|
||||
return new BARO_SIM(busnum, prom_buf);
|
||||
return reinterpret_cast<DevObj *>(new BARO_SIM(busnum, prom_buf));
|
||||
}
|
||||
|
||||
BARO_SIM::BARO_SIM(uint8_t bus, barosim::prom_u &prom) :
|
||||
|
||||
@ -36,13 +36,14 @@
|
||||
*
|
||||
* Shared defines for the ms5611 driver.
|
||||
*/
|
||||
#include "DevObj.hpp"
|
||||
|
||||
#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */
|
||||
#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */
|
||||
#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start pressure conversion */
|
||||
#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start temperature conversion */
|
||||
#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */
|
||||
#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */
|
||||
#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */
|
||||
#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */
|
||||
#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */
|
||||
#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */
|
||||
|
||||
/* interface ioctls */
|
||||
#define IOCTL_RESET 2
|
||||
@ -79,6 +80,8 @@ extern bool crc4(uint16_t *n_prom);
|
||||
|
||||
} /* namespace */
|
||||
|
||||
using namespace DriverFramework;
|
||||
|
||||
/* interface factories */
|
||||
extern device::Device *BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum);
|
||||
typedef device::Device *(*BAROSIM_constructor)(barosim::prom_u &prom_buf, uint8_t busnum);
|
||||
extern DevObj *BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum);
|
||||
typedef DevObj *(*BAROSIM_constructor)(barosim::prom_u &prom_buf, uint8_t busnum);
|
||||
|
||||
@ -426,7 +426,7 @@ GYROSIM::init()
|
||||
_gyro_scale.z_scale = 1.0f;
|
||||
|
||||
|
||||
/* do VDev init for the gyro device node, keep it optional */
|
||||
/* do init for the gyro device node, keep it optional */
|
||||
ret = _gyro->init();
|
||||
|
||||
/* if probe/setup failed, bail now */
|
||||
|
||||
@ -109,17 +109,18 @@
|
||||
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#include "VirtDevObj.hpp"
|
||||
|
||||
class ToneAlarm : public device::VDev
|
||||
using namespace DriverFramework;
|
||||
|
||||
class ToneAlarm : public VirtDevObj
|
||||
{
|
||||
public:
|
||||
ToneAlarm();
|
||||
~ToneAlarm();
|
||||
|
||||
virtual int init();
|
||||
|
||||
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
|
||||
virtual ssize_t write(device::file_t *filp, const char *buffer, size_t len);
|
||||
virtual int devIOCTL(unsigned long cmd, void *arg);
|
||||
virtual ssize_t devWrite(const void *buffer, size_t len);
|
||||
inline const char *name(int tune)
|
||||
{
|
||||
return _tune_names[tune];
|
||||
@ -196,6 +197,8 @@ private:
|
||||
//
|
||||
static void next_trampoline(void *arg);
|
||||
|
||||
// Unused
|
||||
virtual void _measure() {}
|
||||
};
|
||||
|
||||
// semitone offsets from C for the characters 'A'-'G'
|
||||
@ -208,7 +211,7 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]);
|
||||
|
||||
|
||||
ToneAlarm::ToneAlarm() :
|
||||
VDev("tone_alarm", TONEALARM0_DEVICE_PATH),
|
||||
VirtDevObj("tone_alarm", TONEALARM0_DEVICE_PATH, nullptr, 0),
|
||||
_default_tune_number(0),
|
||||
_user_tune(nullptr),
|
||||
_tune(nullptr),
|
||||
@ -251,21 +254,6 @@ ToneAlarm::~ToneAlarm()
|
||||
{
|
||||
}
|
||||
|
||||
int
|
||||
ToneAlarm::init()
|
||||
{
|
||||
int ret;
|
||||
|
||||
ret = VDev::init();
|
||||
|
||||
if (ret != OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
DEVICE_DEBUG("ready");
|
||||
return OK;
|
||||
}
|
||||
|
||||
unsigned
|
||||
ToneAlarm::note_to_divisor(unsigned note)
|
||||
{
|
||||
@ -653,21 +641,21 @@ ToneAlarm::next_trampoline(void *arg)
|
||||
|
||||
|
||||
int
|
||||
ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
||||
ToneAlarm::devIOCTL(unsigned long cmd, void *arg)
|
||||
{
|
||||
int result = OK;
|
||||
|
||||
DEVICE_DEBUG("ioctl %i %lu", cmd, arg);
|
||||
unsigned long ul_arg = (unsigned long)arg;
|
||||
|
||||
// irqstate_t flags = irqsave();
|
||||
PX4_DEBUG("ToneAlarm::devIOCTL %i %lu", cmd, ul_arg);
|
||||
|
||||
/* decide whether to increase the alarm level to cmd or leave it alone */
|
||||
switch (cmd) {
|
||||
case TONE_SET_ALARM:
|
||||
PX4_INFO("TONE_SET_ALARM %lu", arg);
|
||||
PX4_INFO("TONE_SET_ALARM %lu", ul_arg);
|
||||
|
||||
if (arg < TONE_NUMBER_OF_TUNES) {
|
||||
if (arg == TONE_STOP_TUNE) {
|
||||
if (ul_arg < TONE_NUMBER_OF_TUNES) {
|
||||
if (ul_arg == TONE_STOP_TUNE) {
|
||||
// stop the tune
|
||||
_tune = nullptr;
|
||||
_next = nullptr;
|
||||
@ -676,10 +664,10 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
||||
|
||||
} else {
|
||||
/* always interrupt alarms, unless they are repeating and already playing */
|
||||
if (!(_repeat && _default_tune_number == arg)) {
|
||||
if (!(_repeat && _default_tune_number == ul_arg)) {
|
||||
/* play the selected tune */
|
||||
_default_tune_number = arg;
|
||||
start_tune(_default_tunes[arg]);
|
||||
_default_tune_number = ul_arg;
|
||||
start_tune(_default_tunes[ul_arg]);
|
||||
}
|
||||
}
|
||||
|
||||
@ -694,18 +682,16 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
||||
break;
|
||||
}
|
||||
|
||||
// irqrestore(flags);
|
||||
|
||||
/* give it to the superclass if we didn't like it */
|
||||
if (result == -ENOTTY) {
|
||||
result = VDev::ioctl(filp, cmd, arg);
|
||||
result = VirtDevObj::devIOCTL(cmd, arg);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
ssize_t
|
||||
ToneAlarm::write(device::file_t *filp, const char *buffer, size_t len)
|
||||
ToneAlarm::devWrite(const void *buffer, size_t len)
|
||||
{
|
||||
// sanity-check the buffer for length and nul-termination
|
||||
if (len > _tune_max) {
|
||||
@ -726,13 +712,15 @@ ToneAlarm::write(device::file_t *filp, const char *buffer, size_t len)
|
||||
_user_tune = nullptr;
|
||||
}
|
||||
|
||||
const char *buf = reinterpret_cast<const char *>(buffer);
|
||||
|
||||
// if the new tune is empty, we're done
|
||||
if (buffer[0] == '\0') {
|
||||
if (buf[0] == '\0') {
|
||||
return OK;
|
||||
}
|
||||
|
||||
// allocate a copy of the new tune
|
||||
_user_tune = strndup(buffer, len);
|
||||
_user_tune = strndup(buf, len);
|
||||
|
||||
if (_user_tune == nullptr) {
|
||||
return -ENOMEM;
|
||||
@ -778,17 +766,18 @@ play_tune(unsigned tune)
|
||||
int
|
||||
play_string(const char *str, bool free_buffer)
|
||||
{
|
||||
int fd, ret;
|
||||
int ret;
|
||||
|
||||
fd = px4_open(TONEALARM0_DEVICE_PATH, O_WRONLY);
|
||||
DevHandle h;
|
||||
DevMgr::getHandle(TONEALARM0_DEVICE_PATH, h);
|
||||
|
||||
if (fd < 0) {
|
||||
PX4_WARN("Error: failed to open %s", TONEALARM0_DEVICE_PATH);
|
||||
if (!h.isValid()) {
|
||||
PX4_WARN("Error: failed to get handle to %s", TONEALARM0_DEVICE_PATH);
|
||||
return 1;
|
||||
}
|
||||
|
||||
ret = px4_write(fd, str, strlen(str) + 1);
|
||||
px4_close(fd);
|
||||
ret = h.write(str, strlen(str) + 1);
|
||||
DevMgr::releaseHandle(h);
|
||||
|
||||
if (free_buffer) {
|
||||
free((void *)str);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user