diff options
author | px4dev <px4@purgatory.org> | 2013-04-28 18:14:46 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2013-04-28 18:14:46 -0700 |
commit | e67022f874f0fa091ed7dffd617c70c0c0253b5c (patch) | |
tree | 4308fd72c7dc6f782d464ac5886b226c371fd74d /src/modules/px4iofirmware/serial.c | |
parent | 8f7200e0114d6bd9fcac7ec088b125e54761c2e0 (diff) | |
download | px4-firmware-e67022f874f0fa091ed7dffd617c70c0c0253b5c.tar.gz px4-firmware-e67022f874f0fa091ed7dffd617c70c0c0253b5c.tar.bz2 px4-firmware-e67022f874f0fa091ed7dffd617c70c0c0253b5c.zip |
Serial interface for IOv2
Diffstat (limited to 'src/modules/px4iofirmware/serial.c')
-rw-r--r-- | src/modules/px4iofirmware/serial.c | 129 |
1 files changed, 129 insertions, 0 deletions
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 |