diff options
-rw-r--r-- | src/modules/px4iofirmware/i2c.c | 15 | ||||
-rw-r--r-- | src/modules/px4iofirmware/module.mk | 10 | ||||
-rw-r--r-- | src/modules/px4iofirmware/protocol.h | 39 | ||||
-rw-r--r-- | src/modules/px4iofirmware/px4io.c | 19 | ||||
-rw-r--r-- | src/modules/px4iofirmware/px4io.h | 22 | ||||
-rw-r--r-- | src/modules/px4iofirmware/serial.c | 129 | ||||
-rw-r--r-- | src/modules/systemlib/hx_stream.c | 42 | ||||
-rw-r--r-- | src/modules/systemlib/hx_stream.h | 18 |
8 files changed, 249 insertions, 45 deletions
diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c index 4485daa5b..10aeb5c9f 100644 --- a/src/modules/px4iofirmware/i2c.c +++ b/src/modules/px4iofirmware/i2c.c @@ -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); diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk index 016be6d3b..4dd1aa8d7 100644 --- a/src/modules/px4iofirmware/module.mk +++ b/src/modules/px4iofirmware/module.mk @@ -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 diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 8d8b7e941..1a687bd2a 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -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. diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 39f05c112..385920d53 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -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(); diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 272cdb7bf..2077e6244 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -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, ...); diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c new file mode 100644 index 000000000..a12d58aca --- /dev/null +++ b/src/modules/px4iofirmware/serial.c @@ -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); +}
\ No newline at end of file diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c index 8d77f14a8..88f7f762c 100644 --- a/src/modules/systemlib/hx_stream.c +++ b/src/modules/systemlib/hx_stream.c @@ -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; } diff --git a/src/modules/systemlib/hx_stream.h b/src/modules/systemlib/hx_stream.h index 128689953..be4850f74 100644 --- a/src/modules/systemlib/hx_stream.h +++ b/src/modules/systemlib/hx_stream.h @@ -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 |