diff options
author | px4dev <px4@purgatory.org> | 2013-06-25 23:08:34 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2013-06-25 23:08:34 -0700 |
commit | 90c458cb618754905ab6d373f22d76e3309adf4c (patch) | |
tree | 7d89fa4817e61968c8fbf8bcbf133d8f821197f1 /src/modules | |
parent | 758ebf6c04206d78f817d91ef714ddf78cd8dc43 (diff) | |
download | px4-firmware-90c458cb618754905ab6d373f22d76e3309adf4c.tar.gz px4-firmware-90c458cb618754905ab6d373f22d76e3309adf4c.tar.bz2 px4-firmware-90c458cb618754905ab6d373f22d76e3309adf4c.zip |
Checkpoint: interface abstraction for px4io driver
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/px4iofirmware/protocol.h | 44 | ||||
-rw-r--r-- | src/modules/systemlib/hx_stream.c | 213 | ||||
-rw-r--r-- | src/modules/systemlib/hx_stream.h | 60 |
3 files changed, 178 insertions, 139 deletions
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 90d63ea1a..0e40bff69 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -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. diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c index 88f7f762c..fdc3edac7 100644 --- a/src/modules/systemlib/hx_stream.c +++ b/src/modules/systemlib/hx_stream.c @@ -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_send(hx_stream_t stream, +hx_stream_start(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; - - if (resid > HX_STREAM_MAX_FRAME) + if (count > HX_STREAM_MAX_FRAME) return -EINVAL; - /* start the frame */ - hx_tx_raw(stream, FBO); + 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; - /* transmit the data */ - while (resid--) - hx_tx_byte(stream, *p++); + /* sort out what we're going to send */ + switch (stream->tx_state) { - /* compute the CRC */ - u.w = crc32(data, count); + case TX_SEND_START: + stream->tx_state = TX_SEND_DATA; + return FBO; - /* send the CRC */ - p = &u.b[0]; - resid = 4; + case TX_SEND_DATA: + c = *stream->tx_buf; - while (resid--) - hx_tx_byte(stream, *p++); + 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; - /* and the trailing frame separator */ - hx_tx_raw(stream, FBO); + 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) +{ + int result; + + result = hx_start(stream, data, count); + if (result != OK) + return result; + + 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; } diff --git a/src/modules/systemlib/hx_stream.h b/src/modules/systemlib/hx_stream.h index be4850f74..1f3927222 100644 --- a/src/modules/systemlib/hx_stream.h +++ b/src/modules/systemlib/hx_stream.h @@ -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. @@ -90,6 +92,44 @@ __EXPORT extern void hx_stream_set_counters(hx_stream_t stream, 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. * * This function will block until all frame bytes are sent if @@ -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 |