From 76a6f194761b1bc4ff17730f38cf688442c2fe88 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 6 Apr 2015 19:31:03 -0700 Subject: [PATCH] Linux: Added device simulator to simulate an I2C or SPI device The simulated device satisfies the factory pattern used by MS5611 to create a specific I2C or SPI device instance. For now the functions just return true, but should/could return simulated data. Signed-off-by: Mark Charlebois --- src/drivers/device/module.mk | 3 +- src/drivers/device/sim.cpp | 115 ++++++++++++ src/drivers/device/sim.h | 112 ++++++++++++ src/drivers/device/vdev.h | 14 +- src/drivers/ms5611/module.mk | 2 +- src/drivers/ms5611/ms5611.h | 1 + src/drivers/ms5611/ms5611_linux.cpp | 36 ++-- src/drivers/ms5611/ms5611_sim.cpp | 194 +++++++++++++++++++++ src/platforms/linux/include/board_config.h | 2 +- 9 files changed, 457 insertions(+), 22 deletions(-) create mode 100644 src/drivers/device/sim.cpp create mode 100644 src/drivers/device/sim.h create mode 100644 src/drivers/ms5611/ms5611_sim.cpp diff --git a/src/drivers/device/module.mk b/src/drivers/device/module.mk index c927426ae7..488e5e759e 100644 --- a/src/drivers/device/module.mk +++ b/src/drivers/device/module.mk @@ -47,5 +47,6 @@ ifeq ($(PX4_TARGET_OS),linux) SRCS = vdev.cpp \ device.cpp \ vdev_posix.cpp \ - i2c_linux.cpp + i2c_linux.cpp \ + sim.cpp endif diff --git a/src/drivers/device/sim.cpp b/src/drivers/device/sim.cpp new file mode 100644 index 0000000000..f1040ea8e7 --- /dev/null +++ b/src/drivers/device/sim.cpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 sim.cpp + * + * Base class for simulated devices. + * + * @todo Bus frequency changes; currently we do nothing with the value + * that is supplied. Should we just depend on the bus knowing? + */ + +#include +#include +#include +#include "sim.h" + +namespace device +{ + +SIM::SIM(const char *name, + const char *devname, + int bus, + uint16_t address) : + // base class + Device(name), + // public + // protected + // private + _bus(bus), + _address(address), + _devname(devname) +{ + + printf("SIM::SIM name = %s devname = %s\n", name, devname); + // fill in _device_id fields for a SIM device + _device_id.devid_s.bus_type = DeviceBusType_SIM; + _device_id.devid_s.bus = bus; + _device_id.devid_s.address = address; + // devtype needs to be filled in by the driver + _device_id.devid_s.devtype = 0; +} + +SIM::~SIM() +{ +} + +int +SIM::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 registers the virtual driver + ret = Device::init(); + + if (ret != PX4_OK) { + debug("VDev::init failed"); + return ret; + } + + return ret; +} + +int +SIM::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) +{ + if (send_len > 0) { + printf("SIM: sending %d bytes\n", send_len); + } + + if (recv_len > 0) { + printf("SIM: receiving %d bytes\n", recv_len); + + // TODO - write data to recv; + } + + printf("I2C SIM: transfer_4 on %s\n", _devname); + + return PX4_OK; +} + +} // namespace device diff --git a/src/drivers/device/sim.h b/src/drivers/device/sim.h new file mode 100644 index 0000000000..05e4fa78c7 --- /dev/null +++ b/src/drivers/device/sim.h @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 sim.h + * + * Base class for devices on simulation bus. + */ + +#pragma once + +#include "vdev.h" + +namespace device __EXPORT +{ + +/** + * Abstract class for character device on SIM + */ +class __EXPORT SIM : public Device +{ + +public: + + /** + * Get the address + */ + int16_t get_address() const { return _address; } + +protected: + /** + * The number of times a read or write operation will be retried on + * error. + */ + unsigned _retries; + + /** + * The SIM bus number the device is attached to. + */ + int _bus; + + /** + * @ Constructor + * + * @param name Driver name + * @param devname Device node name + * @param bus SIM bus on which the device lives + * @param address SIM bus address, or zero if set_address will be used + */ + SIM(const char *name, + const char *devname, + int bus, + uint16_t address); + virtual ~SIM(); + + virtual int init(); + + /** + * Perform an SIM transaction to the device. + * + * At least one of send_len and recv_len must be non-zero. + * + * @param send Pointer to bytes to send. + * @param send_len Number of bytes to send. + * @param recv Pointer to buffer for bytes received. + * @param recv_len Number of bytes to receive. + * @return OK if the transfer was successful, -errno + * otherwise. + */ + int transfer(const uint8_t *send, unsigned send_len, + uint8_t *recv, unsigned recv_len); + +private: + uint16_t _address; + const char * _devname; + + SIM(const device::SIM&); + SIM operator=(const device::SIM&); +}; + +} // namespace device + diff --git a/src/drivers/device/vdev.h b/src/drivers/device/vdev.h index 586971ed29..e98d13209a 100644 --- a/src/drivers/device/vdev.h +++ b/src/drivers/device/vdev.h @@ -71,10 +71,9 @@ struct px4_dev_handle_t { }; /** - * Fundamental base class for all device drivers. + * Fundamental base class for all physical drivers (I2C, SPI). * - * This class handles the basic "being a driver" things, including - * interrupt registration and dispatch. + * This class provides the basic driver template for I2C and SPI devices */ class __EXPORT Device { @@ -107,7 +106,7 @@ public: * @param count The number of items to read. * @return The number of items read on success, negative errno otherwise. */ - int dev_read(unsigned address, void *data, unsigned count); + virtual int dev_read(unsigned address, void *data, unsigned count); /** * Write directly to the device. @@ -119,7 +118,7 @@ public: * @param count The number of items to write. * @return The number of items written on success, negative errno otherwise. */ - int dev_write(unsigned address, void *data, unsigned count); + virtual int dev_write(unsigned address, void *data, unsigned count); /** * Perform a device-specific operation. @@ -128,7 +127,7 @@ public: * @param arg An argument to the operation. * @return Negative errno on error, OK or positive value on success. */ - int dev_ioctl(unsigned operation, unsigned &arg); + virtual int dev_ioctl(unsigned operation, unsigned &arg); /* device bus types for DEVID @@ -138,6 +137,7 @@ public: DeviceBusType_I2C = 1, DeviceBusType_SPI = 2, DeviceBusType_UAVCAN = 3, + DeviceBusType_SIM = 4, }; /* @@ -452,7 +452,7 @@ private: /* do not allow copying this class */ VDev(const VDev&); - VDev operator=(const VDev&); + //VDev operator=(const VDev&); }; #if 0 diff --git a/src/drivers/ms5611/module.mk b/src/drivers/ms5611/module.mk index ba1ac7efcf..59316f4bfe 100644 --- a/src/drivers/ms5611/module.mk +++ b/src/drivers/ms5611/module.mk @@ -40,7 +40,7 @@ MODULE_COMMAND = ms5611 ifeq ($(PX4_TARGET_OS),nuttx) SRCS = ms5611_nuttx.cpp ms5611_spi.cpp ms5611_i2c.cpp else -SRCS = ms5611_linux.cpp ms5611_spi.cpp ms5611_i2c.cpp +SRCS = ms5611_linux.cpp ms5611_spi.cpp ms5611_i2c.cpp ms5611_sim.cpp endif MAXOPTIMIZATION = -Os diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h index d129891bda..1d6239467d 100644 --- a/src/drivers/ms5611/ms5611.h +++ b/src/drivers/ms5611/ms5611.h @@ -82,4 +82,5 @@ extern bool crc4(uint16_t *n_prom); /* interface factories */ extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) __attribute__((weak)); extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) __attribute__((weak)); +extern device::Device *MS5611_sim_interface(ms5611::prom_u &prom_buf, uint8_t busnum) __attribute__((weak)); typedef device::Device* (*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum); diff --git a/src/drivers/ms5611/ms5611_linux.cpp b/src/drivers/ms5611/ms5611_linux.cpp index 4acb2f251b..4ee61a1277 100644 --- a/src/drivers/ms5611/ms5611_linux.cpp +++ b/src/drivers/ms5611/ms5611_linux.cpp @@ -71,7 +71,8 @@ enum MS5611_BUS { MS5611_BUS_I2C_INTERNAL, MS5611_BUS_I2C_EXTERNAL, MS5611_BUS_SPI_INTERNAL, - MS5611_BUS_SPI_EXTERNAL + MS5611_BUS_SPI_EXTERNAL, + MS5611_BUS_SIM_EXTERNAL }; /* oddly, ERROR is not defined for c++ */ @@ -258,6 +259,7 @@ int MS5611::init() { int ret; + warnx("MS5611::init"); ret = VDev::init(); if (ret != OK) { @@ -287,6 +289,7 @@ MS5611::init() /* do temperature first */ if (OK != measure()) { ret = -EIO; + warnx("temp measure failed"); break; } @@ -294,12 +297,14 @@ MS5611::init() if (OK != collect()) { ret = -EIO; + warnx("temp collect failed"); break; } /* now do a pressure measurement */ if (OK != measure()) { ret = -EIO; + warnx("pressure collect failed"); break; } @@ -307,6 +312,7 @@ MS5611::init() if (OK != collect()) { ret = -EIO; + warnx("pressure collect failed"); break; } @@ -318,10 +324,10 @@ MS5611::init() _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); - - if (_baro_topic < 0) { + if (_baro_topic == (orb_advert_t)(-1)) { warnx("failed to create sensor_baro publication"); } + //warnx("sensor_baro publication %ld", _baro_topic); } while (0); @@ -735,8 +741,13 @@ MS5611::collect() /* publish it */ if (!(_pub_blocked)) { - /* publish it */ - orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); + if (_baro_topic != (orb_advert_t)(-1)) { + /* publish it */ + orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); + } + else { + printf("MS5611::collect _baro_topic not initialized\n"); + } } if (_reports->force(&report)) { @@ -796,6 +807,7 @@ struct ms5611_bus_option { uint8_t busnum; MS5611 *dev; } bus_options[] = { +#if 0 #if defined(PX4_SPIDEV_EXT_BARO) && defined(PX4_SPI_BUS_EXT) { MS5611_BUS_SPI_EXTERNAL, "/dev/ms5611_spi_ext", &MS5611_spi_interface, PX4_SPI_BUS_EXT, NULL }, #endif @@ -808,6 +820,10 @@ struct ms5611_bus_option { #ifdef PX4_I2C_BUS_EXPANSION { MS5611_BUS_I2C_EXTERNAL, "/dev/ms5611_ext", &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION, NULL }, #endif +#endif +#ifdef PX4_SIM_BUS_TEST + { MS5611_BUS_SIM_EXTERNAL, "/dev/ms5611_sim", &MS5611_sim_interface, PX4_SIM_BUS_TEST, NULL }, +#endif }; #define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) @@ -1173,8 +1189,7 @@ usage() warnx("options:"); warnx(" -X (external I2C bus)"); warnx(" -I (intternal I2C bus)"); - warnx(" -S (external SPI bus)"); - warnx(" -s (internal SPI bus)"); + warnx(" -S (Simulation bus)"); } } // namespace @@ -1208,7 +1223,7 @@ ms5611_main(int argc, char *argv[]) /* jump over start/off/etc and look at options first */ int myoptind = 1; - while ((ch = getopt(argc, argv, "XISs", &myoptind)) != EOF) { + while ((ch = getopt(argc, argv, "XIS", &myoptind)) != EOF) { printf("ch = %d\n", ch); switch (ch) { case 'X': @@ -1218,10 +1233,7 @@ ms5611_main(int argc, char *argv[]) busid = MS5611_BUS_I2C_INTERNAL; break; case 'S': - busid = MS5611_BUS_SPI_EXTERNAL; - break; - case 's': - busid = MS5611_BUS_SPI_INTERNAL; + busid = MS5611_BUS_SIM_EXTERNAL; break; default: ms5611::usage(); diff --git a/src/drivers/ms5611/ms5611_sim.cpp b/src/drivers/ms5611/ms5611_sim.cpp new file mode 100644 index 0000000000..782712bc4d --- /dev/null +++ b/src/drivers/ms5611/ms5611_sim.cpp @@ -0,0 +1,194 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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 ms5611_i2c.cpp + * + * SIM interface for MS5611 + */ + +/* XXX trim includes */ +#include +#include + +#include +#include +#include +#include +//#include +#include +#include + +#include +#include "ms5611.h" +#include "board_config.h" + +device::Device *MS5611_sim_interface(ms5611::prom_u &prom_buf); + +class MS5611_SIM : public device::SIM +{ +public: + MS5611_SIM(uint8_t bus, ms5611::prom_u &prom_buf); + virtual ~MS5611_SIM(); + + virtual int init(); + virtual int dev_read(unsigned offset, void *data, unsigned count); + virtual int dev_ioctl(unsigned operation, unsigned &arg); + +private: + ms5611::prom_u &_prom; + + /** + * Send a reset command to the MS5611. + * + * This is required after any bus reset. + */ + int _reset(); + + /** + * Send a measure command to the MS5611. + * + * @param addr Which address to use for the measure operation. + */ + int _measure(unsigned addr); + + /** + * Read the MS5611 PROM + * + * @return PX4_OK if the PROM reads successfully. + */ + int _read_prom(); + +}; + +device::Device * +MS5611_sim_interface(ms5611::prom_u &prom_buf, uint8_t busnum) +{ + return new MS5611_SIM(busnum, prom_buf); +} + +MS5611_SIM::MS5611_SIM(uint8_t bus, ms5611::prom_u &prom) : + SIM("MS5611_SIM", "/vdev/MS5611_SIM", bus, 0), + _prom(prom) +{ +} + +MS5611_SIM::~MS5611_SIM() +{ +} + +int +MS5611_SIM::init() +{ + return SIM::init(); +} + +int +MS5611_SIM::dev_read(unsigned offset, void *data, unsigned count) +{ + union _cvt { + uint8_t b[4]; + uint32_t w; + } *cvt = (_cvt *)data; + uint8_t buf[3]; + + /* read the most recent measurement */ + uint8_t cmd = 0; + int ret = transfer(&cmd, 1, &buf[0], 3); + if (ret == PX4_OK) { + /* fetch the raw value */ + cvt->b[0] = buf[2]; + cvt->b[1] = buf[1]; + cvt->b[2] = buf[0]; + cvt->b[3] = 0; + } + + return ret; +} + +int +MS5611_SIM::dev_ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + case IOCTL_RESET: + ret = _reset(); + break; + + case IOCTL_MEASURE: + ret = _measure(arg); + break; + + default: + ret = EINVAL; + } + + return ret; +} + +int +MS5611_SIM::_reset() +{ + unsigned old_retrycount = _retries; + uint8_t cmd = ADDR_RESET_CMD; + int result; + + /* bump the retry count */ + _retries = 10; + result = transfer(&cmd, 1, nullptr, 0); + _retries = old_retrycount; + + return result; +} + +int +MS5611_SIM::_measure(unsigned addr) +{ + /* + * Disable retries on this command; we can't know whether failure + * means the device did or did not see the command. + */ + _retries = 0; + + uint8_t cmd = addr; + return transfer(&cmd, 1, nullptr, 0); +} + +int +MS5611_SIM::_read_prom() +{ + int ret = 1; + // TODO input simlation data + return ret; +} diff --git a/src/platforms/linux/include/board_config.h b/src/platforms/linux/include/board_config.h index 81bcc8f34d..afbdd98cd2 100644 --- a/src/platforms/linux/include/board_config.h +++ b/src/platforms/linux/include/board_config.h @@ -4,6 +4,6 @@ * I2C busses */ #define PX4_I2C_BUS_ESC 1 -#define PX4_I2C_BUS_ONBOARD 2 +#define PX4_SIM_BUS_TEST 2 #define PX4_I2C_BUS_EXPANSION 3 #define PX4_I2C_BUS_LED 3