forked from Archive/PX4-Autopilot
Serial interface for IOv2
This commit is contained in:
px4dev
parent
8f7200e011
commit
e67022f874
|
|
@ -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
|
||||
|
|
@ -69,6 +69,7 @@ static void i2c_rx_setup(void);
|
|||
static void i2c_tx_setup(void);
|
||||
static void i2c_rx_complete(void);
|
||||
static void i2c_tx_complete(void);
|
||||
static void i2c_dump(void);
|
||||
|
||||
static DMA_HANDLE rx_dma;
|
||||
static DMA_HANDLE tx_dma;
|
||||
|
|
@ -92,7 +93,7 @@ enum {
|
|||
} direction;
|
||||
|
||||
void
|
||||
i2c_init(void)
|
||||
interface_init(void)
|
||||
{
|
||||
debug("i2c init");
|
||||
|
||||
|
|
@ -148,12 +149,18 @@ i2c_init(void)
|
|||
#endif
|
||||
}
|
||||
|
||||
void
|
||||
interface_tick()
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
reset the I2C bus
|
||||
used to recover from lockups
|
||||
*/
|
||||
void i2c_reset(void)
|
||||
void
|
||||
i2c_reset(void)
|
||||
{
|
||||
rCR1 |= I2C_CR1_SWRST;
|
||||
rCR1 = 0;
|
||||
|
|
@ -330,7 +337,7 @@ i2c_tx_complete(void)
|
|||
i2c_tx_setup();
|
||||
}
|
||||
|
||||
void
|
||||
static void
|
||||
i2c_dump(void)
|
||||
{
|
||||
debug("CR1 0x%08x CR2 0x%08x", rCR1, rCR2);
|
||||
|
|
|
|||
|
|
@ -8,7 +8,6 @@ SRCS = adc.c \
|
|||
safety.c \
|
||||
sbus.c \
|
||||
../systemlib/up_cxxinitialize.c \
|
||||
../systemlib/hx_stream.c \
|
||||
../systemlib/perf_counter.c \
|
||||
mixer.cpp \
|
||||
../systemlib/mixer/mixer.cpp \
|
||||
|
|
@ -16,11 +15,10 @@ SRCS = adc.c \
|
|||
../systemlib/mixer/mixer_multirotor.cpp \
|
||||
../systemlib/mixer/mixer_simple.cpp \
|
||||
|
||||
ifneq ($(CONFIG_ARCH_BOARD_PX4IO),)
|
||||
ifeq ($(BOARD),px4io)
|
||||
SRCS += i2c.c
|
||||
EXTRADEFINES += -DINTERFACE_I2C
|
||||
endif
|
||||
ifneq ($(CONFIG_ARCH_BOARD_PX4IOV2),)
|
||||
#SRCS += serial.c
|
||||
EXTRADEFINES += -DINTERFACE_SERIAL
|
||||
ifeq ($(BOARD),px4iov2)
|
||||
SRCS += serial.c \
|
||||
../systemlib/hx_stream.c
|
||||
endif
|
||||
|
|
|
|||
|
|
@ -36,7 +36,7 @@
|
|||
/**
|
||||
* @file protocol.h
|
||||
*
|
||||
* PX4IO I2C 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.
|
||||
|
|
@ -62,6 +62,27 @@
|
|||
* 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 bytes 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
|
||||
|
|
@ -75,12 +96,14 @@
|
|||
#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 I2C transfer size */
|
||||
#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum packet 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 */
|
||||
|
|
@ -141,7 +164,7 @@
|
|||
#define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */
|
||||
|
||||
/* setup page */
|
||||
#define PX4IO_PAGE_SETUP 100
|
||||
#define PX4IO_PAGE_SETUP 64
|
||||
#define PX4IO_P_SETUP_FEATURES 0
|
||||
|
||||
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
|
||||
|
|
@ -160,13 +183,13 @@
|
|||
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
|
||||
|
||||
/* autopilot control values, -10000..10000 */
|
||||
#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */
|
||||
#define PX4IO_PAGE_CONTROLS 65 /* 0..CONFIG_CONTROL_COUNT */
|
||||
|
||||
/* raw text load to the mixer parser - ignores offset */
|
||||
#define PX4IO_PAGE_MIXERLOAD 102
|
||||
#define PX4IO_PAGE_MIXERLOAD 66 /* see px4io_mixdata structure below */
|
||||
|
||||
/* R/C channel config */
|
||||
#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */
|
||||
#define PX4IO_PAGE_RC_CONFIG 67 /* 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 */
|
||||
|
|
@ -178,10 +201,10 @@
|
|||
#define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */
|
||||
|
||||
/* PWM output - overrides mixer */
|
||||
#define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
#define PX4IO_PAGE_DIRECT_PWM 68 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
/* PWM failsafe values - zero disables the output */
|
||||
#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
#define PX4IO_PAGE_FAILSAFE_PWM 69 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
/**
|
||||
* As-needed mixer data upload.
|
||||
|
|
|
|||
|
|
@ -142,7 +142,7 @@ user_start(int argc, char *argv[])
|
|||
LED_BLUE(false);
|
||||
LED_SAFETY(false);
|
||||
|
||||
/* turn on servo power */
|
||||
/* turn on servo power (if supported) */
|
||||
POWER_SERVO(true);
|
||||
|
||||
/* start the safety switch handler */
|
||||
|
|
@ -154,13 +154,11 @@ user_start(int argc, char *argv[])
|
|||
/* initialise the control inputs */
|
||||
controls_init();
|
||||
|
||||
#ifdef INTERFACE_I2C
|
||||
/* start the i2c handler */
|
||||
i2c_init();
|
||||
#endif
|
||||
#ifdef INTERFACE_SERIAL
|
||||
/* start the serial interface */
|
||||
#endif
|
||||
/* start the FMU interface */
|
||||
interface_init();
|
||||
|
||||
/* add a performance counter for the interface */
|
||||
perf_counter_t interface_perf = perf_alloc(PC_ELAPSED, "interface");
|
||||
|
||||
/* add a performance counter for mixing */
|
||||
perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
|
||||
|
|
@ -203,6 +201,11 @@ user_start(int argc, char *argv[])
|
|||
/* track the rate at which the loop is running */
|
||||
perf_count(loop_perf);
|
||||
|
||||
/* kick the interface */
|
||||
perf_begin(interface_perf);
|
||||
interface_tick();
|
||||
perf_end(interface_perf);
|
||||
|
||||
/* kick the mixer */
|
||||
perf_begin(mixer_perf);
|
||||
mixer_tick();
|
||||
|
|
|
|||
|
|
@ -125,16 +125,16 @@ extern struct sys_state_s system_state;
|
|||
|
||||
#define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
|
||||
#ifdef GPIO_ACC1_PWR_EN
|
||||
#define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
|
||||
# define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
|
||||
#endif
|
||||
#ifdef GPIO_ACC2_PWR_EN
|
||||
#define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
|
||||
# define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
|
||||
#endif
|
||||
#ifdef GPIO_RELAY1_EN
|
||||
#define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
|
||||
# define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
|
||||
#endif
|
||||
#ifdef GPIO_RELAY2_EN
|
||||
#define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
|
||||
# define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
|
||||
#endif
|
||||
|
||||
#define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT))
|
||||
|
|
@ -156,12 +156,11 @@ extern void mixer_handle_text(const void *buffer, size_t length);
|
|||
*/
|
||||
extern void safety_init(void);
|
||||
|
||||
#ifdef CONFIG_STM32_I2C1
|
||||
/**
|
||||
* FMU communications
|
||||
*/
|
||||
extern void i2c_init(void);
|
||||
#endif
|
||||
extern void interface_init(void);
|
||||
extern void interface_tick(void);
|
||||
|
||||
/**
|
||||
* Register space
|
||||
|
|
@ -190,10 +189,5 @@ extern bool sbus_input(uint16_t *values, uint16_t *num_values);
|
|||
/** global debug level for isr_debug() */
|
||||
extern volatile uint8_t debug_level;
|
||||
|
||||
/* send a debug message to the console */
|
||||
extern void isr_debug(uint8_t level, const char *fmt, ...);
|
||||
|
||||
#ifdef CONFIG_STM32_I2C1
|
||||
void i2c_dump(void);
|
||||
void i2c_reset(void);
|
||||
#endif
|
||||
/** send a debug message to the console */
|
||||
extern void isr_debug(uint8_t level, const char *fmt, ...);
|
||||
|
|
|
|||
|
|
@ -0,0 +1,129 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
* 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 serial.c
|
||||
*
|
||||
* Serial communication for the PX4IO module.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <unistd.h>
|
||||
#include <termios.h>
|
||||
#include <fcntl.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <nuttx/arch.h>
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include <systemlib/hx_stream.h>
|
||||
|
||||
//#define DEBUG
|
||||
#include "px4io.h"
|
||||
|
||||
static uint8_t tx_buf[66]; /* XXX hardcoded magic number */
|
||||
|
||||
static hx_stream_t if_stream;
|
||||
|
||||
static void serial_callback(void *arg, const void *data, unsigned length);
|
||||
|
||||
void
|
||||
interface_init(void)
|
||||
{
|
||||
|
||||
int fd = open("/dev/ttyS1", O_RDWR, O_NONBLOCK);
|
||||
if (fd < 0) {
|
||||
debug("serial fail");
|
||||
return;
|
||||
}
|
||||
|
||||
/* configure serial port - XXX increase port speed? */
|
||||
struct termios t;
|
||||
tcgetattr(fd, &t);
|
||||
cfsetspeed(&t, 115200);
|
||||
t.c_cflag &= ~(CSTOPB | PARENB);
|
||||
tcsetattr(fd, TCSANOW, &t);
|
||||
|
||||
/* allocate the HX stream we'll use for communication */
|
||||
if_stream = hx_stream_init(fd, serial_callback, NULL);
|
||||
|
||||
/* XXX add stream stats counters? */
|
||||
|
||||
debug("serial init");
|
||||
}
|
||||
|
||||
void
|
||||
interface_tick()
|
||||
{
|
||||
/* process incoming bytes */
|
||||
hx_stream_rx(if_stream);
|
||||
}
|
||||
|
||||
static void
|
||||
serial_callback(void *arg, const void *data, unsigned length)
|
||||
{
|
||||
const uint8_t *message = (const uint8_t *)data;
|
||||
|
||||
/* malformed frame, ignore it */
|
||||
if (length < 2)
|
||||
return;
|
||||
|
||||
/* it's a write operation, pass it to the register API */
|
||||
if (message[0] & PX4IO_PAGE_WRITE) {
|
||||
registers_set(message[0] & ~PX4IO_PAGE_WRITE, message[1],
|
||||
(const uint16_t *)&message[2],
|
||||
(length - 2) / 2);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/* it's a read */
|
||||
uint16_t *registers;
|
||||
unsigned count;
|
||||
|
||||
tx_buf[0] = message[0];
|
||||
tx_buf[1] = message[1];
|
||||
|
||||
/* get registers for response, send an empty reply on error */
|
||||
if (registers_get(message[0], message[1], ®isters, &count) < 0)
|
||||
count = 0;
|
||||
|
||||
/* fill buffer with message */
|
||||
#define TX_MAX ((sizeof(tx_buf) - 2) / 2)
|
||||
if (count > TX_MAX)
|
||||
count = TX_MAX;
|
||||
memcpy(&tx_buf[2], registers, count * 2);
|
||||
|
||||
/* try to send the message */
|
||||
hx_stream_send(if_stream, tx_buf, count * 2 + 2);
|
||||
}
|
||||
|
|
@ -76,6 +76,7 @@ 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)
|
||||
|
|
@ -224,13 +225,13 @@ hx_stream_send(hx_stream_t stream,
|
|||
return 0;
|
||||
}
|
||||
|
||||
void
|
||||
hx_stream_rx(hx_stream_t stream, uint8_t c)
|
||||
static bool
|
||||
hx_rx_char(hx_stream_t stream, uint8_t c)
|
||||
{
|
||||
/* frame end? */
|
||||
if (c == FBO) {
|
||||
hx_rx_frame(stream);
|
||||
return;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* escaped? */
|
||||
|
|
@ -241,10 +242,43 @@ hx_stream_rx(hx_stream_t stream, uint8_t c)
|
|||
} else if (c == CEO) {
|
||||
/* now escaped, ignore the byte */
|
||||
stream->escaped = true;
|
||||
return;
|
||||
return false;
|
||||
}
|
||||
|
||||
/* 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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -114,9 +114,25 @@ __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(hx_stream_t stream,
|
||||
__EXPORT extern void hx_stream_rx_char(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
|
||||
|
|
|
|||
Loading…
Reference in New Issue