aboutsummaryrefslogtreecommitdiff
path: root/src/modules
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules')
-rw-r--r--src/modules/px4iofirmware/protocol.h44
-rw-r--r--src/modules/systemlib/hx_stream.c213
-rw-r--r--src/modules/systemlib/hx_stream.h60
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
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-09 19:47:28 +0000