mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-23 04:43:34 +08:00
Support for building more modules with Linux
Added more queue support to linux/px4_layer. Use virt char devices for ms5611, and mavlink. Added more HRT functionality. uORB latency test now fails. Likely due to bad HRT impl for Linux. Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois
+16
-143
@@ -34,64 +34,24 @@
|
||||
/**
|
||||
* @file device.cpp
|
||||
*
|
||||
* Fundamental driver base class for the device framework.
|
||||
* Fundamental driver base class for the virtual device framework.
|
||||
*/
|
||||
|
||||
#include "device.h"
|
||||
|
||||
#include <nuttx/arch.h>
|
||||
#include <px4_defines.h>
|
||||
#include <px4_posix.h>
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
#include <drivers/drv_device.h>
|
||||
|
||||
namespace device
|
||||
{
|
||||
|
||||
/**
|
||||
* Interrupt dispatch table entry.
|
||||
*/
|
||||
struct irq_entry {
|
||||
int irq;
|
||||
Device *owner;
|
||||
};
|
||||
|
||||
static const unsigned irq_nentries = 8; /**< size of the interrupt dispatch table */
|
||||
static irq_entry irq_entries[irq_nentries]; /**< interrupt dispatch table (XXX should be a vector) */
|
||||
|
||||
/**
|
||||
* Register an interrupt to a specific device.
|
||||
*
|
||||
* @param irq The interrupt number to register.
|
||||
* @param owner The device receiving the interrupt.
|
||||
* @return OK if the interrupt was registered.
|
||||
*/
|
||||
static int register_interrupt(int irq, Device *owner);
|
||||
|
||||
/**
|
||||
* Unregister an interrupt.
|
||||
*
|
||||
* @param irq The previously-registered interrupt to be de-registered.
|
||||
*/
|
||||
static void unregister_interrupt(int irq);
|
||||
|
||||
/**
|
||||
* Handle an interrupt.
|
||||
*
|
||||
* @param irq The interrupt being invoked.
|
||||
* @param context The interrupt register context.
|
||||
* @return Always returns OK.
|
||||
*/
|
||||
static int interrupt(int irq, void *context);
|
||||
|
||||
Device::Device(const char *name,
|
||||
int irq) :
|
||||
Device::Device(const char *name) :
|
||||
// public
|
||||
// protected
|
||||
_name(name),
|
||||
_debug_enabled(false),
|
||||
// private
|
||||
_irq(irq),
|
||||
_irq_attached(false)
|
||||
_debug_enabled(false)
|
||||
{
|
||||
sem_init(&_lock, 0, 1);
|
||||
|
||||
@@ -107,9 +67,6 @@ Device::Device(const char *name,
|
||||
Device::~Device()
|
||||
{
|
||||
sem_destroy(&_lock);
|
||||
|
||||
if (_irq_attached)
|
||||
unregister_interrupt(_irq);
|
||||
}
|
||||
|
||||
int
|
||||
@@ -117,45 +74,9 @@ Device::init()
|
||||
{
|
||||
int ret = OK;
|
||||
|
||||
// If assigned an interrupt, connect it
|
||||
if (_irq) {
|
||||
/* ensure it's disabled */
|
||||
up_disable_irq(_irq);
|
||||
|
||||
/* register */
|
||||
ret = register_interrupt(_irq, this);
|
||||
|
||||
if (ret != OK)
|
||||
goto out;
|
||||
|
||||
_irq_attached = true;
|
||||
}
|
||||
|
||||
out:
|
||||
return ret;
|
||||
}
|
||||
|
||||
void
|
||||
Device::interrupt_enable()
|
||||
{
|
||||
if (_irq_attached)
|
||||
up_enable_irq(_irq);
|
||||
}
|
||||
|
||||
void
|
||||
Device::interrupt_disable()
|
||||
{
|
||||
if (_irq_attached)
|
||||
up_disable_irq(_irq);
|
||||
}
|
||||
|
||||
void
|
||||
Device::interrupt(void *context)
|
||||
{
|
||||
// default action is to disable the interrupt so we don't get called again
|
||||
interrupt_disable();
|
||||
}
|
||||
|
||||
void
|
||||
Device::log(const char *fmt, ...)
|
||||
{
|
||||
@@ -184,74 +105,26 @@ Device::debug(const char *fmt, ...)
|
||||
}
|
||||
}
|
||||
|
||||
static int
|
||||
register_interrupt(int irq, Device *owner)
|
||||
int
|
||||
Device::dev_read(unsigned offset, void *data, unsigned count)
|
||||
{
|
||||
int ret = -ENOMEM;
|
||||
|
||||
// look for a slot where we can register the interrupt
|
||||
for (unsigned i = 0; i < irq_nentries; i++) {
|
||||
if (irq_entries[i].irq == 0) {
|
||||
|
||||
// great, we could put it here; try attaching it
|
||||
ret = irq_attach(irq, &interrupt);
|
||||
|
||||
if (ret == OK) {
|
||||
irq_entries[i].irq = irq;
|
||||
irq_entries[i].owner = owner;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static void
|
||||
unregister_interrupt(int irq)
|
||||
{
|
||||
for (unsigned i = 0; i < irq_nentries; i++) {
|
||||
if (irq_entries[i].irq == irq) {
|
||||
irq_entries[i].irq = 0;
|
||||
irq_entries[i].owner = nullptr;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static int
|
||||
interrupt(int irq, void *context)
|
||||
{
|
||||
for (unsigned i = 0; i < irq_nentries; i++) {
|
||||
if (irq_entries[i].irq == irq) {
|
||||
irq_entries[i].owner->interrupt(context);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return OK;
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
int
|
||||
Device::read(unsigned offset, void *data, unsigned count)
|
||||
Device::dev_write(unsigned offset, void *data, unsigned count)
|
||||
{
|
||||
return -ENODEV;
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
int
|
||||
Device::write(unsigned offset, void *data, unsigned count)
|
||||
Device::dev_ioctl(unsigned operation, unsigned &arg)
|
||||
{
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
int
|
||||
Device::ioctl(unsigned operation, unsigned &arg)
|
||||
{
|
||||
switch (operation) {
|
||||
case DEVIOCGDEVICEID:
|
||||
return (int)_device_id.devid;
|
||||
}
|
||||
return -ENODEV;
|
||||
switch (operation) {
|
||||
case PX4_DEVIOCGDEVICEID:
|
||||
return (int)_device_id.devid;
|
||||
}
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
} // namespace device
|
||||
|
||||
@@ -0,0 +1,257 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2014 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 device.cpp
|
||||
*
|
||||
* Fundamental driver base class for the device framework.
|
||||
*/
|
||||
|
||||
#include "device.h"
|
||||
|
||||
#include <nuttx/arch.h>
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
#include <drivers/drv_device.h>
|
||||
|
||||
namespace device
|
||||
{
|
||||
|
||||
/**
|
||||
* Interrupt dispatch table entry.
|
||||
*/
|
||||
struct irq_entry {
|
||||
int irq;
|
||||
Device *owner;
|
||||
};
|
||||
|
||||
static const unsigned irq_nentries = 8; /**< size of the interrupt dispatch table */
|
||||
static irq_entry irq_entries[irq_nentries]; /**< interrupt dispatch table (XXX should be a vector) */
|
||||
|
||||
/**
|
||||
* Register an interrupt to a specific device.
|
||||
*
|
||||
* @param irq The interrupt number to register.
|
||||
* @param owner The device receiving the interrupt.
|
||||
* @return OK if the interrupt was registered.
|
||||
*/
|
||||
static int register_interrupt(int irq, Device *owner);
|
||||
|
||||
/**
|
||||
* Unregister an interrupt.
|
||||
*
|
||||
* @param irq The previously-registered interrupt to be de-registered.
|
||||
*/
|
||||
static void unregister_interrupt(int irq);
|
||||
|
||||
/**
|
||||
* Handle an interrupt.
|
||||
*
|
||||
* @param irq The interrupt being invoked.
|
||||
* @param context The interrupt register context.
|
||||
* @return Always returns OK.
|
||||
*/
|
||||
static int interrupt(int irq, void *context);
|
||||
|
||||
Device::Device(const char *name,
|
||||
int irq) :
|
||||
// public
|
||||
// protected
|
||||
_name(name),
|
||||
_debug_enabled(false),
|
||||
// private
|
||||
_irq(irq),
|
||||
_irq_attached(false)
|
||||
{
|
||||
sem_init(&_lock, 0, 1);
|
||||
|
||||
/* setup a default device ID. When bus_type is UNKNOWN the
|
||||
other fields are invalid */
|
||||
_device_id.devid = 0;
|
||||
_device_id.devid_s.bus_type = DeviceBusType_UNKNOWN;
|
||||
_device_id.devid_s.bus = 0;
|
||||
_device_id.devid_s.address = 0;
|
||||
_device_id.devid_s.devtype = 0;
|
||||
}
|
||||
|
||||
Device::~Device()
|
||||
{
|
||||
sem_destroy(&_lock);
|
||||
|
||||
if (_irq_attached)
|
||||
unregister_interrupt(_irq);
|
||||
}
|
||||
|
||||
int
|
||||
Device::init()
|
||||
{
|
||||
int ret = OK;
|
||||
|
||||
// If assigned an interrupt, connect it
|
||||
if (_irq) {
|
||||
/* ensure it's disabled */
|
||||
up_disable_irq(_irq);
|
||||
|
||||
/* register */
|
||||
ret = register_interrupt(_irq, this);
|
||||
|
||||
if (ret != OK)
|
||||
goto out;
|
||||
|
||||
_irq_attached = true;
|
||||
}
|
||||
|
||||
out:
|
||||
return ret;
|
||||
}
|
||||
|
||||
void
|
||||
Device::interrupt_enable()
|
||||
{
|
||||
if (_irq_attached)
|
||||
up_enable_irq(_irq);
|
||||
}
|
||||
|
||||
void
|
||||
Device::interrupt_disable()
|
||||
{
|
||||
if (_irq_attached)
|
||||
up_disable_irq(_irq);
|
||||
}
|
||||
|
||||
void
|
||||
Device::interrupt(void *context)
|
||||
{
|
||||
// default action is to disable the interrupt so we don't get called again
|
||||
interrupt_disable();
|
||||
}
|
||||
|
||||
void
|
||||
Device::log(const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
|
||||
printf("[%s] ", _name);
|
||||
va_start(ap, fmt);
|
||||
vprintf(fmt, ap);
|
||||
va_end(ap);
|
||||
printf("\n");
|
||||
fflush(stdout);
|
||||
}
|
||||
|
||||
void
|
||||
Device::debug(const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
|
||||
if (_debug_enabled) {
|
||||
printf("<%s> ", _name);
|
||||
va_start(ap, fmt);
|
||||
vprintf(fmt, ap);
|
||||
va_end(ap);
|
||||
printf("\n");
|
||||
fflush(stdout);
|
||||
}
|
||||
}
|
||||
|
||||
static int
|
||||
register_interrupt(int irq, Device *owner)
|
||||
{
|
||||
int ret = -ENOMEM;
|
||||
|
||||
// look for a slot where we can register the interrupt
|
||||
for (unsigned i = 0; i < irq_nentries; i++) {
|
||||
if (irq_entries[i].irq == 0) {
|
||||
|
||||
// great, we could put it here; try attaching it
|
||||
ret = irq_attach(irq, &interrupt);
|
||||
|
||||
if (ret == OK) {
|
||||
irq_entries[i].irq = irq;
|
||||
irq_entries[i].owner = owner;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static void
|
||||
unregister_interrupt(int irq)
|
||||
{
|
||||
for (unsigned i = 0; i < irq_nentries; i++) {
|
||||
if (irq_entries[i].irq == irq) {
|
||||
irq_entries[i].irq = 0;
|
||||
irq_entries[i].owner = nullptr;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static int
|
||||
interrupt(int irq, void *context)
|
||||
{
|
||||
for (unsigned i = 0; i < irq_nentries; i++) {
|
||||
if (irq_entries[i].irq == irq) {
|
||||
irq_entries[i].owner->interrupt(context);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int
|
||||
Device::read(unsigned offset, void *data, unsigned count)
|
||||
{
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
int
|
||||
Device::write(unsigned offset, void *data, unsigned count)
|
||||
{
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
int
|
||||
Device::ioctl(unsigned operation, unsigned &arg)
|
||||
{
|
||||
switch (operation) {
|
||||
case DEVIOCGDEVICEID:
|
||||
return (int)_device_id.devid;
|
||||
}
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
} // namespace device
|
||||
@@ -42,7 +42,7 @@
|
||||
|
||||
#include "device.h"
|
||||
|
||||
#include <nuttx/i2c.h>
|
||||
#include <px4_i2c.h>
|
||||
|
||||
namespace device __EXPORT
|
||||
{
|
||||
@@ -124,7 +124,7 @@ protected:
|
||||
* @return OK if the transfer was successful, -errno
|
||||
* otherwise.
|
||||
*/
|
||||
int transfer(i2c_msg_s *msgv, unsigned msgs);
|
||||
int transfer(px4_i2c_msg_t *msgv, unsigned msgs);
|
||||
|
||||
/**
|
||||
* Change the bus address.
|
||||
@@ -142,7 +142,7 @@ protected:
|
||||
private:
|
||||
uint16_t _address;
|
||||
uint32_t _frequency;
|
||||
struct i2c_dev_s *_dev;
|
||||
px4_i2c_dev_t *_dev;
|
||||
|
||||
I2C(const device::I2C &);
|
||||
I2C operator=(const device::I2C &);
|
||||
|
||||
@@ -0,0 +1,257 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 i2c.cpp
|
||||
*
|
||||
* Base class for devices attached via the I2C bus.
|
||||
*
|
||||
* @todo Bus frequency changes; currently we do nothing with the value
|
||||
* that is supplied. Should we just depend on the bus knowing?
|
||||
*/
|
||||
|
||||
#include "i2c.h"
|
||||
|
||||
namespace device
|
||||
{
|
||||
|
||||
unsigned int I2C::_bus_clocks[3] = { 100000, 100000, 100000 };
|
||||
|
||||
I2C::I2C(const char *name,
|
||||
const char *devname,
|
||||
int bus,
|
||||
uint16_t address,
|
||||
uint32_t frequency,
|
||||
int irq) :
|
||||
// base class
|
||||
CDev(name, devname),
|
||||
// public
|
||||
// protected
|
||||
_retries(0),
|
||||
// private
|
||||
_bus(bus),
|
||||
_address(address),
|
||||
_frequency(frequency),
|
||||
_dev(nullptr)
|
||||
{
|
||||
// fill in _device_id fields for a I2C device
|
||||
_device_id.devid_s.bus_type = DeviceBusType_I2C;
|
||||
_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;
|
||||
}
|
||||
|
||||
I2C::~I2C()
|
||||
{
|
||||
if (_dev) {
|
||||
px4_i2cuninitialize(_dev);
|
||||
_dev = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
I2C::set_bus_clock(unsigned bus, unsigned clock_hz)
|
||||
{
|
||||
int index = bus - 1;
|
||||
|
||||
if (index < 0 || index >= static_cast<int>(sizeof(_bus_clocks) / sizeof(_bus_clocks[0]))) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
if (_bus_clocks[index] > 0) {
|
||||
// debug("overriding clock of %u with %u Hz\n", _bus_clocks[index], clock_hz);
|
||||
}
|
||||
_bus_clocks[index] = clock_hz;
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
int
|
||||
I2C::init()
|
||||
{
|
||||
int ret = PX4_OK;
|
||||
unsigned bus_index;
|
||||
|
||||
// attach to the i2c bus
|
||||
_dev = px4_i2cinitialize(_bus);
|
||||
|
||||
if (_dev == nullptr) {
|
||||
debug("failed to init I2C");
|
||||
ret = -ENOENT;
|
||||
goto out;
|
||||
}
|
||||
|
||||
// the above call fails for a non-existing bus index,
|
||||
// so the index math here is safe.
|
||||
bus_index = _bus - 1;
|
||||
|
||||
// abort if the max frequency we allow (the frequency we ask)
|
||||
// is smaller than the bus frequency
|
||||
if (_bus_clocks[bus_index] > _frequency) {
|
||||
(void)px4_i2cuninitialize(_dev);
|
||||
_dev = nullptr;
|
||||
log("FAIL: too slow for bus #%u: %u KHz, device max: %u KHz)",
|
||||
_bus, _bus_clocks[bus_index] / 1000, _frequency / 1000);
|
||||
ret = -EINVAL;
|
||||
goto out;
|
||||
}
|
||||
|
||||
// set the bus frequency on the first access if it has
|
||||
// not been set yet
|
||||
if (_bus_clocks[bus_index] == 0) {
|
||||
_bus_clocks[bus_index] = _frequency;
|
||||
}
|
||||
|
||||
// set frequency for this instance once to the bus speed
|
||||
// the bus speed is the maximum supported by all devices on the bus,
|
||||
// as we have to prioritize performance over compatibility.
|
||||
// If a new device requires a lower clock speed, this has to be
|
||||
// manually set via "fmu i2c <bus> <clock>" before starting any
|
||||
// drivers.
|
||||
// This is necessary as automatically lowering the bus speed
|
||||
// for maximum compatibility could induce timing issues on
|
||||
// critical sensors the adopter might be unaware of.
|
||||
I2C_SETFREQUENCY(_dev, _bus_clocks[bus_index]);
|
||||
|
||||
// call the probe function to check whether the device is present
|
||||
ret = probe();
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
debug("probe failed");
|
||||
goto out;
|
||||
}
|
||||
|
||||
// do base class init, which will create device node, etc
|
||||
ret = CDev::init();
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
debug("cdev init failed");
|
||||
goto out;
|
||||
}
|
||||
|
||||
// tell the world where we are
|
||||
log("on I2C bus %d at 0x%02x (bus: %u KHz, max: %u KHz)",
|
||||
_bus, _address, _bus_clocks[bus_index] / 1000, _frequency / 1000);
|
||||
|
||||
out:
|
||||
if ((ret != PX4_OK) && (_dev != nullptr)) {
|
||||
px4_i2cuninitialize(_dev);
|
||||
_dev = nullptr;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
I2C::probe()
|
||||
{
|
||||
// Assume the device is too stupid to be discoverable.
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
int
|
||||
I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
|
||||
{
|
||||
px4_i2c_msg_t msgv[2];
|
||||
unsigned msgs;
|
||||
int ret;
|
||||
unsigned retry_count = 0;
|
||||
|
||||
do {
|
||||
// debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len);
|
||||
|
||||
msgs = 0;
|
||||
|
||||
if (send_len > 0) {
|
||||
msgv[msgs].addr = _address;
|
||||
msgv[msgs].flags = 0;
|
||||
msgv[msgs].buffer = const_cast<uint8_t *>(send);
|
||||
msgv[msgs].length = send_len;
|
||||
msgs++;
|
||||
}
|
||||
|
||||
if (recv_len > 0) {
|
||||
msgv[msgs].addr = _address;
|
||||
msgv[msgs].flags = I2C_M_READ;
|
||||
msgv[msgs].buffer = recv;
|
||||
msgv[msgs].length = recv_len;
|
||||
msgs++;
|
||||
}
|
||||
|
||||
if (msgs == 0)
|
||||
return -EINVAL;
|
||||
|
||||
ret = I2C_TRANSFER(_dev, &msgv[0], msgs);
|
||||
|
||||
/* success */
|
||||
if (ret == PX4_OK)
|
||||
break;
|
||||
|
||||
/* if we have already retried once, or we are going to give up, then reset the bus */
|
||||
if ((retry_count >= 1) || (retry_count >= _retries))
|
||||
px4_i2creset(_dev);
|
||||
|
||||
} while (retry_count++ < _retries);
|
||||
|
||||
return ret;
|
||||
|
||||
}
|
||||
|
||||
int
|
||||
I2C::transfer(px4_i2c_msg_t *msgv, unsigned msgs)
|
||||
{
|
||||
int ret;
|
||||
unsigned retry_count = 0;
|
||||
|
||||
/* force the device address into the message vector */
|
||||
for (unsigned i = 0; i < msgs; i++)
|
||||
msgv[i].addr = _address;
|
||||
|
||||
|
||||
do {
|
||||
ret = I2C_TRANSFER(_dev, msgv, msgs);
|
||||
|
||||
/* success */
|
||||
if (ret == PX4_OK)
|
||||
break;
|
||||
|
||||
/* if we have already retried once, or we are going to give up, then reset the bus */
|
||||
if ((retry_count >= 1) || (retry_count >= _retries))
|
||||
px4_i2creset(_dev);
|
||||
|
||||
} while (retry_count++ < _retries);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
} // namespace device
|
||||
@@ -37,14 +37,15 @@
|
||||
|
||||
ifeq ($(PX4_TARGET_OS),nuttx)
|
||||
SRCS = \
|
||||
device.cpp \
|
||||
cdev.cpp \
|
||||
i2c.cpp \
|
||||
device_nuttx.cpp \
|
||||
cdev_nuttx.cpp \
|
||||
i2c_nuttx.cpp \
|
||||
pio.cpp \
|
||||
spi.cpp
|
||||
endif
|
||||
ifeq ($(PX4_TARGET_OS),linux)
|
||||
SRCS = vcdev.cpp \
|
||||
vdevice.cpp \
|
||||
vcdev_posix.cpp
|
||||
device.cpp \
|
||||
vcdev_posix.cpp \
|
||||
i2c_linux.cpp
|
||||
endif
|
||||
|
||||
@@ -503,7 +503,7 @@ RingBuffer::resize(unsigned new_size)
|
||||
void
|
||||
RingBuffer::print_info(const char *name)
|
||||
{
|
||||
printf("%s %u/%u (%u/%u @ %p)\n",
|
||||
printf("%s %u/%lu (%u/%u @ %p)\n",
|
||||
name,
|
||||
_num_items,
|
||||
_num_items * _item_size,
|
||||
|
||||
@@ -42,7 +42,7 @@
|
||||
|
||||
#include "device.h"
|
||||
|
||||
#include <nuttx/spi.h>
|
||||
#include <px4_spi.h>
|
||||
|
||||
namespace device __EXPORT
|
||||
{
|
||||
|
||||
@@ -1,107 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2014 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 device.cpp
|
||||
*
|
||||
* Fundamental driver base class for the virtual device framework.
|
||||
*/
|
||||
|
||||
#include "device.h"
|
||||
|
||||
#include <px4_defines.h>
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
|
||||
namespace device
|
||||
{
|
||||
|
||||
Device::Device(const char *name) :
|
||||
// public
|
||||
// protected
|
||||
_name(name),
|
||||
_debug_enabled(false)
|
||||
{
|
||||
sem_init(&_lock, 0, 1);
|
||||
|
||||
/* setup a default device ID. When bus_type is UNKNOWN the
|
||||
other fields are invalid */
|
||||
_device_id.devid = 0;
|
||||
_device_id.devid_s.bus_type = DeviceBusType_UNKNOWN;
|
||||
_device_id.devid_s.bus = 0;
|
||||
_device_id.devid_s.address = 0;
|
||||
_device_id.devid_s.devtype = 0;
|
||||
}
|
||||
|
||||
Device::~Device()
|
||||
{
|
||||
sem_destroy(&_lock);
|
||||
}
|
||||
|
||||
int
|
||||
Device::init()
|
||||
{
|
||||
int ret = OK;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void
|
||||
Device::log(const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
|
||||
printf("[%s] ", _name);
|
||||
va_start(ap, fmt);
|
||||
vprintf(fmt, ap);
|
||||
va_end(ap);
|
||||
printf("\n");
|
||||
fflush(stdout);
|
||||
}
|
||||
|
||||
void
|
||||
Device::debug(const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
|
||||
if (_debug_enabled) {
|
||||
printf("<%s> ", _name);
|
||||
va_start(ap, fmt);
|
||||
vprintf(fmt, ap);
|
||||
va_end(ap);
|
||||
printf("\n");
|
||||
fflush(stdout);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace device
|
||||
@@ -97,6 +97,39 @@ public:
|
||||
*/
|
||||
virtual int init();
|
||||
|
||||
/**
|
||||
* Read directly from the device.
|
||||
*
|
||||
* The actual size of each unit quantity is device-specific.
|
||||
*
|
||||
* @param offset The device address at which to start reading
|
||||
* @param data The buffer into which the read values should be placed.
|
||||
* @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);
|
||||
|
||||
/**
|
||||
* Write directly to the device.
|
||||
*
|
||||
* The actual size of each unit quantity is device-specific.
|
||||
*
|
||||
* @param address The device address at which to start writing.
|
||||
* @param data The buffer from which values should be read.
|
||||
* @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);
|
||||
|
||||
/**
|
||||
* Perform a device-specific operation.
|
||||
*
|
||||
* @param operation The operation to perform.
|
||||
* @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);
|
||||
|
||||
/*
|
||||
device bus types for DEVID
|
||||
*/
|
||||
|
||||
Reference in New Issue
Block a user