Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 08:36:08 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

Linux: Added device simulator to simulate an I2C or SPI device

Browse Source 
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 <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois
2015-04-06 19:31:03 -07:00
parent 4780353cd8
commit 76a6f19476
9 changed files with 457 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/device/module.mk
+2 -1
View File
@@ -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
src/drivers/device/sim.cpp
+115
View File
@@ -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 <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#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
src/drivers/device/sim.h
+112
View File
@@ -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
src/drivers/device/vdev.h
+7 -7
View File
@@ -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
src/drivers/ms5611/module.mk
+1 -1
View File
@@ -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
src/drivers/ms5611/ms5611.h
+1
View File
@@ -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);
src/drivers/ms5611/ms5611_linux.cpp
+24 -12
View File
@@ -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();
src/drivers/ms5611/ms5611_sim.cpp
+194
View File
@@ -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 <px4_config.h>
#include <px4_defines.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <assert.h>
//#include <debug.h>
#include <errno.h>
#include <unistd.h>
#include <drivers/device/sim.h>
#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;
}
src/platforms/linux/include/board_config.h
+1 -1
View File
@@ -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