fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Checkpoint: interface abstraction for px4io driver

Browse Source
This commit is contained in:
px4dev 2013-06-25 23:08:34 -07:00
parent 758ebf6c04
commit 90c458cb61
9 changed files with 847 additions and 193 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
makefiles/config_px4fmuv2_default.mk
View File

@ -15,6 +15,7 @@ MODULES += drivers/stm32
MODULES += drivers/stm32/adc
MODULES += drivers/stm32/tone_alarm
MODULES += drivers/px4fmu
MODULES += drivers/px4io
MODULES += drivers/boards/px4fmuv2
MODULES += drivers/rgbled
MODULES += drivers/lsm303d

78
src/drivers/px4io/interface.h Normal file
View File

@ -0,0 +1,78 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file interface.h
*
* PX4IO interface classes.
*/
#include <nuttx/config.h>
#include <stdint.h>
class PX4IO_interface
{
public:
/**
* Check that the interface initialised OK.
*
* Does not check that communication has been established.
*/
virtual bool ok() = 0;
/**
* Set PX4IO registers.
*
* @param page The register page to write
* @param offset Offset of the first register to write
* @param values Pointer to values to write
* @param num_values The number of values to write
* @return Zero on success.
*/
virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) = 0;
/**
* Get PX4IO registers.
*
* @param page The register page to read
* @param offset Offset of the first register to read
* @param values Pointer to store values read
* @param num_values The number of values to read
* @return Zero on success.
*/
virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) = 0;
};
extern PX4IO_interface *io_i2c_interface(int bus, uint8_t address);
extern PX4IO_interface *io_serial_interface(int port);

179
src/drivers/px4io/interface_i2c.cpp Normal file
View File

@ -0,0 +1,179 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file interface_i2c.cpp
*
* I2C interface for PX4IO
*/
/* XXX trim includes */
#include <nuttx/config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <assert.h>
#include <debug.h>
#include <errno.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <nuttx/i2c.h>
#include <mavlink/mavlink_log.h>
#include "uploader.h"
#include <modules/px4iofirmware/protocol.h>
#include "interface.h"
class PX4IO_I2C : public PX4IO_interface
{
public:
PX4IO_I2C(int bus, uint8_t address);
virtual ~PX4IO_I2C();
virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values);
virtual bool ok();
private:
static const unsigned _retries = 2;
struct i2c_dev_s *_dev;
uint8_t _address;
};
PX4IO_interface *io_i2c_interface(int bus, uint8_t address)
{
return new PX4IO_I2C(bus, address);
}
PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) :
_dev(nullptr),
_address(address)
{
_dev = up_i2cinitialize(bus);
if (_dev)
I2C_SETFREQUENCY(_dev, 400000);
}
PX4IO_I2C::~PX4IO_I2C()
{
if (_dev)
up_i2cuninitialize(_dev);
}
bool
PX4IO_I2C::ok()
{
if (!_dev)
return false;
/* check any other status here */
return true;
}
int
PX4IO_I2C::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
{
int ret;
/* set up the transfer */
uint8_t addr[2] = {
page,
offset
};
i2c_msg_s msgv[2];
msgv[0].addr = _address;
msgv[0].flags = 0;
msgv[0].buffer = addr;
msgv[0].length = 2;
msgv[1].addr = _address;
msgv[1].flags = I2C_M_NORESTART;
msgv[1].buffer = (uint8_t *)values;
msgv[1].length = num_values * sizeof(*values);
unsigned tries = 0;
do {
/* perform the transfer */
ret = I2C_TRANSFER(_dev, msgv, 2);
if (ret == OK)
break;
} while (tries++ < _retries);
return ret;
}
int
PX4IO_I2C::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values)
{
int ret;
/* set up the transfer */
uint8_t addr[2] = {
page,
offset
};
i2c_msg_s msgv[2];
msgv[0].addr = _address;
msgv[0].flags = 0;
msgv[0].buffer = addr;
msgv[0].length = 2;
msgv[1].addr = _address;
msgv[1].flags = I2C_M_READ;
msgv[1].buffer = (uint8_t *)values;
msgv[1].length = num_values * sizeof(*values);
unsigned tries = 0;
do {
/* perform the transfer */
ret = I2C_TRANSFER(_dev, msgv, 2);
if (ret == OK)
break;
} while (tries++ < _retries);
return ret;
}

361
src/drivers/px4io/interface_serial.cpp Normal file
View File

