Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 02:17:07 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Support for building more modules with Linux

Browse Source 
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
2015-03-13 23:36:46 -07:00
parent 2abfb7a5be
commit d013ac0927
53 changed files with 8455 additions and 354 deletions
Download Patch File Download Diff File Expand all files Collapse all files
makefiles/linux/config_linux_default.mk
+4 -4
View File
@@ -11,7 +11,8 @@
# Board support modules
#
MODULES += drivers/device
#MODULES += modules/sensors
MODULES += modules/sensors
MODULES += drivers/ms5611
#
# System commands
@@ -21,8 +22,7 @@ MODULES += drivers/device
#
# General system control
#
#MODULES += modules/mavlink
#MODULES += modules/mavlink
MODULES += modules/mavlink
#
# Estimation modules (EKF/ SO3 / other filters)
@@ -48,7 +48,7 @@ MODULES += modules/dataman
MODULES += lib/mathlib
MODULES += lib/geo
MODULES += lib/geo_lookup
#MODULES += lib/conversion
MODULES += lib/conversion
#
# Linux port
makefiles/toolchain_native.mk
+3 -2
View File
@@ -81,7 +81,8 @@ CPP = clang$(CLANGVER) -E
DEV_VER_SUPPORTED = 4.2.1
endif
LD = ld.gold
#LD = ld.gold
LD = ld
AR = ar rcs
NM = nm
OBJCOPY = objcopy
@@ -286,7 +287,7 @@ endef
define PRELINK
@$(ECHO) "PRELINK: $1"
@$(MKDIR) -p $(dir $1)
$(Q) $(LD) -r -o $1 $2
$(Q) $(LD) -Ur -o $1 $2
endef
# $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1
src/drivers/device/cdev.cpp → src/drivers/device/cdev_nuttx.cpp
View File
src/drivers/device/device.cpp
+16 -143
View File
@@ -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
src/drivers/device/device_nuttx.cpp
+257
View File
@@ -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
src/drivers/device/i2c.h
+3 -3
View File
@@ -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 &);
src/drivers/device/i2c_linux.cpp
+257
View File
@@ -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
src/drivers/device/i2c.cpp → src/drivers/device/i2c_nuttx.cpp
View File
src/drivers/device/module.mk
+6 -5
View File
@@ -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
src/drivers/device/ringbuffer.h
+1 -1
View File
@@ -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,
src/drivers/device/spi.h
+1 -1
View File
@@ -42,7 +42,7 @@
#include "device.h"
#include <nuttx/spi.h>
#include <px4_spi.h>
namespace device __EXPORT
{
src/drivers/device/vdevice.h
+33
View File
@@ -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
*/
src/drivers/drv_accel.h
+1 -1
View File
@@ -94,7 +94,7 @@ ORB_DECLARE(sensor_accel);
*/
#define _ACCELIOCBASE (0x2100)
#define _ACCELIOC(_n) (_IOC(_ACCELIOCBASE, _n))
#define _ACCELIOC(_n) (_PX4_IOC(_ACCELIOCBASE, _n))
/** set the accel internal sample rate to at least (arg) Hz */
src/drivers/drv_airspeed.h
+1 -1
View File
@@ -59,7 +59,7 @@
*/
#define _AIRSPEEDIOCBASE (0x7700)
#define __AIRSPEEDIOC(_n) (_IOC(_AIRSPEEDIOCBASE, _n))
#define __AIRSPEEDIOC(_n) (_PX4_IOC(_AIRSPEEDIOCBASE, _n))
#define AIRSPEEDIOCSSCALE __AIRSPEEDIOC(0)
#define AIRSPEEDIOCGSCALE __AIRSPEEDIOC(1)
src/drivers/drv_baro.h
+2 -1
View File
@@ -40,6 +40,7 @@
#ifndef _DRV_BARO_H
#define _DRV_BARO_H
#include <px4_defines.h>
#include <stdint.h>
#include <sys/ioctl.h>
@@ -71,7 +72,7 @@ ORB_DECLARE(sensor_baro);
*/
#define _BAROIOCBASE (0x2200)
#define _BAROIOC(_n) (_IOC(_BAROIOCBASE, _n))
#define _BAROIOC(_n) (_PX4_IOC(_BAROIOCBASE, _n))
/** set corrected MSL pressure in pascals */
#define BAROIOCSMSLPRESSURE _BAROIOC(0)
src/drivers/drv_device.h
+1 -1
View File
@@ -51,7 +51,7 @@
*/
#define _DEVICEIOCBASE (0x100)
#define _DEVICEIOC(_n) (_IOC(_DEVICEIOCBASE, _n))
#define _DEVICEIOC(_n) (_PX4_IOC(_DEVICEIOCBASE, _n))
/** ask device to stop publishing */
#define DEVIOCSPUBBLOCK _DEVICEIOC(0)
src/drivers/drv_gyro.h
+1 -1
View File
@@ -91,7 +91,7 @@ ORB_DECLARE(sensor_gyro);
*/
#define _GYROIOCBASE (0x2300)
#define _GYROIOC(_n) (_IOC(_GYROIOCBASE, _n))
#define _GYROIOC(_n) (_PX4_IOC(_GYROIOCBASE, _n))
/** set the gyro internal sample rate to at least (arg) Hz */
#define GYROIOCSSAMPLERATE _GYROIOC(0)
src/drivers/drv_mag.h
+1 -1
View File
@@ -91,7 +91,7 @@ ORB_DECLARE(sensor_mag);
*/
#define _MAGIOCBASE (0x2400)
#define _MAGIOC(_n) (_IOC(_MAGIOCBASE, _n))
#define _MAGIOC(_n) (_PX4_IOC(_MAGIOCBASE, _n))
/** set the mag internal sample rate to at least (arg) Hz */
#define MAGIOCSSAMPLERATE _MAGIOC(0)
src/drivers/drv_sensor.h
+2 -1
View File
@@ -40,6 +40,7 @@
#ifndef _DRV_SENSOR_H
#define _DRV_SENSOR_H
#include <px4_defines.h>
#include <stdint.h>
#include <sys/ioctl.h>
@@ -69,7 +70,7 @@
*/
#define _SENSORIOCBASE (0x2000)
#define _SENSORIOC(_n) (_IOC(_SENSORIOCBASE, _n))
#define _SENSORIOC(_n) (_PX4_IOC(_SENSORIOCBASE, _n))
/**
* Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE
src/drivers/ms5611/module.mk
+5 -1
View File
@@ -37,6 +37,10 @@
MODULE_COMMAND = ms5611
SRCS = ms5611.cpp ms5611_spi.cpp ms5611_i2c.cpp
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
endif
MAXOPTIMIZATION = -Os
src/drivers/ms5611/ms5611.h
+3 -3
View File
@@ -80,6 +80,6 @@ extern bool crc4(uint16_t *n_prom);
} /* namespace */
/* interface factories */
extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function;
extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function;
typedef device::Device *(*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum);
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));
typedef device::Device* (*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum);
src/drivers/ms5611/ms5611_i2c.cpp
+16 -15
View File
@@ -39,12 +39,13 @@
/* 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 <debug.h>
#include <errno.h>
#include <unistd.h>
@@ -70,8 +71,8 @@ public:
virtual ~MS5611_I2C();
virtual int init();
virtual int read(unsigned offset, void *data, unsigned count);
virtual int ioctl(unsigned operation, unsigned &arg);
virtual int dev_read(unsigned offset, void *data, unsigned count);
virtual int dev_ioctl(unsigned operation, unsigned &arg);
protected:
virtual int probe();
@@ -98,7 +99,7 @@ private:
/**
* Read the MS5611 PROM
*
* @return OK if the PROM reads successfully.
* @return PX4_OK if the PROM reads successfully.
*/
int _read_prom();
@@ -128,7 +129,7 @@ MS5611_I2C::init()
}
int
MS5611_I2C::read(unsigned offset, void *data, unsigned count)
MS5611_I2C::dev_read(unsigned offset, void *data, unsigned count)
{
union _cvt {
uint8_t b[4];
@@ -139,7 +140,7 @@ MS5611_I2C::read(unsigned offset, void *data, unsigned count)
/* read the most recent measurement */
uint8_t cmd = 0;
int ret = transfer(&cmd, 1, &buf[0], 3);
if (ret == OK) {
if (ret == PX4_OK) {
/* fetch the raw value */
cvt->b[0] = buf[2];
cvt->b[1] = buf[1];
@@ -151,7 +152,7 @@ MS5611_I2C::read(unsigned offset, void *data, unsigned count)
}
int
MS5611_I2C::ioctl(unsigned operation, unsigned &arg)
MS5611_I2C::dev_ioctl(unsigned operation, unsigned &arg)
{
int ret;
@@ -176,14 +177,14 @@ MS5611_I2C::probe()
{
_retries = 10;
if ((OK == _probe_address(MS5611_ADDRESS_1)) ||
(OK == _probe_address(MS5611_ADDRESS_2))) {
if ((PX4_OK == _probe_address(MS5611_ADDRESS_1)) ||
(PX4_OK == _probe_address(MS5611_ADDRESS_2))) {
/*
* Disable retries; we may enable them selectively in some cases,
* but the device gets confused if we retry some of the commands.
*/
_retries = 0;
return OK;
return PX4_OK;
}
return -EIO;
@@ -196,14 +197,14 @@ MS5611_I2C::_probe_address(uint8_t address)
set_address(address);
/* send reset command */
if (OK != _reset())
if (PX4_OK != _reset())
return -EIO;
/* read PROM */
if (OK != _read_prom())
if (PX4_OK != _read_prom())
return -EIO;
return OK;
return PX4_OK;
}
@@ -254,7 +255,7 @@ MS5611_I2C::_read_prom()
for (int i = 0; i < 8; i++) {
uint8_t cmd = ADDR_PROM_SETUP + (i * 2);
if (OK != transfer(&cmd, 1, &prom_buf[0], 2))
if (PX4_OK != transfer(&cmd, 1, &prom_buf[0], 2))
break;
/* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */
@@ -264,5 +265,5 @@ MS5611_I2C::_read_prom()
}
/* calculate CRC and return success/failure accordingly */
return ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
return ms5611::crc4(&_prom.c[0]) ? PX4_OK : -EIO;
}
src/drivers/ms5611/ms5611_linux.cpp
+1215
View File
File diff suppressed because it is too large Load Diff
src/drivers/ms5611/ms5611.cpp → src/drivers/ms5611/ms5611_nuttx.cpp
View File
src/drivers/ms5611/ms5611_spi.cpp
+1 -1
View File
@@ -44,7 +44,7 @@
#include <stdint.h>
#include <stdbool.h>
#include <assert.h>
#include <debug.h>
//#include <debug.h>
#include <errno.h>
#include <unistd.h>
src/modules/mavlink/mavlink_ftp.h
+1 -1
View File
@@ -39,7 +39,7 @@
#include <dirent.h>
#include <queue.h>
#include <nuttx/wqueue.h>
#include <px4_workqueue.h>
#include <systemlib/err.h>
#include "mavlink_messages.h"
src/modules/mavlink/mavlink_ftp_linux.cpp
+861
View File
File diff suppressed because it is too large Load Diff
src/modules/mavlink/mavlink_main.h
+9 -10
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -30,17 +30,9 @@
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mavlink_main.h
* MAVLink 1.0 protocol interface definition.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#pragma once
<<<<<<< HEAD
#include <stdbool.h>
#include <nuttx/fs/fs.h>
#include <systemlib/param/param.h>
@@ -413,3 +405,10 @@ private:
Mavlink(const Mavlink&);
Mavlink operator=(const Mavlink&);
};
=======
#ifdef __PX4_NUTTX
#include "mavlink_main_nuttx.h"
#else
#include "mavlink_main_linux.h"
#endif
>>>>>>> Support for building more modules with Linux
src/modules/mavlink/mavlink_main_linux.cpp
+1795
View File
File diff suppressed because it is too large Load Diff
src/modules/mavlink/mavlink_main_linux.h
+417
View File
@@ -0,0 +1,417 @@
/****************************************************************************
*
* 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 mavlink_main_linux.h
* MAVLink 1.0 protocol interface definition.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Mark Charlebois <charlebm@gmail.com>
*/
#pragma once
#include <stdbool.h>
#include <drivers/device/device.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <pthread.h>
#include <mavlink/mavlink_log.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/telemetry_status.h>
#include "mavlink_bridge_header.h"
#include "mavlink_orb_subscription.h"
#include "mavlink_stream.h"
#include "mavlink_messages.h"
#include "mavlink_mission.h"
#include "mavlink_parameters.h"
class Mavlink : public device::CDev
{
public:
/**
* Constructor
*/
Mavlink();
/**
* Destructor, also kills the mavlinks task.
*/
~Mavlink();
/**
* Start the mavlink task.
*
* @return OK on success.
*/
static int start(int argc, char *argv[]);
/**
* Display the mavlink status.
*/
void display_status();
static int stream_command(int argc, char *argv[]);
static int instance_count();
static Mavlink *new_instance();
static Mavlink *get_instance(unsigned instance);
static Mavlink *get_instance_for_device(const char *device_name);
static int destroy_all_instances();
static int get_status_all_instances();
static bool instance_exists(const char *device_name, Mavlink *self);
static void forward_message(const mavlink_message_t *msg, Mavlink *self);
static int get_uart_fd(unsigned index);
int get_uart_fd();
/**
* Get the MAVLink system id.
*
* @return The system ID of this vehicle
*/
int get_system_id();
/**
* Get the MAVLink component id.
*
* @return The component ID of this vehicle
*/
int get_component_id();
const char *_device_name;
enum MAVLINK_MODE {
MAVLINK_MODE_NORMAL = 0,
MAVLINK_MODE_CUSTOM,
MAVLINK_MODE_ONBOARD
};
void set_mode(enum MAVLINK_MODE);
enum MAVLINK_MODE get_mode() { return _mode; }
bool get_hil_enabled() { return _hil_enabled; }
bool get_use_hil_gps() { return _use_hil_gps; }
bool get_forward_externalsp() { return _forward_externalsp; }
bool get_flow_control_enabled() { return _flow_control_enabled; }
bool get_forwarding_on() { return _forwarding_on; }
static int start_helper(int argc, char *argv[]);
/**
* Handle parameter related messages.
*/
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
/**
* Enable / disable Hardware in the Loop simulation mode.
*
* @param hil_enabled The new HIL enable/disable state.
* @return OK if the HIL state changed, ERROR if the
* requested change could not be made or was
* redundant.
*/
int set_hil_enabled(bool hil_enabled);
void send_message(const uint8_t msgid, const void *msg);
/**
* Resend message as is, don't change sequence number and CRC.
*/
void resend_message(mavlink_message_t *msg);
void handle_message(const mavlink_message_t *msg);
MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance=0);
int get_instance_id();
/**
* Enable / disable hardware flow control.
*
* @param enabled True if hardware flow control should be enabled
*/
int enable_flow_control(bool enabled);
mavlink_channel_t get_channel();
void configure_stream_threadsafe(const char *stream_name, float rate);
bool _task_should_exit; /**< if true, mavlink task should exit */
int get_mavlink_fd() { return _mavlink_fd; }
/**
* Send a status text with loglevel INFO
*
* @param string the message to send (will be capped by mavlink max string length)
*/
void send_statustext_info(const char *string);
/**
* Send a status text with loglevel CRITICAL
*
* @param string the message to send (will be capped by mavlink max string length)
*/
void send_statustext_critical(const char *string);
/**
* Send a status text with loglevel EMERGENCY
*
* @param string the message to send (will be capped by mavlink max string length)
*/
void send_statustext_emergency(const char *string);
/**
* Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent
* only on this mavlink connection. Useful for reporting communication specific, not system-wide info
* only to client interested in it. Message will be not sent immediately but queued in buffer as
* for mavlink_log_xxx().
*
* @param string the message to send (will be capped by mavlink max string length)
* @param severity the log level
*/
void send_statustext(unsigned char severity, const char *string);
MavlinkStream * get_streams() const { return _streams; }
float get_rate_mult();
/* Functions for waiting to start transmission until message received. */
void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
bool get_has_received_messages() { return _received_messages; }
void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
bool get_wait_to_transmit() { return _wait_to_transmit; }
bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
bool message_buffer_write(const void *ptr, int size);
void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
/**
* Count a transmision error
*/
void count_txerr();
/**
* Count transmitted bytes
*/
void count_txbytes(unsigned n) { _bytes_tx += n; };
/**
* Count bytes not transmitted because of errors
*/
void count_txerrbytes(unsigned n) { _bytes_txerr += n; };
/**
* Count received bytes
*/
void count_rxbytes(unsigned n) { _bytes_rx += n; };
/**
* Get the receive status of this MAVLink link
*/
struct telemetry_status_s& get_rx_status() { return _rstatus; }
struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; }
unsigned get_system_type() { return _system_type; }
protected:
Mavlink *next;
private:
int _instance_id;
int _mavlink_fd;
bool _task_running;
/* states */
bool _hil_enabled; /**< Hardware In the Loop mode */
bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */
bool _is_usb_uart; /**< Port is USB */
bool _wait_to_transmit; /**< Wait to transmit until received messages. */
bool _received_messages; /**< Whether we've received valid mavlink messages. */
unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */
MavlinkOrbSubscription *_subscriptions;
MavlinkStream *_streams;
MavlinkMissionManager *_mission_manager;
MavlinkParametersManager *_parameters_manager;
MAVLINK_MODE _mode;
mavlink_channel_t _channel;
struct mavlink_logbuffer _logbuffer;
unsigned int _total_counter;
pthread_t _receive_thread;
bool _verbose;
bool _forwarding_on;
bool _passing_on;
bool _ftp_on;
int _uart_fd;
int _baudrate;
int _datarate; ///< data rate for normal streams (attitude, position, etc.)
int _datarate_events; ///< data rate for params, waypoints, text messages
float _rate_mult;
/**
* If the queue index is not at 0, the queue sending
* logic will send parameters from the current index
* to len - 1, the end of the param list.
*/
unsigned int _mavlink_param_queue_index;
bool mavlink_link_termination_allowed;
char *_subscribe_to_stream;
float _subscribe_to_stream_rate;
bool _flow_control_enabled;
uint64_t _last_write_success_time;
uint64_t _last_write_try_time;
unsigned _bytes_tx;
unsigned _bytes_txerr;
unsigned _bytes_rx;
uint64_t _bytes_timestamp;
float _rate_tx;
float _rate_txerr;
float _rate_rx;
struct telemetry_status_s _rstatus; ///< receive status
struct mavlink_message_buffer {
int write_ptr;
int read_ptr;
int size;
char *data;
};
mavlink_message_buffer _message_buffer;
pthread_mutex_t _message_buffer_mutex;
pthread_mutex_t _send_mutex;
bool _param_initialized;
param_t _param_system_id;
param_t _param_component_id;
param_t _param_system_type;
param_t _param_use_hil_gps;
param_t _param_forward_externalsp;
unsigned _system_type;
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _txerr_perf; /**< TX error counter */
void mavlink_update_system();
int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
/**
* Get the free space in the transmit buffer
*
* @return free space in the UART TX buffer
*/
unsigned get_free_tx_buf();
static unsigned int interval_from_rate(float rate);
int configure_stream(const char *stream_name, const float rate);
/**
* Adjust the stream rates based on the current rate
*
* @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease
*/
void adjust_stream_rates(const float multiplier);
int message_buffer_init(int size);
void message_buffer_destroy();
int message_buffer_count();
int message_buffer_is_empty();
int message_buffer_get_ptr(void **ptr, bool *is_part);
void message_buffer_mark_read(int n);
void pass_message(const mavlink_message_t *msg);
/**
* Update rate mult so total bitrate will be equal to _datarate.
*/
void update_rate_mult();
virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg);
static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
/**
* Main mavlink task.
*/
int task_main(int argc, char *argv[]);
/* do not allow copying this class */
Mavlink(const Mavlink&);
Mavlink operator=(const Mavlink&);
};
src/modules/mavlink/mavlink_main_nuttx.h
+415
View File
@@ -0,0 +1,415 @@
/****************************************************************************
*
* 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 mavlink_main_nuttx.h
* MAVLink 1.0 protocol interface definition.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#pragma once
#include <stdbool.h>
#include <nuttx/fs/fs.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <pthread.h>
#include <mavlink/mavlink_log.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/telemetry_status.h>
#include "mavlink_bridge_header.h"
#include "mavlink_orb_subscription.h"
#include "mavlink_stream.h"
#include "mavlink_messages.h"
#include "mavlink_mission.h"
#include "mavlink_parameters.h"
class Mavlink
{
public:
/**
* Constructor
*/
Mavlink();
/**
* Destructor, also kills the mavlinks task.
*/
~Mavlink();
/**
* Start the mavlink task.
*
* @return OK on success.
*/
static int start(int argc, char *argv[]);
/**
* Display the mavlink status.
*/
void display_status();
static int stream_command(int argc, char *argv[]);
static int instance_count();
static Mavlink *new_instance();
static Mavlink *get_instance(unsigned instance);
static Mavlink *get_instance_for_device(const char *device_name);
static int destroy_all_instances();
static int get_status_all_instances();
static bool instance_exists(const char *device_name, Mavlink *self);
static void forward_message(const mavlink_message_t *msg, Mavlink *self);
static int get_uart_fd(unsigned index);
int get_uart_fd();
/**
* Get the MAVLink system id.
*
* @return The system ID of this vehicle
*/
int get_system_id();
/**
* Get the MAVLink component id.
*
* @return The component ID of this vehicle
*/
int get_component_id();
const char *_device_name;
enum MAVLINK_MODE {
MAVLINK_MODE_NORMAL = 0,
MAVLINK_MODE_CUSTOM,
MAVLINK_MODE_ONBOARD
};
void set_mode(enum MAVLINK_MODE);
enum MAVLINK_MODE get_mode() { return _mode; }
bool get_hil_enabled() { return _hil_enabled; }
bool get_use_hil_gps() { return _use_hil_gps; }
bool get_forward_externalsp() { return _forward_externalsp; }
bool get_flow_control_enabled() { return _flow_control_enabled; }
bool get_forwarding_on() { return _forwarding_on; }
/**
* Get the free space in the transmit buffer
*
* @return free space in the UART TX buffer
*/
unsigned get_free_tx_buf();
static int start_helper(int argc, char *argv[]);
/**
* Handle parameter related messages.
*/
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
/**
* Enable / disable Hardware in the Loop simulation mode.
*
* @param hil_enabled The new HIL enable/disable state.
* @return OK if the HIL state changed, ERROR if the
* requested change could not be made or was
* redundant.
*/
int set_hil_enabled(bool hil_enabled);
void send_message(const uint8_t msgid, const void *msg, uint8_t component_ID = 0);
/**
* Resend message as is, don't change sequence number and CRC.
*/
void resend_message(mavlink_message_t *msg);
void handle_message(const mavlink_message_t *msg);
MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance=0);
int get_instance_id();
/**
* Enable / disable hardware flow control.
*
* @param enabled True if hardware flow control should be enabled
*/
int enable_flow_control(bool enabled);
mavlink_channel_t get_channel();
void configure_stream_threadsafe(const char *stream_name, float rate);
bool _task_should_exit; /**< if true, mavlink task should exit */
int get_mavlink_fd() { return _mavlink_fd; }
/**
* Send a status text with loglevel INFO
*
* @param string the message to send (will be capped by mavlink max string length)
*/
void send_statustext_info(const char *string);
/**
* Send a status text with loglevel CRITICAL
*
* @param string the message to send (will be capped by mavlink max string length)
*/
void send_statustext_critical(const char *string);
/**
* Send a status text with loglevel EMERGENCY
*
* @param string the message to send (will be capped by mavlink max string length)
*/
void send_statustext_emergency(const char *string);
/**
* Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent
* only on this mavlink connection. Useful for reporting communication specific, not system-wide info
* only to client interested in it. Message will be not sent immediately but queued in buffer as
* for mavlink_log_xxx().
*
* @param string the message to send (will be capped by mavlink max string length)
* @param severity the log level
*/
void send_statustext(unsigned char severity, const char *string);
MavlinkStream * get_streams() const { return _streams; }
float get_rate_mult();
/* Functions for waiting to start transmission until message received. */
void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
bool get_has_received_messages() { return _received_messages; }
void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
bool get_wait_to_transmit() { return _wait_to_transmit; }
bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
bool message_buffer_write(const void *ptr, int size);
void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
/**
* Count a transmision error
*/
void count_txerr();
/**
* Count transmitted bytes
*/
void count_txbytes(unsigned n) { _bytes_tx += n; };
/**
* Count bytes not transmitted because of errors
*/
void count_txerrbytes(unsigned n) { _bytes_txerr += n; };
/**
* Count received bytes
*/
void count_rxbytes(unsigned n) { _bytes_rx += n; };
/**
* Get the receive status of this MAVLink link
*/
struct telemetry_status_s& get_rx_status() { return _rstatus; }
struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; }
unsigned get_system_type() { return _system_type; }
protected:
Mavlink *next;
private:
int _instance_id;
int _mavlink_fd;
bool _task_running;
/* states */
bool _hil_enabled; /**< Hardware In the Loop mode */
bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */
bool _is_usb_uart; /**< Port is USB */
bool _wait_to_transmit; /**< Wait to transmit until received messages. */
bool _received_messages; /**< Whether we've received valid mavlink messages. */
unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */
MavlinkOrbSubscription *_subscriptions;
MavlinkStream *_streams;
MavlinkMissionManager *_mission_manager;
MavlinkParametersManager *_parameters_manager;
MAVLINK_MODE _mode;
mavlink_channel_t _channel;
struct mavlink_logbuffer _logbuffer;
unsigned int _total_counter;
pthread_t _receive_thread;
bool _verbose;
bool _forwarding_on;
bool _passing_on;
bool _ftp_on;
int _uart_fd;
int _baudrate;
int _datarate; ///< data rate for normal streams (attitude, position, etc.)
int _datarate_events; ///< data rate for params, waypoints, text messages
float _rate_mult;
/**
* If the queue index is not at 0, the queue sending
* logic will send parameters from the current index
* to len - 1, the end of the param list.
*/
unsigned int _mavlink_param_queue_index;
bool mavlink_link_termination_allowed;
char *_subscribe_to_stream;
float _subscribe_to_stream_rate;
bool _flow_control_enabled;
uint64_t _last_write_success_time;
uint64_t _last_write_try_time;
unsigned _bytes_tx;
unsigned _bytes_txerr;
unsigned _bytes_rx;
uint64_t _bytes_timestamp;
float _rate_tx;
float _rate_txerr;
float _rate_rx;
struct telemetry_status_s _rstatus; ///< receive status
struct mavlink_message_buffer {
int write_ptr;
int read_ptr;
int size;
char *data;
};
mavlink_message_buffer _message_buffer;
pthread_mutex_t _message_buffer_mutex;
pthread_mutex_t _send_mutex;
bool _param_initialized;
param_t _param_system_id;
param_t _param_component_id;
param_t _param_system_type;
param_t _param_use_hil_gps;
param_t _param_forward_externalsp;
unsigned _system_type;
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _txerr_perf; /**< TX error counter */
void mavlink_update_system();
int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
static unsigned int interval_from_rate(float rate);
int configure_stream(const char *stream_name, const float rate);
/**
* Adjust the stream rates based on the current rate
*
* @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease
*/
void adjust_stream_rates(const float multiplier);
int message_buffer_init(int size);
void message_buffer_destroy();
int message_buffer_count();
int message_buffer_is_empty();
int message_buffer_get_ptr(void **ptr, bool *is_part);
void message_buffer_mark_read(int n);
void pass_message(const mavlink_message_t *msg);
/**
* Update rate mult so total bitrate will be equal to _datarate.
*/
void update_rate_mult();
static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
/**
* Main mavlink task.
*/
int task_main(int argc, char *argv[]);
/* do not allow copying this class */
Mavlink(const Mavlink&);
Mavlink operator=(const Mavlink&);
};
src/modules/mavlink/mavlink_receiver.cpp
+4 -5
View File
@@ -59,7 +59,6 @@
#include <time.h>
#include <float.h>
#include <unistd.h>
#include <nuttx/sched.h>
#include <sys/prctl.h>
#include <termios.h>
#include <errno.h>
@@ -992,7 +991,7 @@ MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg)
clock_gettime(CLOCK_REALTIME, &tv);
// date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009
bool onb_unix_valid = tv.tv_sec > PX4_EPOCH_SECS;
bool onb_unix_valid = (unsigned long long)tv.tv_sec > PX4_EPOCH_SECS;
bool ofb_unix_valid = time.time_unix_usec > PX4_EPOCH_SECS * 1000ULL;
if (!onb_unix_valid && ofb_unix_valid) {
@@ -1496,17 +1495,17 @@ MavlinkReceiver::receive_thread(void *arg)
sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id());
prctl(PR_SET_NAME, thread_name, getpid());
struct pollfd fds[1];
px4_pollfd_struct_t fds[1];
fds[0].fd = uart_fd;
fds[0].events = POLLIN;
ssize_t nread = 0;
while (!_mavlink->_task_should_exit) {
if (poll(fds, 1, timeout) > 0) {
if (px4_poll(fds, 1, timeout) > 0) {
/* non-blocking read. read may return negative values */
if ((nread = read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) {
if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) {
/* to avoid reading very small chunks wait for data before reading */
usleep(1000);
}
src/modules/mavlink/module.mk
+10 -4
View File
@@ -36,16 +36,22 @@
#
MODULE_COMMAND = mavlink
SRCS += mavlink_main.cpp \
mavlink.c \
ifeq ($(PX$_TARGET_OS),nuttx)
SRCS += mavlink_main_nuttx.cpp \
mavlink_ftp_nuttx.cpp
else
SRCS += mavlink_main_linux.cpp \
mavlink_ftp_linux.cpp
endif
SRCS += mavlink.c \
mavlink_receiver.cpp \
mavlink_mission.cpp \
mavlink_parameters.cpp \
mavlink_orb_subscription.cpp \
mavlink_messages.cpp \
mavlink_stream.cpp \
mavlink_rate_limiter.cpp \
mavlink_ftp.cpp
mavlink_rate_limiter.cpp
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
src/modules/sensors/module.mk
+6 -2
View File
@@ -38,8 +38,12 @@
MODULE_COMMAND = sensors
MODULE_PRIORITY = "SCHED_PRIORITY_MAX-5"
SRCS = sensors.cpp \
sensor_params.c
ifeq ($(PX$_TARGET_OS),nuttx)
SRCS = sensors_nuttx.cpp
else
SRCS = sensors_linux.cpp
endif
SRCS += sensor_params.c
MODULE_STACKSIZE = 1200
src/modules/sensors/sensors_linux.cpp
+2308
View File
File diff suppressed because it is too large Load Diff
src/modules/sensors/sensors.cpp → src/modules/sensors/sensors_nuttx.cpp
View File
src/platforms/linux/include/queue.h
-8
View File
@@ -96,14 +96,6 @@ struct dq_queue_s
};
typedef struct dq_queue_s dq_queue_t;
// FIXME - hack for mavlink until kernel work queue can be removed
#define LPWORK 1
typedef struct {
dq_entry_t dq;
} work_s;
inline void work_queue(int worktype, work_s *dummy, void (*_worker_trampoline)(void *arg), void *req, int dummy2);
inline void work_queue(int worktype, work_s *dummy, void (*_worker_trampoline)(void *arg), void *req, int dummy2) {}
/************************************************************************
* Global Function Prototypes
************************************************************************/
src/platforms/linux/main.cpp
+38 -21
View File
@@ -41,6 +41,7 @@
#include <fstream>
#include <string>
#include <sstream>
#include <vector>
using namespace std;
@@ -48,16 +49,22 @@ typedef int (*px4_main_t)(int argc, char *argv[]);
#include "apps.h"
// FIXME - the code below only passes 1 arg for now
void run_cmd(const string &command, const string &apparg);
void run_cmd(const string &command, const string &apparg) {
const char *arg[3];
void run_cmd(const vector<string> &appargs);
void run_cmd(const vector<string> &appargs) {
// command is appargs[0]
string command = appargs[0];
cout << "appargs.size() = " << appargs.size() << endl;
if (apps.find(command) != apps.end()) {
arg[0] = (char *)command.c_str();
arg[1] = (char *)apparg.c_str();
arg[2] = (char *)0;
apps[command](2,(char **)arg);
const char *arg[appargs.size()+2];
unsigned int i = 0;
while (i < appargs.size() && appargs[i] != "") {
arg[i] = (char *)appargs[i].c_str();
++i;
}
arg[i] = (char *)0;
cout << command << " " << i << endl;
apps[command](i,(char **)arg);
}
else
{
@@ -65,12 +72,22 @@ void run_cmd(const string &command, const string &apparg) {
}
}
void process_line(string &line)
{
vector<string> appargs(5);
stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4];
cout << "Command " << appargs[0] << endl;
unsigned int i = 1;
while ( i < appargs.size() && appargs[i] != "") {
cout << " appargs[" << i << "] = " << appargs[i] << endl;
++i;
}
run_cmd(appargs);
}
int main(int argc, char **argv)
{
string mystr;
string command;
string apparg;
// Execute a command list of provided
if (argc == 2) {
ifstream infile(argv[1]);
@@ -78,21 +95,21 @@ int main(int argc, char **argv)
//vector<string> tokens;
for (string line; getline(infile, line, '\n'); ) {
stringstream(line) >> command >> apparg;
cout << "Command " << command << ", apparg " << apparg << endl;
run_cmd(command, apparg);
process_line(line);
}
}
string mystr;
vector<string> appargs(2);
appargs[0] = "list_builtins";
while(1) {
run_cmd("list_builtins", "");
run_cmd(appargs);
cout << "Enter a command and its args:" << endl;
getline (cin,mystr);
stringstream(mystr) >> command >> apparg;
run_cmd(command, apparg);
process_line(mystr);
mystr = "";
command = "";
apparg = "";
}
}
src/platforms/linux/px4_layer/dq_addlast.c
+74
View File
@@ -0,0 +1,74 @@
/************************************************************
* libc/queue/dq_addlast.c
*
* Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* 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 NuttX 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.
*
************************************************************/
/************************************************************
* Compilation Switches
************************************************************/
/************************************************************
* Included Files
************************************************************/
#include <queue.h>
/************************************************************
* Public Functions
************************************************************/
/************************************************************
* Name: dq_addlast
*
* Description
* dq_addlast adds 'node' to the end of 'queue'
*
************************************************************/
void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue)
{
node->flink = NULL;
node->blink = queue->tail;
if (!queue->head)
{
queue->head = node;
queue->tail = node;
}
else
{
queue->tail->flink = node;
queue->tail = node;
}
}
src/platforms/linux/px4_layer/dq_remfirst.c
+82
View File
@@ -0,0 +1,82 @@
/************************************************************
* libc/queue/dq_remfirst.c
*
* Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* 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 NuttX 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.
*
************************************************************/
/************************************************************
* Compilation Switches
************************************************************/
/************************************************************
* Included Files
************************************************************/
#include <queue.h>
/************************************************************
* Public Functions
************************************************************/
/************************************************************
* Name: dq_remfirst
*
* Description:
* dq_remfirst removes 'node' from the head of 'queue'
*
************************************************************/
FAR dq_entry_t *dq_remfirst(dq_queue_t *queue)
{
FAR dq_entry_t *ret = queue->head;
if (ret)
{
FAR dq_entry_t *next = ret->flink;
if (!next)
{
queue->head = NULL;
queue->tail = NULL;
}
else
{
queue->head = next;
next->blink = NULL;
}
ret->flink = NULL;
ret->blink = NULL;
}
return ret;
}
src/platforms/linux/px4_layer/drv_hrt.c
+163 -34
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012, 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
@@ -32,14 +32,16 @@
****************************************************************************/
/**
* @file drv_hrt.h
* @file drv_hrt.c
*
* High-resolution timer with callouts and timekeeping.
*/
#include <drivers/drv_hrt.h>
#include <time.h>
#include <string.h>
static hrt_abstime dummy_timer = 0;
static struct sq_queue_s callout_queue;
/* latency histogram */
#define LATENCY_BUCKET_COUNT 8
@@ -47,12 +49,20 @@ __EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT;
__EXPORT const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1];
static void hrt_call_reschedule(void);
#define HRT_INTERVAL_MIN 50
#define HRT_INTERVAL_MAX 50000
/*
* Get absolute time.
*/
hrt_abstime hrt_absolute_time(void)
{
return dummy_timer;
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
return ts_to_abstime(&ts);
}
/*
@@ -60,7 +70,12 @@ hrt_abstime hrt_absolute_time(void)
*/
hrt_abstime ts_to_abstime(struct timespec *ts)
{
return dummy_timer;
hrt_abstime result;
result = (hrt_abstime)(ts->tv_sec) * 1000000;
result += ts->tv_nsec / 1000;
return result;
}
/*
@@ -77,7 +92,8 @@ void abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
*/
hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then)
{
return dummy_timer;
hrt_abstime delta = hrt_absolute_time() - *then;
return delta;
}
/*
@@ -87,35 +103,10 @@ hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then)
*/
hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now)
{
return dummy_timer;
hrt_abstime ts = hrt_absolute_time();
return ts;
}
/*
* Call callout(arg) after delay has elapsed.
*
* If callout is NULL, this can be used to implement a timeout by testing the call
* with hrt_called().
*/
void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
{
}
/*
* Call callout(arg) at absolute time calltime.
*/
void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
{
}
/*
* Call callout(arg) after delay, and then after every interval.
*
* Note thet the interval is timed between scheduled, not actual, call times, so the call rate may
* jitter but should not drift.
*/
void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
{
}
/*
* If this returns true, the entry has been invoked and removed from the callout list,
@@ -125,7 +116,7 @@ void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime inter
*/
bool hrt_called(struct hrt_call *entry)
{
return true;
return (entry->deadline == 0);
}
/*
@@ -133,6 +124,15 @@ bool hrt_called(struct hrt_call *entry)
*/
void hrt_cancel(struct hrt_call *entry)
{
// FIXME - need a lock
sq_rem(&entry->link, &callout_queue);
entry->deadline = 0;
/* if this is a periodic call being removed by the callout, prevent it from
* being re-entered when the callout returns.
*/
entry->period = 0;
// endif
}
/*
@@ -140,6 +140,7 @@ void hrt_cancel(struct hrt_call *entry)
*/
void hrt_call_init(struct hrt_call *entry)
{
memset(entry, 0, sizeof(*entry));
}
/*
@@ -151,6 +152,7 @@ void hrt_call_init(struct hrt_call *entry)
*/
void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
{
entry->deadline = hrt_absolute_time() + delay;
}
/*
@@ -158,5 +160,132 @@ void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
*/
void hrt_init(void)
{
sq_init(&callout_queue);
}
static void
hrt_call_enter(struct hrt_call *entry)
{
struct hrt_call *call, *next;
call = (struct hrt_call *)sq_peek(&callout_queue);
if ((call == NULL) || (entry->deadline < call->deadline)) {
sq_addfirst(&entry->link, &callout_queue);
//lldbg("call enter at head, reschedule\n");
/* we changed the next deadline, reschedule the timer event */
hrt_call_reschedule();
} else {
do {
next = (struct hrt_call *)sq_next(&call->link);
if ((next == NULL) || (entry->deadline < next->deadline)) {
//lldbg("call enter after head\n");
sq_addafter(&call->link, &entry->link, &callout_queue);
break;
}
} while ((call = next) != NULL);
}
//lldbg("scheduled\n");
}
/**
* Reschedule the next timer interrupt.
*
* This routine must be called with interrupts disabled.
*/
static void
hrt_call_reschedule()
{
hrt_abstime now = hrt_absolute_time();
struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue);
hrt_abstime deadline = now + HRT_INTERVAL_MAX;
/*
* Determine what the next deadline will be.
*
* Note that we ensure that this will be within the counter
* period, so that when we truncate all but the low 16 bits
* the next time the compare matches it will be the deadline
* we want.
*
* It is important for accurate timekeeping that the compare
* interrupt fires sufficiently often that the base_time update in
* hrt_absolute_time runs at least once per timer period.
*/
if (next != NULL) {
//lldbg("entry in queue\n");
if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
//lldbg("pre-expired\n");
/* set a minimal deadline so that we call ASAP */
deadline = now + HRT_INTERVAL_MIN;
} else if (next->deadline < deadline) {
//lldbg("due soon\n");
deadline = next->deadline;
}
}
}
static void
hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
{
/* if the entry is currently queued, remove it */
/* note that we are using a potentially uninitialised
entry->link here, but it is safe as sq_rem() doesn't
dereference the passed node unless it is found in the
list. So we potentially waste a bit of time searching the
queue for the uninitialised entry->link but we don't do
anything actually unsafe.
*/
if (entry->deadline != 0)
sq_rem(&entry->link, &callout_queue);
entry->deadline = deadline;
entry->period = interval;
entry->callout = callout;
entry->arg = arg;
hrt_call_enter(entry);
}
/*
* Call callout(arg) after delay has elapsed.
*
* If callout is NULL, this can be used to implement a timeout by testing the call
* with hrt_called().
*/
void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
{
hrt_call_internal(entry,
hrt_absolute_time() + delay,
0,
callout,
arg);
}
/*
* Call callout(arg) after delay, and then after every interval.
*
* Note thet the interval is timed between scheduled, not actual, call times, so the call rate may
* jitter but should not drift.
*/
void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
{
hrt_call_internal(entry,
hrt_absolute_time() + delay,
interval,
callout,
arg);
}
/*
* Call callout(arg) at absolute time calltime.
*/
void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
{
hrt_call_internal(entry, calltime, 0, callout, arg);
}
src/platforms/linux/px4_layer/module.mk
+4 -1
View File
@@ -41,7 +41,10 @@ SRCS = \
lib_crc32.c \
drv_hrt.c \
queue.c \
dq_addlast.c \
dq_remfirst.c \
sq_addlast.c \
sq_remfirst.c
sq_remfirst.c \
sq_addafter.c
MAXOPTIMIZATION = -Os
src/platforms/linux/px4_layer/px4_linux_impl.cpp
+21
View File
@@ -37,9 +37,12 @@
* PX4 Middleware Wrapper Linux Implementation
*/
#include <px4_defines.h>
#include <px4_middleware.h>
#include <px4_workqueue.h>
#include <stdint.h>
#include <stdio.h>
#include <unistd.h>
#include "systemlib/param/param.h"
__BEGIN_DECLS
@@ -49,6 +52,8 @@ struct param_info_s param_array[256];
struct param_info_s *param_info_base;
struct param_info_s *param_info_limit;
long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK);
__END_DECLS
namespace px4
@@ -92,3 +97,19 @@ void init(int argc, char *argv[], const char *app_name)
}
}
int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay)
{
printf("work_queue: UNIMPLEMENTED\n");
return PX4_OK;
}
int work_cancel(int qid, struct work_s *work)
{
printf("work_cancel: UNIMPLEMENTED\n");
return PX4_OK;
}
src/platforms/linux/px4_layer/px4_linux_tasks.c
+23 -11
View File
@@ -151,21 +151,14 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
int px4_task_delete(px4_task_t id)
{
int rv = 0;
int i;
pthread_t pid;
//printf("Called px4_task_delete\n");
// Get pthread ID from the opaque ID
for (i=0; i<PX4_MAX_TASKS; ++i) {
// FIXME - precludes pthread task to have an ID of 0
if (taskmap[i] == id) {
pid = taskmap[i];
break;
}
}
if (i>=PX4_MAX_TASKS) {
if (id < PX4_MAX_TASKS && taskmap[id] != 0)
pid = taskmap[id];
else
return -EINVAL;
}
// If current thread then exit, otherwise cancel
if (pthread_self() == pid) {
pthread_exit(0);
@@ -178,6 +171,25 @@ int px4_task_delete(px4_task_t id)
return rv;
}
void px4_task_exit(int ret)
{
int i;
pthread_t pid = pthread_self();
// Get pthread ID from the opaque ID
for (i=0; i<PX4_MAX_TASKS; ++i) {
// FIXME - precludes pthread task to have an ID of 0
if (taskmap[i] == pid) {
taskmap[i] = 0;
break;
}
}
if (i>=PX4_MAX_TASKS)
printf("px4_task_exit: self task not found!\n");
pthread_exit((void *)(unsigned long)ret);
}
void px4_killall(void)
{
//printf("Called px4_killall\n");
src/platforms/linux/px4_layer/sq_addafter.c
+71
View File
@@ -0,0 +1,71 @@
/************************************************************
* libc/queue/sq_addafter.c
*
* Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* 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 NuttX 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.
*
************************************************************/
/************************************************************
* Compilation Switches
************************************************************/
/************************************************************
* Included Files
************************************************************/
#include <queue.h>
/************************************************************
* Public Functions
************************************************************/
/************************************************************
* Name: sq_addafter.c
*
* Description:
* The sq_addafter function adds 'node' after 'prev' in the
* 'queue.'
*
************************************************************/
void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node,
sq_queue_t *queue)
{
if (!queue->head || prev == queue->tail)
{
sq_addlast(node, queue);
}
else
{
node->flink = prev->flink;
prev->flink = node;
}
}
src/drivers/device/vdevice.cpp → src/platforms/px4_adc.h
+25 -66
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 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
@@ -32,76 +32,35 @@
****************************************************************************/
/**
* @file device.cpp
* @file px4_adc.h
*
* Fundamental driver base class for the virtual device framework.
* ADC header depending on the build target
*/
#include "device.h"
#pragma once
#include <px4_defines.h>
#include <stdio.h>
#include <unistd.h>
#if defined(__PX4_ROS)
#error "ADC not supported in ROS"
#elif defined(__PX4_NUTTX)
/*
* Building for NuttX
*/
#include <nuttx/analog/adc.h>
#elif defined(__PX4_LINUX)
namespace device
// FIXME - this needs to be a px4_adc_msg_s type
// Curently copied from NuttX
struct adc_msg_s
{
uint8_t am_channel; /* The 8-bit ADC Channel */
int32_t am_data; /* ADC convert result (4 bytes) */
} packed_struct;
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;
}
// Example settings
#define ADC_BATTERY_VOLTAGE_CHANNEL 10
#define ADC_BATTERY_CURRENT_CHANNEL -1
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11
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
#else
#error "No target platform defined"
#endif
src/platforms/px4_config.h
+6
View File
@@ -44,4 +44,10 @@
#include <px4_config.h>
#elif defined (__PX4_LINUX)
#define CONFIG_NFILE_STREAMS 1
extern long PX4_TICKS_PER_SEC;
#define USEC2TICKS(x) (PX4_TICKS_PER_SEC*(long)x/1000000L)
#define px4_errx(x, ...) errx(x, __VA_ARGS__)
#endif
src/platforms/px4_defines.h
+3
View File
@@ -45,6 +45,8 @@
#define PX4_PARAM_DEFINE_INT32(_name) PARAM_DEFINE_INT32(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name))
#define PX4_PARAM_DEFINE_FLOAT(_name) PARAM_DEFINE_FLOAT(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name))
#define PX4_ERROR (-1)
#define PX4_OK 0
#if defined(__PX4_ROS)
/*
@@ -111,6 +113,7 @@ typedef param_t px4_param_t;
/* FIXME - Used to satisfy build */
#define UNIQUE_ID 0x1FFF7A10 //STM DocID018909 Rev 8 Sect 39.1 (Unique device ID Register)
#define USEC2TICK(x) 0
#define getreg32(a) (*(volatile uint32_t *)(a))
src/platforms/px4_i2c.h
+129
View File
@@ -0,0 +1,129 @@
/****************************************************************************
*
* Copyright (c) 2015 Mark Charlebois. 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 px4_i2c.h
*
* Includes device headers depending on the build target
*/
#pragma once
#define PX4_I2C_M_READ 0x0001 /* read data, from slave to master */
#if defined(__PX4_ROS)
#error "Devices not supported in ROS"
#elif defined (__PX4_NUTTX)
/*
* Building for NuttX
*/
#include <sys/ioctl.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <nuttx/i2c.h>
#include <nuttx/irq.h>
#include <nuttx/wqueue.h>
#include <chip.h>
#include <arch/board/board.h>
#include <arch/chip/chip.h>
#include "up_internal.h"
#include "up_arch.h"
#define px4_i2c_msg_t i2c_msg_s
typedef struct i2c_dev_s px4_i2c_dev_t;
#define px4_i2cuninitialize(x) up_i2cuninitialize(x)
#define px4_i2cinitialize(x) up_i2cinitialize(x)
#define px4_i2creset(x) up_i2creset(x)
#define px4_interrupt_context() up_interrupt_context()
#elif defined(__PX4_LINUX)
#include <stdint.h>
#define I2C_M_READ 0x0001 /* read data, from slave to master */
#define I2C_M_TEN 0x0002 /* ten bit address */
#define I2C_M_NORESTART 0x0080 /* message should not begin with (re-)start of transfer */
// NOTE - This is a copy of the NuttX i2c_msg_s structure
typedef struct {
uint16_t addr; /* Slave address */
uint16_t flags; /* See I2C_M_* definitions */
uint8_t *buffer;
int length;
} px4_i2c_msg_t;
struct px4_i2c_ops_t;
// NOTE - This is a copy of the NuttX i2c_ops_s structure
typedef struct {
const struct px4_i2c_ops_t *ops; /* I2C vtable */
} px4_i2c_dev_t;
// FIXME - Stub implementations
inline void px4_i2cuninitialize(px4_i2c_dev_t *dev);
inline void px4_i2cuninitialize(px4_i2c_dev_t *dev) {}
inline px4_i2c_dev_t *px4_i2cinitialize(int bus);
inline px4_i2c_dev_t *px4_i2cinitialize(int bus) { return (px4_i2c_dev_t *)0; }
inline void px4_i2creset(px4_i2c_dev_t *dev);
inline void px4_i2creset(px4_i2c_dev_t *dev) { }
inline bool px4_interrupt_context(void);
inline bool px4_interrupt_context(void) { return false; }
// FIXME - Empty defines for I2C ops
// Original version commented out
//#define I2C_SETFREQUENCY(d,f) ((d)->ops->setfrequency(d,f))
#define I2C_SETFREQUENCY(d,f)
//#define SPI_SELECT(d,id,s) ((d)->ops->select(d,id,s))
#define SPI_SELECT(d,id,s)
// FIXME - Stub implementation
// Original version commented out
//#define I2C_TRANSFER(d,m,c) ((d)->ops->transfer(d,m,c))
inline int I2C_TRANSFER(px4_i2c_dev_t *dev, px4_i2c_msg_t *msg, int count);
inline int I2C_TRANSFER(px4_i2c_dev_t *dev, px4_i2c_msg_t *msg, int count) { return 0; }
struct i2c_msg_s
{
uint16_t addr; /* Slave address */
uint16_t flags; /* See I2C_M_* definitions */
uint8_t *buffer;
int length;
};
#else
#error "No target platform defined"
#endif
src/platforms/px4_posix.h
+5 -5
View File
@@ -39,6 +39,7 @@
#pragma once
#include <px4_defines.h>
#include <stdint.h>
#include <sys/types.h>
#include <poll.h>
@@ -48,12 +49,11 @@
#define PX4_F_RDONLY 1
#define PX4_F_WRONLY 2
#define PX4_DIOC_GETPRIV 1
#define PX4_DEVIOCSPUBBLOCK 2
#define PX4_DEVIOCGPUBBLOCK 3
#define PX4_DEVIOCGDEVICEID 1
#define PX4_ERROR (-1)
#define PX4_OK 0
#define PX4_DIOC_GETPRIV 2
#define PX4_DEVIOCSPUBBLOCK 3
#define PX4_DEVIOCGPUBBLOCK 4
//#define PX4_DEBUG(...)
#define PX4_DEBUG(...) printf(__VA_ARGS__)

Some files were not shown because too many files have changed in this diff Show More