/**************************************************************************** * * Copyright (c) 2014-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ /** * @file accelsim.cpp * Driver for a simulated accelerometer / magnetometer. */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #define ACCELSIM_DEVICE_PATH_ACCEL "/dev/sim_accel" #define ACCELSIM_DEVICE_PATH_MAG "/dev/sim_mag" #define ADDR_WHO_AM_I 0x0F #define ACCELSIM_ACCEL_DEFAULT_RATE 250 #define ACCELSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) #define ACC_READ (1<<5) #define MAG_READ (1<<6) extern "C" { __EXPORT int accelsim_main(int argc, char *argv[]); } using namespace DriverFramework; class ACCELSIM_mag; class ACCELSIM : public VirtDevObj { public: ACCELSIM(const char *path, enum Rotation rotation); virtual ~ACCELSIM(); virtual int init(); virtual ssize_t devRead(void *buffer, size_t buflen); virtual int devIOCTL(unsigned long cmd, unsigned long arg); /** * dump register values */ void print_registers(); protected: friend class ACCELSIM_mag; ssize_t mag_read(void *buffer, size_t buflen); int mag_ioctl(unsigned long cmd, unsigned long arg); int transfer(uint8_t *send, uint8_t *recv, unsigned len); private: ACCELSIM_mag *_mag; ringbuffer::RingBuffer *_accel_reports; ringbuffer::RingBuffer *_mag_reports; struct accel_calibration_s _accel_scale; unsigned _accel_range_m_s2; float _accel_range_scale; unsigned _accel_samplerate; unsigned _accel_onchip_filter_bandwith; struct mag_calibration_s _mag_scale; unsigned _mag_range_ga; float _mag_range_scale; unsigned _mag_samplerate; orb_advert_t _accel_topic; int _accel_orb_class_instance; int _accel_class_instance; unsigned _accel_read; unsigned _mag_read; perf_counter_t _accel_sample_perf; perf_counter_t _mag_sample_perf; perf_counter_t _accel_reschedules; perf_counter_t _bad_registers; perf_counter_t _bad_values; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; math::LowPassFilter2p _accel_filter_z; enum Rotation _rotation; // values used to float _last_accel[3]; uint8_t _constant_accel_count; // last temperature value float _last_temperature; /** * Override Start automatic measurement. */ virtual int start(); /** * Override Stop automatic measurement. */ virtual int stop(); /** * Fetch accel measurements from the sensor and update the report ring. */ virtual void _measure(); /** * Fetch mag measurements from the sensor and update the report ring. */ void mag_measure(); /** * Set the ACCELSIM accel measurement range. * * @param max_g The measurement range of the accel is in g (9.81m/s^2) * Zero selects the maximum supported range. * @return OK if the value can be supported, -ERANGE otherwise. */ int accel_set_range(unsigned max_g); /** * Set the ACCELSIM mag measurement range. * * @param max_ga The measurement range of the mag is in Ga * Zero selects the maximum supported range. * @return OK if the value can be supported, -ERANGE otherwise. */ int mag_set_range(unsigned max_g); /** * Set the driver lowpass filter bandwidth. * * @param bandwidth The anti-alias filter bandwidth in Hz * Zero selects the highest bandwidth * @return OK if the value can be supported, -ERANGE otherwise. */ int accel_set_driver_lowpass_filter(float samplerate, float bandwidth); /* this class cannot be copied */ ACCELSIM(const ACCELSIM &); ACCELSIM operator=(const ACCELSIM &); }; /** * Helper class implementing the mag driver node. */ class ACCELSIM_mag : public VirtDevObj { public: ACCELSIM_mag(ACCELSIM *parent); ~ACCELSIM_mag() = default; virtual ssize_t devRead(void *buffer, size_t buflen); virtual int devIOCTL(unsigned long cmd, unsigned long arg); virtual int start(); virtual int stop(); protected: friend class ACCELSIM; private: ACCELSIM *_parent; orb_advert_t _mag_topic; int _mag_orb_class_instance; int _mag_class_instance; virtual void _measure(); /* this class does not allow copying due to ptr data members */ ACCELSIM_mag(const ACCELSIM_mag &) = delete; ACCELSIM_mag operator=(const ACCELSIM_mag &) = delete; }; ACCELSIM::ACCELSIM(const char *path, enum Rotation rotation) : VirtDevObj("ACCELSIM", path, ACCEL_BASE_DEVICE_PATH, 1e6 / 400), _mag(new ACCELSIM_mag(this)), _accel_reports(nullptr), _mag_reports(nullptr), _accel_scale{}, _accel_range_m_s2(0.0f), _accel_range_scale(0.0f), _accel_samplerate(0), _accel_onchip_filter_bandwith(0), _mag_scale{}, _mag_range_ga(0.0f), _mag_range_scale(0.0f), _mag_samplerate(0), _accel_topic(nullptr), _accel_orb_class_instance(-1), _accel_class_instance(-1), _accel_read(0), _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "sim_accel_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "sim_mag_read")), _accel_reschedules(perf_alloc(PC_COUNT, "sim_accel_resched")), _bad_registers(perf_alloc(PC_COUNT, "sim_bad_registers")), _bad_values(perf_alloc(PC_COUNT, "sim_bad_values")), _accel_filter_x(ACCELSIM_ACCEL_DEFAULT_RATE, ACCELSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(ACCELSIM_ACCEL_DEFAULT_RATE, ACCELSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(ACCELSIM_ACCEL_DEFAULT_RATE, ACCELSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), _constant_accel_count(0), _last_temperature(0) { m_id.dev_id_s.bus = 1; m_id.dev_id_s.devtype = DRV_ACC_DEVTYPE_ACCELSIM; /* Prime _mag with parents devid. */ _mag->m_id.dev_id = m_id.dev_id; _mag->m_id.dev_id_s.devtype = DRV_MAG_DEVTYPE_ACCELSIM; // default scale factors _accel_scale.x_offset = 0.0f; _accel_scale.x_scale = 1.0f; _accel_scale.y_offset = 0.0f; _accel_scale.y_scale = 1.0f; _accel_scale.z_offset = 0.0f; _accel_scale.z_scale = 1.0f; _mag_scale.x_offset = 0.0f; _mag_scale.x_scale = 1.0f; _mag_scale.y_offset = 0.0f; _mag_scale.y_scale = 1.0f; _mag_scale.z_offset = 0.0f; _mag_scale.z_scale = 1.0f; } ACCELSIM::~ACCELSIM() { /* make sure we are truly inactive */ _mag->stop(); stop(); /* free any existing reports */ if (_accel_reports != nullptr) { delete _accel_reports; } if (_mag_reports != nullptr) { delete _mag_reports; } delete _mag; /* delete the perf counter */ perf_free(_accel_sample_perf); perf_free(_mag_sample_perf); perf_free(_bad_registers); perf_free(_bad_values); perf_free(_accel_reschedules); } int ACCELSIM::init() { int ret = -1; struct mag_report mrp = {}; struct accel_report arp = {}; /* do SIM init first */ if (VirtDevObj::init() != 0) { PX4_WARN("SIM init failed"); goto out; } /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); if (_accel_reports == nullptr) { goto out; } _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) { goto out; } /* do init for the mag device node */ ret = _mag->init(); if (ret != OK) { PX4_WARN("MAG init failed"); goto out; } /* fill report structures */ _measure(); /* advertise sensor topic, measure manually to initialize valid report */ _mag_reports->get(&mrp); /* measurement will have generated a report, publish */ _mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); if (_mag->_mag_topic == nullptr) { PX4_WARN("ADVERT ERR"); } /* advertise sensor topic, measure manually to initialize valid report */ _accel_reports->get(&arp); /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, &_accel_orb_class_instance, ORB_PRIO_DEFAULT); if (_accel_topic == nullptr) { PX4_WARN("ADVERT ERR"); } out: return ret; } int ACCELSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) { uint8_t cmd = send[0]; if (cmd & DIR_READ) { // Get data from the simulator Simulator *sim = Simulator::getInstance(); if (sim == nullptr) { return ENODEV; } // FIXME - not sure what interrupt status should be recv[1] = 0; // skip cmd and status bytes if (cmd & ACC_READ) { sim->getRawAccelReport(&recv[2], len - 2); } else if (cmd & MAG_READ) { sim->getMagReport(&recv[2], len - 2); } } return PX4_OK; } ssize_t ACCELSIM::devRead(void *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct accel_report); accel_report *arb = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ if (count < 1) { return -ENOSPC; } /* if automatic measurement is enabled */ if (m_sample_interval_usecs > 0) { /* * While there is space in the caller's buffer, and reports, copy them. */ while (count--) { if (_accel_reports->get(arb)) { ret += sizeof(*arb); arb++; } } /* if there was no data, warn the caller */ return ret ? ret : -EAGAIN; } /* manual measurement */ _measure(); /* measurement will have generated a report, copy it out */ if (_accel_reports->get(arb)) { ret = sizeof(*arb); } return ret; } ssize_t ACCELSIM::mag_read(void *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct mag_report); mag_report *mrb = reinterpret_cast(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; } int ACCELSIM::devIOCTL(unsigned long cmd, unsigned long arg) { unsigned long ul_arg = (unsigned long)arg; switch (cmd) { case SENSORIOCSPOLLRATE: { switch (ul_arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); m_sample_interval_usecs = 0; return OK; /* zero would be bad */ case 0: return -EINVAL; /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: return devIOCTL(SENSORIOCSPOLLRATE, 1600); case SENSOR_POLLRATE_DEFAULT: return devIOCTL(SENSORIOCSPOLLRATE, ACCELSIM_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { /* convert hz to hrt interval via microseconds */ unsigned interval = 1000000 / ul_arg; /* check against maximum sane rate */ if (interval < 500) { return -EINVAL; } /* adjust filters */ accel_set_driver_lowpass_filter((float)ul_arg, _accel_filter_x.get_cutoff_freq()); bool want_start = (m_sample_interval_usecs == 0); /* update interval for next measurement */ setSampleInterval(interval); if (want_start) { start(); } return OK; } } } case SENSORIOCGPOLLRATE: if (m_sample_interval_usecs == 0) { return SENSOR_POLLRATE_MANUAL; } return 1000000 / m_sample_interval_usecs; case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((ul_arg < 1) || (ul_arg > 100)) { return -EINVAL; } if (!_accel_reports->resize(ul_arg)) { return -ENOMEM; } return OK; } case SENSORIOCRESET: // Nothing to do for simulator return OK; case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ struct accel_calibration_s *s = (struct accel_calibration_s *) arg; float sum = s->x_scale + s->y_scale + s->z_scale; if (sum > 2.0f && sum < 4.0f) { memcpy(&_accel_scale, s, sizeof(_accel_scale)); return OK; } else { return -EINVAL; } } default: /* give it to the superclass */ return VirtDevObj::devIOCTL(cmd, arg); } } int ACCELSIM::mag_ioctl(unsigned long cmd, unsigned long arg) { unsigned long ul_arg = (unsigned long)arg; switch (cmd) { case SENSORIOCSPOLLRATE: { switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: _mag->stop(); _mag->m_sample_interval_usecs = 0; return OK; /* zero would be bad */ case 0: return -EINVAL; /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: /* 100 Hz is max for mag */ return mag_ioctl(SENSORIOCSPOLLRATE, 100); /* adjust to a legal polling interval in Hz */ default: { /* convert hz to hrt interval via microseconds */ unsigned interval = 1000000 / ul_arg; /* check against maximum sane rate (1ms) */ if (interval < 10000) { return -EINVAL; } bool want_start = (_mag->m_sample_interval_usecs == 0); /* update interval for next measurement */ _mag->setSampleInterval(interval); if (want_start) { _mag->start(); } return OK; } } } case SENSORIOCGPOLLRATE: if (_mag->m_sample_interval_usecs == 0) { return SENSOR_POLLRATE_MANUAL; } return 1000000 / _mag->m_sample_interval_usecs; case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) { return -EINVAL; } if (!_mag_reports->resize(arg)) { return -ENOMEM; } return OK; } case SENSORIOCRESET: // Nothing to do for simulator return OK; case MAGIOCSSAMPLERATE: // No need to set internal sampling rate for simulator return OK; case MAGIOCGSAMPLERATE: return _mag_samplerate; case MAGIOCSSCALE: /* copy scale in */ memcpy(&_mag_scale, (struct mag_calibration_s *) arg, sizeof(_mag_scale)); return OK; case MAGIOCGSCALE: /* copy scale out */ memcpy((struct mag_calibration_s *) arg, &_mag_scale, sizeof(_mag_scale)); return OK; case MAGIOCSRANGE: return mag_set_range(arg); case MAGIOCGRANGE: return _mag_range_ga; case MAGIOCGEXTERNAL: /* Even if this sensor is on the "external" SPI bus * it is still fixed to the autopilot assembly, * so always return 0. */ return 0; default: /* give it to the superclass */ return VirtDevObj::devIOCTL(cmd, arg); } } int ACCELSIM::accel_set_range(unsigned max_g) { float new_scale_g_digit = 0.732e-3f; _accel_range_scale = new_scale_g_digit * CONSTANTS_ONE_G; return OK; } int ACCELSIM::mag_set_range(unsigned max_ga) { float new_scale_ga_digit = 0.479e-3f; _mag_range_scale = new_scale_ga_digit; return OK; } int ACCELSIM::accel_set_driver_lowpass_filter(float samplerate, float bandwidth) { _accel_filter_x.set_cutoff_frequency(samplerate, bandwidth); _accel_filter_y.set_cutoff_frequency(samplerate, bandwidth); _accel_filter_z.set_cutoff_frequency(samplerate, bandwidth); return OK; } 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; } int ACCELSIM::stop() { //PX4_INFO("ACCELSIM::stop"); return VirtDevObj::stop(); } void ACCELSIM::_measure() { #if 0 static int x = 0; // Verify the samples are being taken at the expected rate if (x == 99) { x = 0; PX4_INFO("ACCELSIM::measure %" PRIu64, hrt_absolute_time()); } else { x++; } #endif /* status register and data as read back from the device */ #pragma pack(push, 1) struct { uint8_t cmd; uint8_t status; float temperature; float x; float y; float z; } raw_accel_report; #pragma pack(pop) accel_report accel_report = {}; /* start the performance counter */ perf_begin(_accel_sample_perf); /* fetch data from the sensor */ memset(&raw_accel_report, 0, sizeof(raw_accel_report)); raw_accel_report.cmd = DIR_READ | ACC_READ; if (OK != transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report))) { return; } /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) * 3) Scale the statically calibrated values with a linear * dynamically obtained factor * * Note: the static sensor offset is the number the sensor outputs * at a nominally 'zero' input. Therefore the offset has to * be subtracted. * * Example: A gyro outputs a value of 74 at zero angular rate * the offset is 74 from the origin and subtracting * 74 from all measurements centers them around zero. */ accel_report.timestamp = hrt_absolute_time(); accel_report.device_id = 1310728; // use the temperature from the last mag reading accel_report.temperature = _last_temperature; // report the error count as the sum of the number of bad // register reads and bad values. This allows the higher level // code to decide if it should use this sensor based on // whether it has had failures accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); if (math::isZero(_accel_range_scale)) { _accel_range_scale = FLT_EPSILON; } accel_report.x_raw = math::constrainFloatToInt16(raw_accel_report.x / _accel_range_scale); accel_report.y_raw = math::constrainFloatToInt16(raw_accel_report.y / _accel_range_scale); accel_report.z_raw = math::constrainFloatToInt16(raw_accel_report.z / _accel_range_scale); accel_report.x = raw_accel_report.x; accel_report.y = raw_accel_report.y; accel_report.z = raw_accel_report.z; accel_report.scaling = _accel_range_scale; _accel_reports->force(&accel_report); if (!(m_pub_blocked)) { /* publish it */ // The first call to measure() is from init() and _accel_topic is not // yet initialized if (_accel_topic != nullptr) { orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); } } _accel_read++; /* stop the perf counter */ perf_end(_accel_sample_perf); } void ACCELSIM::mag_measure() { /* status register and data as read back from the device */ #pragma pack(push, 1) struct { uint8_t cmd; uint8_t status; float temperature; float x; float y; float z; } raw_mag_report; #pragma pack(pop) mag_report mag_report = {}; /* start the performance counter */ perf_begin(_mag_sample_perf); /* fetch data from the sensor */ memset(&raw_mag_report, 0, sizeof(raw_mag_report)); raw_mag_report.cmd = DIR_READ | MAG_READ; if (OK != transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report))) { return; } /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) * 3) Scale the statically calibrated values with a linear * dynamically obtained factor * * Note: the static sensor offset is the number the sensor outputs * at a nominally 'zero' input. Therefore the offset has to * be subtracted. * * Example: A gyro outputs a value of 74 at zero angular rate * the offset is 74 from the origin and subtracting * 74 from all measurements centers them around zero. */ mag_report.timestamp = hrt_absolute_time(); mag_report.device_id = 196616; mag_report.is_external = false; if (math::isZero(_mag_range_scale)) { _mag_range_scale = FLT_EPSILON; } float xraw_f = math::constrainFloatToInt16(raw_mag_report.x / _mag_range_scale); float yraw_f = math::constrainFloatToInt16(raw_mag_report.y / _mag_range_scale); float zraw_f = math::constrainFloatToInt16(raw_mag_report.z / _mag_range_scale); mag_report.x_raw = xraw_f; mag_report.y_raw = yraw_f; mag_report.z_raw = zraw_f; /* apply user specified rotation */ rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); /* remember the temperature. The datasheet isn't clear, but it * seems to be a signed offset from 25 degrees C in units of 0.125C */ _last_temperature = raw_mag_report.temperature; mag_report.temperature = _last_temperature; mag_report.x = raw_mag_report.x; mag_report.y = raw_mag_report.y; mag_report.z = raw_mag_report.z; _mag_reports->force(&mag_report); if (!(m_pub_blocked)) { /* publish it */ orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); } _mag_read++; /* stop the perf counter */ perf_end(_mag_sample_perf); } ACCELSIM_mag::ACCELSIM_mag(ACCELSIM *parent) : VirtDevObj("ACCELSIM_mag", ACCELSIM_DEVICE_PATH_MAG, MAG_BASE_DEVICE_PATH, 10000), _parent(parent), _mag_topic(nullptr), _mag_orb_class_instance(-1), _mag_class_instance(-1) { m_id.dev_id_s.bus = 1; m_id.dev_id_s.devtype = DRV_ACC_DEVTYPE_ACCELSIM; } ssize_t ACCELSIM_mag::devRead(void *buffer, size_t buflen) { return _parent->mag_read(buffer, buflen); } int ACCELSIM_mag::devIOCTL(unsigned long cmd, unsigned long arg) { int ret; switch (cmd) { case DEVIOCGDEVICEID: ret = (int)VirtDevObj::devIOCTL(cmd, arg); //PX4_WARN("DEVICE ID: %d", ret); return ret; break; default: return _parent->mag_ioctl(cmd, arg); } } int ACCELSIM_mag::start() { //PX4_INFO("ACCELSIM_mag::start"); return VirtDevObj::start(); } int ACCELSIM_mag::stop() { //PX4_INFO("ACCELSIM_mag::stop"); return VirtDevObj::stop(); } void ACCELSIM_mag::_measure() { //PX4_INFO("ACCELSIM_mag::_measure"); _parent->mag_measure(); } /** * Local functions in support of the shell command. */ namespace accelsim { ACCELSIM *g_dev; int start(enum Rotation rotation); int info(); void usage(); /** * Start the driver. * * This function call only returns once the driver is * up and running or failed to detect the sensor. */ int start(enum Rotation rotation) { if (g_dev != nullptr) { PX4_WARN("already started"); return 0; } DevHandle h; DevHandle h_mag; /* create the driver */ g_dev = new ACCELSIM(ACCELSIM_DEVICE_PATH_ACCEL, rotation); if (g_dev == nullptr) { PX4_ERR("failed instantiating ACCELSIM obj"); goto fail; } if (OK != g_dev->init()) { PX4_ERR("failed init of ACCELSIM obj"); goto fail; } /* set the poll rate to default, starts automatic data collection */ DevMgr::getHandle(ACCELSIM_DEVICE_PATH_ACCEL, h); if (!h.isValid()) { PX4_WARN("open %s failed", ACCELSIM_DEVICE_PATH_ACCEL); goto fail; } if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); DevMgr::releaseHandle(h); goto fail; } DevMgr::getHandle(ACCELSIM_DEVICE_PATH_MAG, h_mag); /* don't fail if mag dev cannot be opened */ if (h_mag.isValid()) { if (h_mag.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_MAG); } } else { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_MAG); } DevMgr::releaseHandle(h); DevMgr::releaseHandle(h_mag); return 0; fail: if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } PX4_ERR("driver start failed"); return 1; } /** * Print a little info about the driver. */ int info() { if (g_dev == nullptr) { PX4_ERR("driver not running"); return 1; } PX4_DEBUG("state @ %p", g_dev); unsigned dummy = 0; PX4_WARN("device_id: %u", (unsigned int)g_dev->devIOCTL(DEVIOCGDEVICEID, dummy)); return 0; } void usage() { PX4_WARN("Usage: accelsim 'start', 'info'"); PX4_WARN("options:"); PX4_WARN(" -R rotation"); } } // namespace int accelsim_main(int argc, char *argv[]) { int ch; enum Rotation rotation = ROTATION_NONE; int ret; int myoptind = 1; const char *myoptarg = nullptr; /* jump over start/off/etc and look at options first */ while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'R': rotation = (enum Rotation)atoi(myoptarg); break; default: accelsim::usage(); return 0; } } if (myoptind >= argc) { accelsim::usage(); return 1; } const char *verb = argv[myoptind]; /* * Start/load the driver. */ if (!strcmp(verb, "start")) { ret = accelsim::start(rotation); } /* * Print driver information. */ else if (!strcmp(verb, "info")) { ret = accelsim::info(); } else { accelsim::usage(); return 1; } return ret; }