@ -0,0 +1,361 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file interface_serial.cpp
*
* Serial interface for PX4IO
*/
/* XXX trim includes */
#include <nuttx/config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <assert.h>
#include <debug.h>
#include <time.h>
#include <errno.h>
#include <string.h>
#include <arch/board/board.h>
/* XXX might be able to prune these */
#include <chip.h>
#include <up_internal.h>
#include <up_arch.h>
#include <stm32_internal.h>
#include <debug.h>
#include <systemlib/hx_stream.h>
#include "interface.h"
class PX4IO_serial : public PX4IO_interface
{
public:
PX4IO_serial(int port);
virtual ~PX4IO_serial();
virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values);
virtual bool ok();
private:
volatile uint32_t *_serial_base;
int _vector;
uint8_t *_tx_buf;
unsigned _tx_size;
const uint8_t *_rx_buf;
unsigned _rx_size;
hx_stream_t _stream;
sem_t _bus_semaphore;
sem_t _completion_semaphore;
/**
* Send _tx_size bytes from the buffer, then
* if _rx_size is greater than zero wait for a packet
* to come back.
*/
int _wait_complete();
/**
* Interrupt handler.
*/
static int _interrupt(int irq, void *context);
void _do_interrupt();
/**
* Stream transmit callback
*/
static void _tx(void *arg, uint8_t data);
void _do_tx(uint8_t data);
/**
* Stream receive callback
*/
static void _rx(void *arg, const void *data, size_t length);
void _do_rx(const uint8_t *data, size_t length);
/**
* Serial register accessors.
*/
volatile uint32_t &_sreg(unsigned offset)
{
return *(_serial_base + (offset / sizeof(uint32_t)));
}
volatile uint32_t &_SR() { return _sreg(STM32_USART_SR_OFFSET); }
volatile uint32_t &_DR() { return _sreg(STM32_USART_DR_OFFSET); }
volatile uint32_t &_BRR() { return _sreg(STM32_USART_BRR_OFFSET); }
volatile uint32_t &_CR1() { return _sreg(STM32_USART_CR1_OFFSET); }
volatile uint32_t &_CR2() { return _sreg(STM32_USART_CR2_OFFSET); }
volatile uint32_t &_CR3() { return _sreg(STM32_USART_CR3_OFFSET); }
volatile uint32_t &_GTPR() { return _sreg(STM32_USART_GTPR_OFFSET); }
};
/* XXX hack to avoid expensive IRQ lookup */
static PX4IO_serial *io_serial;
PX4IO_interface *io_serial_interface(int port)
{
return new PX4IO_serial(port);
}
PX4IO_serial::PX4IO_serial(int port) :
_serial_base(0),
_vector(0),
_tx_buf(nullptr),
_tx_size(0),
_rx_size(0),
_stream(0)
{
/* only allow one instance */
if (io_serial != nullptr)
return;
switch (port) {
case 5:
_serial_base = (volatile uint32_t *)STM32_UART5_BASE;
_vector = STM32_IRQ_UART5;
break;
default:
/* not a supported port */
return;
}
/* need space for worst-case escapes + hx protocol overhead */
/* XXX this is kinda gross, but hx transmits a byte at a time */
_tx_buf = new uint8_t[HX_STREAM_MAX_FRAME];
irq_attach(_vector, &_interrupt);
_stream = hx_stream_init(-1, _rx, this);
sem_init(&_completion_semaphore, 0, 0);
sem_init(&_bus_semaphore, 0, 1);
}
PX4IO_serial::~PX4IO_serial()
{
if (_tx_buf != nullptr)
delete[] _tx_buf;
if (_vector)
irq_detach(_vector);
if (io_serial == this)
io_serial = nullptr;
if (_stream)
hx_stream_free(_stream);
sem_destroy(&_completion_semaphore);
sem_destroy(&_bus_semaphore);
}
bool
PX4IO_serial::ok()
{
if (_serial_base == 0)
return false;
if (_vector == 0)
return false;
if (_tx_buf == nullptr)
return false;
if (!_stream)
return false;
return true;
}
int
PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
{
unsigned count = num_values * sizeof(*values);
if (count > (HX_STREAM_MAX_FRAME - 2))
return -EINVAL;
sem_wait(&_bus_semaphore);
_tx_buf[0] = page;
_tx_buf[1] = offset;
memcpy(&_tx_buf[2], (void *)values, count);
_tx_size = count + 2;
_rx_size = 0;
/* start the transaction and wait for it to complete */
int result = _wait_complete();
sem_post(&_bus_semaphore);
return result;
}
int
PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values)
{
unsigned count = num_values * sizeof(*values);
if (count > HX_STREAM_MAX_FRAME)
return -EINVAL;
sem_wait(&_bus_semaphore);
_tx_buf[0] = page;
_tx_buf[1] = offset;
_tx_buf[2] = num_values;
_tx_size = 3; /* this tells IO that this is a read request */
_rx_size = count;
/* start the transaction and wait for it to complete */
int result = _wait_complete();
if (result != OK)
goto out;
/* compare the received count with the expected count */
if (_rx_size != count) {
return -EIO;
} else {
/* copy back the result */
memcpy(values, &_tx_buf[0], count);
}
out:
sem_post(&_bus_semaphore);
return OK;
}
int
PX4IO_serial::_wait_complete()
{
/* prepare the stream for transmission */
hx_stream_reset(_stream);
hx_stream_start(_stream, _tx_buf, _tx_size);
/* enable UART */
_CR1() |= USART_CR1_RE |
USART_CR1_TE |
USART_CR1_TXEIE |
USART_CR1_RXNEIE |
USART_CR1_UE;
/* compute the deadline for a 5ms timeout */
struct timespec abstime;
clock_gettime(CLOCK_REALTIME, &abstime);
abstime.tv_nsec += 5000000; /* 5ms timeout */
while (abstime.tv_nsec > 1000000000) {
abstime.tv_sec++;
abstime.tv_nsec -= 1000000000;
}
/* wait for the transaction to complete */
int ret = sem_timedwait(&_completion_semaphore, &abstime);
/* disable the UART */
_CR1() &= ~(USART_CR1_RE |
USART_CR1_TE |
USART_CR1_TXEIE |
USART_CR1_RXNEIE |
USART_CR1_UE);
return ret;
}
int
PX4IO_serial::_interrupt(int irq, void *context)
{
/* ... because NuttX doesn't give us a handle per vector */
io_serial->_do_interrupt();
return 0;
}
void
PX4IO_serial::_do_interrupt()
{
uint32_t sr = _SR();
/* handle transmit completion */
if (sr & USART_SR_TXE) {
int c = hx_stream_send_next(_stream);
if (c == -1) {
/* transmit (nearly) done, not interested in TX-ready interrupts now */
_CR1() &= ~USART_CR1_TXEIE;
/* was this a tx-only operation? */
if (_rx_size == 0) {
/* wake up waiting sender */
sem_post(&_completion_semaphore);
}
} else {
_DR() = c;
}
}
if (sr & USART_SR_RXNE) {
uint8_t c = _DR();
hx_stream_rx(_stream, c);
}
}
void
PX4IO_serial::_rx(void *arg, const void *data, size_t length)
{
PX4IO_serial *pserial = reinterpret_cast<PX4IO_serial *>(arg);
pserial->_do_rx((const uint8_t *)data, length);
}
void
PX4IO_serial::_do_rx(const uint8_t *data, size_t length)
{
_rx_buf = data;
if (length < _rx_size)
_rx_size = length;
/* notify waiting receiver */
sem_post(&_completion_semaphore);
}

