diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-05 20:11:44 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-05 20:11:44 +0200 |
commit | 86e5d39b89a95ba81cfbbadc22a5ebc03f068509 (patch) | |
tree | db65deb9e716dee3272c8d045ca9d8567d70e23a /src/modules | |
parent | cac4a6f5789ce4fff8ba0f697777b6d59e0fb068 (diff) | |
parent | 901a9c3e35456445465e7008d3c69b0bd3481e9e (diff) | |
download | px4-firmware-86e5d39b89a95ba81cfbbadc22a5ebc03f068509.tar.gz px4-firmware-86e5d39b89a95ba81cfbbadc22a5ebc03f068509.tar.bz2 px4-firmware-86e5d39b89a95ba81cfbbadc22a5ebc03f068509.zip |
Merge branch 'master' of github.com:PX4/Firmware into mpu6k_queue
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/systemlib/hx_stream.c | 185 | ||||
-rw-r--r-- | src/modules/systemlib/hx_stream.h | 42 | ||||
-rw-r--r-- | src/modules/systemlib/perf_counter.c | 41 | ||||
-rw-r--r-- | src/modules/systemlib/perf_counter.h | 14 |
4 files changed, 211 insertions, 71 deletions
diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c index 8d77f14a8..8e9c2bfcf 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; @@ -81,21 +93,7 @@ static void hx_tx_raw(hx_stream_t stream, uint8_t c) { if (write(stream->fd, &c, 1) != 1) - stream->txerror = true; -} - -static void -hx_tx_byte(hx_stream_t stream, uint8_t c) -{ - switch (c) { - case FBO: - case CEO: - hx_tx_raw(stream, CEO); - c ^= 0x20; - break; - } - - hx_tx_raw(stream, c); + stream->tx_error = true; } static int @@ -105,11 +103,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) { @@ -122,11 +120,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; } @@ -134,7 +132,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; } @@ -150,8 +148,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; @@ -179,49 +177,112 @@ 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; + + /* sort out what we're going to send */ + switch (stream->tx_state) { - /* transmit the data */ - while (resid--) - hx_tx_byte(stream, *p++); + case TX_SEND_START: + stream->tx_state = TX_SEND_DATA; + return FBO; - /* compute the CRC */ - u.w = crc32(data, count); + case TX_SEND_DATA: + c = *stream->tx_buf; - /* send the CRC */ - p = &u.b[0]; - resid = 4; + 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) +{ + int result; - while (resid--) - hx_tx_byte(stream, *p++); + result = hx_stream_start(stream, data, count); + if (result != OK) + return result; - /* and the trailing frame separator */ - hx_tx_raw(stream, FBO); + int c; + while ((c = hx_stream_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; } void @@ -234,17 +295,17 @@ hx_stream_rx(hx_stream_t stream, uint8_t c) } /* 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; + /* 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; + 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 128689953..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 diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index 879f4715a..3c1e10287 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -201,23 +201,50 @@ perf_end(perf_counter_t handle) switch (handle->type) { case PC_ELAPSED: { struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; - hrt_abstime elapsed = hrt_absolute_time() - pce->time_start; - pce->event_count++; - pce->time_total += elapsed; + if (pce->time_start != 0) { + hrt_abstime elapsed = hrt_absolute_time() - pce->time_start; - if ((pce->time_least > elapsed) || (pce->time_least == 0)) - pce->time_least = elapsed; + pce->event_count++; + pce->time_total += elapsed; - if (pce->time_most < elapsed) - pce->time_most = elapsed; + if ((pce->time_least > elapsed) || (pce->time_least == 0)) + pce->time_least = elapsed; + + if (pce->time_most < elapsed) + pce->time_most = elapsed; + + pce->time_start = 0; + } + } + break; + + default: + break; + } +} + +void +perf_cancel(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_ELAPSED: { + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + + pce->time_start = 0; } + break; default: break; } } + + void perf_reset(perf_counter_t handle) { diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index 5c2cb15b2..4cd8b67a1 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -92,13 +92,25 @@ __EXPORT extern void perf_begin(perf_counter_t handle); * End a performance event. * * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * If a call is made without a corresopnding perf_begin call, or if perf_cancel + * has been called subsequently, no change is made to the counter. * * @param handle The handle returned from perf_alloc. */ __EXPORT extern void perf_end(perf_counter_t handle); /** - * Reset a performance event. + * Cancel a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * It reverts the effect of a previous perf_begin. + * + * @param handle The handle returned from perf_alloc. + */ +__EXPORT extern void perf_cancel(perf_counter_t handle); + +/** + * Reset a performance counter. * * This call resets performance counter to initial state * |