From 49aca1ae5ba240fc9ae695e58ca392b8d61c939e Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 4 Jul 2013 08:50:34 +0200 Subject: Add in missing files. --- src/drivers/hott/hott_sensors/hott_sensors.c | 232 +++++++++++++++++++++++++++ 1 file changed, 232 insertions(+) create mode 100644 src/drivers/hott/hott_sensors/hott_sensors.c (limited to 'src/drivers/hott/hott_sensors/hott_sensors.c') diff --git a/src/drivers/hott/hott_sensors/hott_sensors.c b/src/drivers/hott/hott_sensors/hott_sensors.c new file mode 100644 index 000000000..dc10685b4 --- /dev/null +++ b/src/drivers/hott/hott_sensors/hott_sensors.c @@ -0,0 +1,232 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * 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 hott_sensors.c + * @author Simon Wilks + * + * Graupner HoTT sensor driver implementation. + * + * Poll any sensors connected to the PX4 via the telemetry wire. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../comms.h" +#include "../messages.h" + +static int thread_should_exit = false; /**< Deamon exit flag */ +static int thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ +static const char daemon_name[] = "hott_sensors"; +static const char commandline_usage[] = "usage: hott_sensor start|status|stop [-d ]"; + +/** + * Deamon management function. + */ +__EXPORT int hott_sensors_main(int argc, char *argv[]); + +/** + * Mainloop of daemon. + */ +int hott_sensors_thread_main(int argc, char *argv[]); + +static int recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id); +static int send_poll(int uart, uint8_t *buffer, size_t size); + +int +send_poll(int uart, uint8_t *buffer, size_t size) +{ + for (size_t i = 0; i < size; i++) { + write(uart, &buffer[i], sizeof(buffer[i])); + + /* Sleep before sending the next byte. */ + usleep(POST_WRITE_DELAY_IN_USECS); + } + + /* A hack the reads out what was written so the next read from the receiver doesn't get it. */ + /* TODO: Fix this!! */ + uint8_t dummy[size]; + read(uart, &dummy, size); + + return OK; +} + +int +recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id) +{ + usleep(5000); + + static const int timeout_ms = 1000; + struct pollfd fds[] = { { .fd = uart, .events = POLLIN } }; + + + // XXX should this poll be inside the while loop??? + if (poll(fds, 1, timeout_ms) > 0) { + int i = 0; + bool stop_byte_read = false; + while (true) { + read(uart, &buffer[i], sizeof(buffer[i])); + //printf("[%d]: %d\n", i, buffer[i]); + + if (stop_byte_read) { + // XXX process checksum + *size = ++i; + return OK; + } + // XXX can some other field not have the STOP BYTE value? + if (buffer[i] == STOP_BYTE) { + *id = buffer[1]; + stop_byte_read = true; + } + i++; + } + } + return ERROR; +} + +int +hott_sensors_thread_main(int argc, char *argv[]) +{ + warnx("starting"); + + thread_running = true; + + const char *device = "/dev/ttyS2"; /**< Default telemetry port: USART5 */ + + /* read commandline arguments */ + for (int i = 0; i < argc && argv[i]; i++) { + if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set + if (argc > i + 1) { + device = argv[i + 1]; + + } else { + thread_running = false; + errx(1, "missing parameter to -d\n%s", commandline_usage); + } + } + } + + /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ + struct termios uart_config_original; + const int uart = open_uart(device, &uart_config_original); + + if (uart < 0) { + errx(1, "Failed opening HoTT UART, exiting."); + thread_running = false; + } + + uint8_t buffer[MESSAGE_BUFFER_SIZE]; + size_t size = 0; + uint8_t id = 0; + bool connected = true; + while (!thread_should_exit) { + // Currently we only support a General Air Module sensor. + build_gam_request(&buffer, &size); + send_poll(uart, buffer, size); + recv_data(uart, &buffer, &size, &id); + + // Determine which moduel sent it and process accordingly. + if (id == GAM_SENSOR_ID) { + //warnx("extract"); + extract_gam_message(buffer); + } else { + warnx("Unknown sensor ID received: %d", id); + } + } + + warnx("exiting"); + + close(uart); + + thread_running = false; + + return 0; +} + +/** + * Process command line arguments and start the daemon. + */ +int +hott_sensors_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "missing command\n%s", commandline_usage); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("deamon already running"); + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn(daemon_name, + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 2048, + hott_sensors_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("daemon is running"); + + } else { + warnx("daemon not started"); + } + + exit(0); + } + + errx(1, "unrecognized command\n%s", commandline_usage); +} -- cgit v1.2.3