7
src/drivers/px4io/module.mk
View File

@ -38,4 +38,9 @@
MODULE_COMMAND = px4io
SRCS = px4io.cpp \
uploader.cpp
uploader.cpp \
interface_serial.cpp \
interface_i2c.cpp
# XXX prune to just get UART registers
INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common

91
src/drivers/px4io/px4io.cpp
View File

@ -58,7 +58,6 @@
#include <arch/board/board.h>
#include <drivers/device/device.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
@ -83,16 +82,18 @@
#include <debug.h>
#include <mavlink/mavlink_log.h>
#include "uploader.h"
#include <modules/px4iofirmware/protocol.h>
#include "uploader.h"
#include "interface.h"
#define PX4IO_SET_DEBUG _IOC(0xff00, 0)
#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1)
class PX4IO : public device::I2C
class PX4IO : public device::CDev
{
public:
PX4IO();
PX4IO(PX4IO_interface *interface);
virtual ~PX4IO();
virtual int init();
@ -130,6 +131,8 @@ public:
void print_status();
private:
PX4IO_interface *_interface;
// XXX
unsigned _max_actuators;
unsigned _max_controls;
@ -312,8 +315,9 @@ PX4IO *g_dev;
}
PX4IO::PX4IO() :
I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000),
PX4IO::PX4IO(PX4IO_interface *interface) :
CDev("px4io", "/dev/px4io"),
_interface(interface),
_max_actuators(0),
_max_controls(0),
_max_rc_input(0),
@ -364,6 +368,9 @@ PX4IO::~PX4IO()
if (_task != -1)
task_delete(_task);
if (_interface != nullptr)
delete _interface;
g_dev = nullptr;
}
@ -375,18 +382,10 @@ PX4IO::init()
ASSERT(_task == -1);
/* do regular cdev init */
ret = I2C::init();
ret = CDev::init();
if (ret != OK)
return ret;
/*
* Enable a couple of retries for operations to IO.
*
* Register read/write operations are intentionally idempotent
* so this is safe as designed.
*/
_retries = 2;
/* get some parameters */
_max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT);
_max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT);
@ -395,7 +394,7 @@ PX4IO::init()
_max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT);
if ((_max_actuators < 1) || (_max_actuators > 255) ||
(_max_relays < 1) || (_max_relays > 255) ||
(_max_transfer < 16) || (_max_transfer > 255) ||
(_max_transfer < 16) || (_max_transfer > 255) ||
(_max_rc_input < 1) || (_max_rc_input > 255)) {
log("failed getting parameters from PX4IO");
@ -700,8 +699,6 @@ PX4IO::io_set_control_state()
int
PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
{
uint16_t regs[_max_actuators];
if (len > _max_actuators)
/* fail with error */
return E2BIG;
@ -1114,22 +1111,7 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned
return -EINVAL;
}
/* set up the transfer */
uint8_t addr[2] = {
page,
offset
};
i2c_msg_s msgv[2];
msgv[0].flags = 0;
msgv[0].buffer = addr;
msgv[0].length = 2;
msgv[1].flags = I2C_M_NORESTART;
msgv[1].buffer = (uint8_t *)values;
msgv[1].length = num_values * sizeof(*values);
/* perform the transfer */
int ret = transfer(msgv, 2);
int ret = _interface->set_reg(page, offset, values, num_values);
if (ret != OK)
debug("io_reg_set: error %d", ret);
return ret;
@ -1144,22 +1126,13 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value)
int
PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values)
{
/* set up the transfer */
uint8_t addr[2] = {
page,
offset
};
i2c_msg_s msgv[2];
/* range check the transfer */
if (num_values > ((_max_transfer) / sizeof(*values))) {
debug("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2);
return -EINVAL;
}
msgv[0].flags = 0;
msgv[0].buffer = addr;
msgv[0].length = 2;
msgv[1].flags = I2C_M_READ;
msgv[1].buffer = (uint8_t *)values;
msgv[1].length = num_values * sizeof(*values);
/* perform the transfer */
int ret = transfer(msgv, 2);
int ret = _interface->get_reg(page, offset, values, num_values);
if (ret != OK)
debug("io_reg_get: data error %d", ret);
return ret;
@ -1603,8 +1576,26 @@ start(int argc, char *argv[])
if (g_dev != nullptr)
errx(1, "already loaded");
PX4IO_interface *interface;
#if defined(CONFIG_ARCH_BOARD_PX4FMUV2)
interface = io_serial_interface(5); /* XXX wrong port! */
#elif defined(CONFIG_ARCH_BOARD_PX4FMU)
interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO);
#else
# error Unknown board - cannot select interface.
#endif
if (interface == nullptr)
errx(1, "cannot alloc interface");
if (!interface->ok()) {
delete interface;
errx(1, "interface init failed");
}
/* create the driver - it will set g_dev */
(void)new PX4IO();
(void)new PX4IO(interface);
if (g_dev == nullptr)
errx(1, "driver alloc failed");

44
src/modules/px4iofirmware/protocol.h
View File

@ -36,8 +36,7 @@
/**
* @file protocol.h
*
* PX4IO interface protocol
* ========================
* PX4IO interface protocol.
*
* Communication is performed via writes to and reads from 16-bit virtual
* registers organised into pages of 255 registers each.
@ -46,7 +45,7 @@
* respectively. Subsequent reads and writes increment the offset within
* the page.
*
* Most pages are readable or writable but not both.
* Some pages are read- or write-only.
*
* Note that some pages may permit offset values greater than 255, which
* can only be achieved by long writes. The offset does not wrap.
@ -63,29 +62,6 @@
* Note that the implementation of readable pages prefers registers within
* readable pages to be densely packed. Page numbers do not need to be
* packed.
*
* PX4IO I2C interface notes
* -------------------------
*
* Register read/write operations are mapped directly to PX4IO register
* read/write operations.
*
* PX4IO Serial interface notes
* ----------------------------
*
* The MSB of the page number is used to distinguish between read and
* write operations. If set, the operation is a write and additional
* data is expected to follow in the packet as for I2C writes.
*
* If clear, the packet is expected to contain a single byte giving the
* number of registers to be read. PX4IO will respond with a packet containing
* the same header (page, offset) and the requested data.
*
* If a read is requested when PX4IO does not have buffer space to store
* the reply, the request will be dropped. PX4IO is always configured with
* enough space to receive one full-sized write and one read request, and
* to send one full-sized read response.
*
*/
#define PX4IO_CONTROL_CHANNELS 8
@ -99,14 +75,12 @@
#define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f)
#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
#define PX4IO_PAGE_WRITE (1<<7)
/* static configuration page */
#define PX4IO_PAGE_CONFIG 0
#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */
#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */
#define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */
#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum packet transfer size */
#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */
#define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */
#define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */
#define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */
@ -168,7 +142,7 @@
#define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */
/* setup page */
#define PX4IO_PAGE_SETUP 64
#define PX4IO_PAGE_SETUP 100
#define PX4IO_P_SETUP_FEATURES 0
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
@ -186,13 +160,13 @@
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
/* autopilot control values, -10000..10000 */
#define PX4IO_PAGE_CONTROLS 65 /* 0..CONFIG_CONTROL_COUNT */
#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */
/* raw text load to the mixer parser - ignores offset */
#define PX4IO_PAGE_MIXERLOAD 66 /* see px4io_mixdata structure below */
#define PX4IO_PAGE_MIXERLOAD 102
/* R/C channel config */
#define PX4IO_PAGE_RC_CONFIG 67 /* R/C input configuration */
#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */
#define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */
#define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */
#define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */
@ -204,10 +178,10 @@
#define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */
/* PWM output - overrides mixer */
#define PX4IO_PAGE_DIRECT_PWM 68 /* 0..CONFIG_ACTUATOR_COUNT-1 */
#define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM failsafe values - zero disables the output */
#define PX4IO_PAGE_FAILSAFE_PWM 69 /* 0..CONFIG_ACTUATOR_COUNT-1 */
#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/**
* As-needed mixer data upload.

219
src/modules/systemlib/hx_stream.c
View File

@ -53,14 +53,26 @@
struct hx_stream {
uint8_t buf[HX_STREAM_MAX_FRAME + 4];
unsigned frame_bytes;
bool escaped;
bool txerror;
/* RX state */
uint8_t rx_buf[HX_STREAM_MAX_FRAME + 4];
unsigned rx_frame_bytes;
bool rx_escaped;
hx_stream_rx_callback rx_callback;
void *rx_callback_arg;
/* TX state */
int fd;
hx_stream_rx_callback callback;
void *callback_arg;
bool tx_error;
uint8_t *tx_buf;
unsigned tx_resid;
uint32_t tx_crc;
enum {
TX_IDLE = 0,
TX_SEND_START,
TX_SEND_DATA,
TX_SENT_ESCAPE,
TX_SEND_END
} tx_state;
perf_counter_t pc_tx_frames;
perf_counter_t pc_rx_frames;
@ -76,13 +88,12 @@ struct hx_stream {
static void hx_tx_raw(hx_stream_t stream, uint8_t c);
static void hx_tx_raw(hx_stream_t stream, uint8_t c);
static int hx_rx_frame(hx_stream_t stream);
static bool hx_rx_char(hx_stream_t stream, uint8_t c);
static void
hx_tx_raw(hx_stream_t stream, uint8_t c)
{
if (write(stream->fd, &c, 1) != 1)
stream->txerror = true;
stream->tx_error = true;
}
static void
@ -106,11 +117,11 @@ hx_rx_frame(hx_stream_t stream)
uint8_t b[4];
uint32_t w;
} u;
unsigned length = stream->frame_bytes;
unsigned length = stream->rx_frame_bytes;
/* reset the stream */
stream->frame_bytes = 0;
stream->escaped = false;
stream->rx_frame_bytes = 0;
stream->rx_escaped = false;
/* not a real frame - too short */
if (length < 4) {
@ -123,11 +134,11 @@ hx_rx_frame(hx_stream_t stream)
length -= 4;
/* compute expected CRC */
u.w = crc32(&stream->buf[0], length);
u.w = crc32(&stream->rx_buf[0], length);
/* compare computed and actual CRC */
for (unsigned i = 0; i < 4; i++) {
if (u.b[i] != stream->buf[length + i]) {
if (u.b[i] != stream->rx_buf[length + i]) {
perf_count(stream->pc_rx_errors);
return 0;
}
@ -135,7 +146,7 @@ hx_rx_frame(hx_stream_t stream)
/* frame is good */
perf_count(stream->pc_rx_frames);
stream->callback(stream->callback_arg, &stream->buf[0], length);
stream->rx_callback(stream->rx_callback_arg, &stream->rx_buf[0], length);
return 1;
}
@ -151,8 +162,8 @@ hx_stream_init(int fd,
if (stream != NULL) {
memset(stream, 0, sizeof(struct hx_stream));
stream->fd = fd;
stream->callback = callback;
stream->callback_arg = arg;
stream->rx_callback = callback;
stream->rx_callback_arg = arg;
}
return stream;
@ -180,105 +191,135 @@ hx_stream_set_counters(hx_stream_t stream,
stream->pc_rx_errors = rx_errors;
}
void
hx_stream_reset(hx_stream_t stream)
{
stream->rx_frame_bytes = 0;
stream->rx_escaped = false;
stream->tx_buf = NULL;
stream->tx_resid = 0;
stream->tx_state = TX_IDLE;
}
int
hx_stream_start(hx_stream_t stream,
const void *data,
size_t count)
{
if (count > HX_STREAM_MAX_FRAME)
return -EINVAL;
stream->tx_buf = data;
stream->tx_resid = count;
stream->tx_state = TX_SEND_START;
stream->tx_crc = crc32(data, count);
return OK;
}
int
hx_stream_send_next(hx_stream_t stream)
{
int c;
/* sort out what we're going to send */
switch (stream->tx_state) {
case TX_SEND_START:
stream->tx_state = TX_SEND_DATA;
return FBO;
case TX_SEND_DATA:
c = *stream->tx_buf;
switch (c) {
case FBO:
case CEO:
stream->tx_state = TX_SENT_ESCAPE;
return CEO;
}
break;
case TX_SENT_ESCAPE:
c = *stream->tx_buf ^ 0x20;
stream->tx_state = TX_SEND_DATA;
break;
case TX_SEND_END:
stream->tx_state = TX_IDLE;
return FBO;
case TX_IDLE:
default:
return -1;
}
/* if we are here, we have consumed a byte from the buffer */
stream->tx_resid--;
stream->tx_buf++;
/* buffer exhausted */
if (stream->tx_resid == 0) {
uint8_t *pcrc = (uint8_t *)&stream->tx_crc;
/* was the buffer the frame CRC? */
if (stream->tx_buf == (pcrc + sizeof(stream->tx_crc))) {
stream->tx_state = TX_SEND_END;
} else {
/* no, it was the payload - switch to sending the CRC */
stream->tx_buf = pcrc;
stream->tx_resid = sizeof(stream->tx_crc);
}
}
return c;
}
int
hx_stream_send(hx_stream_t stream,
const void *data,
size_t count)
{
union {
uint8_t b[4];
uint32_t w;
} u;
const uint8_t *p = (const uint8_t *)data;
unsigned resid = count;
int result;
if (resid > HX_STREAM_MAX_FRAME)
return -EINVAL;
result = hx_start(stream, data, count);
if (result != OK)
return result;
/* start the frame */
hx_tx_raw(stream, FBO);
/* transmit the data */
while (resid--)
hx_tx_byte(stream, *p++);
/* compute the CRC */
u.w = crc32(data, count);
/* send the CRC */
p = &u.b[0];
resid = 4;
while (resid--)
hx_tx_byte(stream, *p++);
/* and the trailing frame separator */
hx_tx_raw(stream, FBO);
int c;
while ((c = hx_send_next(stream)) >= 0)
hx_tx_raw(stream, c);
/* check for transmit error */
if (stream->txerror) {
stream->txerror = false;
if (stream->tx_error) {
stream->tx_error = false;
return -EIO;
}
perf_count(stream->pc_tx_frames);
return 0;
return OK;
}
static bool
hx_rx_char(hx_stream_t stream, uint8_t c)
void
hx_stream_rx(hx_stream_t stream, uint8_t c)
{
/* frame end? */
if (c == FBO) {
hx_rx_frame(stream);
return true;
return;
}
/* escaped? */
if (stream->escaped) {
stream->escaped = false;
if (stream->rx_escaped) {
stream->rx_escaped = false;
c ^= 0x20;
} else if (c == CEO) {
/* now escaped, ignore the byte */
stream->escaped = true;
return false;
/* now rx_escaped, ignore the byte */
stream->rx_escaped = true;
return;
}
/* save for later */
if (stream->frame_bytes < sizeof(stream->buf))
stream->buf[stream->frame_bytes++] = c;
return false;
}
void
hx_stream_rx_char(hx_stream_t stream, uint8_t c)
{
hx_rx_char(stream, c);
}
int
hx_stream_rx(hx_stream_t stream)
{
uint16_t buf[16];
ssize_t len;
/* read bytes */
len = read(stream->fd, buf, sizeof(buf));
if (len <= 0) {
/* nonblocking stream and no data */
if (errno == EWOULDBLOCK)
return 0;
/* error or EOF */
return -errno;
}
/* process received characters */
for (int i = 0; i < len; i++)
hx_rx_char(stream, buf[i]);
return 0;
if (stream->rx_frame_bytes < sizeof(stream->rx_buf))
stream->rx_buf[stream->rx_frame_bytes++] = c;
}

60
src/modules/systemlib/hx_stream.h
View File

@ -58,7 +58,8 @@ __BEGIN_DECLS
* Allocate a new hx_stream object.
*
* @param fd The file handle over which the protocol will
* communicate.
* communicate, or -1 if the protocol will use
* hx_stream_start/hx_stream_send_next.
* @param callback Called when a frame is received.
* @param callback_arg Passed to the callback.
* @return A handle to the stream, or NULL if memory could
@ -80,6 +81,7 @@ __EXPORT extern void hx_stream_free(hx_stream_t stream);
*
* Any counter may be set to NULL to disable counting that datum.
*
* @param stream A handle returned from hx_stream_init.
* @param tx_frames Counter for transmitted frames.
* @param rx_frames Counter for received frames.
* @param rx_errors Counter for short and corrupt received frames.
@ -89,6 +91,44 @@ __EXPORT extern void hx_stream_set_counters(hx_stream_t stream,
perf_counter_t rx_frames,
perf_counter_t rx_errors);
/**
* Reset a stream.
*
* Forces the local stream state to idle.
*
* @param stream A handle returned from hx_stream_init.
*/
__EXPORT extern void hx_stream_reset(hx_stream_t stream);
/**
* Prepare to send a frame.
*
* Use this in conjunction with hx_stream_send_next to
* set the frame to be transmitted.
*
* Use hx_stream_send() to write to the stream fd directly.
*
* @param stream A handle returned from hx_stream_init.
* @param data Pointer to the data to send.
* @param count The number of bytes to send.
* @return Zero on success, -errno on error.
*/
__EXPORT extern int hx_stream_start(hx_stream_t stream,
const void *data,
size_t count);
/**
* Get the next byte to send for a stream.
*
* This requires that the stream be prepared for sending by
* calling hx_stream_start first.
*
* @param stream A handle returned from hx_stream_init.
* @return The byte to send, or -1 if there is
* nothing left to send.
*/
__EXPORT extern int hx_stream_send_next(hx_stream_t stream);
/**
* Send a frame.
*
@ -114,25 +154,9 @@ __EXPORT extern int hx_stream_send(hx_stream_t stream,
* @param stream A handle returned from hx_stream_init.
* @param c The character to process.
*/
__EXPORT extern void hx_stream_rx_char(hx_stream_t stream,
__EXPORT extern void hx_stream_rx(hx_stream_t stream,
uint8_t c);
/**
* Handle received bytes from the stream.
*
* Note that this interface should only be used with blocking streams
* when it is OK for the call to block until a frame is received.
*
* When used with a non-blocking stream, it will typically return
* immediately, or after handling a received frame.
*
* @param stream A handle returned from hx_stream_init.
* @return -errno on error, nonzero if a frame
* has been received, or if not enough
* bytes are available to complete a frame.
*/
__EXPORT extern int hx_stream_rx(hx_stream_t stream);
__END_DECLS
#endif