From 527fe4b37496a498bb8ad920d873f3aca35b761f Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 23 Jan 2016 10:34:03 -0700 Subject: [PATCH] remove sPort_telemetry directory --- src/drivers/frsky_telemetry/CMakeLists.txt | 1 + src/drivers/frsky_telemetry/frsky_telemetry.c | 6 +- src/drivers/sPort_telemetry/CMakeLists.txt | 45 -- src/drivers/sPort_telemetry/sPort_data.c | 216 ---------- src/drivers/sPort_telemetry/sPort_data.h | 87 ---- src/drivers/sPort_telemetry/sPort_telemetry.c | 407 ------------------ 6 files changed, 4 insertions(+), 758 deletions(-) delete mode 100644 src/drivers/sPort_telemetry/CMakeLists.txt delete mode 100644 src/drivers/sPort_telemetry/sPort_data.c delete mode 100644 src/drivers/sPort_telemetry/sPort_data.h delete mode 100644 src/drivers/sPort_telemetry/sPort_telemetry.c diff --git a/src/drivers/frsky_telemetry/CMakeLists.txt b/src/drivers/frsky_telemetry/CMakeLists.txt index 7d6c17f463..a35310aaf7 100644 --- a/src/drivers/frsky_telemetry/CMakeLists.txt +++ b/src/drivers/frsky_telemetry/CMakeLists.txt @@ -38,6 +38,7 @@ px4_add_module( -Os SRCS frsky_data.c + sPort_data.c frsky_telemetry.c DEPENDS platforms__common diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 6661bff8ff..90df63eea0 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -59,13 +59,13 @@ #include #include "sPort_data.h" -#include "../frsky_telemetry/frsky_data.h" +#include "frsky_data.h" /* thread state */ static volatile bool thread_should_exit = false; static volatile bool thread_running = false; -static int sPort_task; +static int frsky_task; /* functions */ static int sPort_open_uart(const char *uart_name, struct termios *uart_config, struct termios *uart_config_original); @@ -377,7 +377,7 @@ int frsky_telemetry_main(int argc, char *argv[]) } thread_should_exit = false; - sPort_task = px4_task_spawn_cmd("sPort_telemetry", + frsky_task = px4_task_spawn_cmd("frsky_telemetry", SCHED_DEFAULT, 200, 2000, diff --git a/src/drivers/sPort_telemetry/CMakeLists.txt b/src/drivers/sPort_telemetry/CMakeLists.txt deleted file mode 100644 index d19291720d..0000000000 --- a/src/drivers/sPort_telemetry/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 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. -# -############################################################################ -px4_add_module( - MODULE drivers__sPort_telemetry - MAIN sPort_telemetry - STACK 1200 - COMPILE_FLAGS - -Os - SRCS - sPort_data.c - sPort_telemetry.c - DEPENDS - platforms__common - ) -# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/sPort_telemetry/sPort_data.c b/src/drivers/sPort_telemetry/sPort_data.c deleted file mode 100644 index 5267f44f61..0000000000 --- a/src/drivers/sPort_telemetry/sPort_data.c +++ /dev/null @@ -1,216 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. - * Author: Stefan Rado - * - * 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 sPort_data.c - * @author Stefan Rado - * - * FrSky SmartPort telemetry implementation. - * - */ - -#include "sPort_data.h" - -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#define frac(f) (f - (int)f) - -static int battery_sub = -1; -static int sensor_sub = -1; -static int global_position_sub = -1; -static int vehicle_status_sub = -1; - -/** - * Initializes the uORB subscriptions. - */ -void sPort_init() -{ - battery_sub = orb_subscribe(ORB_ID(battery_status)); - global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); -} - -/** - * Sends a 0x10 start byte. - */ -//static void sPort_send_start(int uart) -//{ -// static const uint8_t c = 0x10; -// write(uart, &c, 1); -//} - -static void update_crc(uint16_t *crc, unsigned char b) -{ - *crc += b; - *crc += *crc >> 8; - *crc &= 0xFF; -} - -/** - * Sends one byte, performing byte-stuffing if necessary. - */ -static void sPort_send_byte(int uart, uint8_t value) -{ - const uint8_t x7E[] = { 0x7D, 0x5E }; - const uint8_t x7D[] = { 0x7D, 0x5D }; - - switch (value) { - case 0x7E: - write(uart, x7E, sizeof(x7E)); - break; - - case 0x7D: - write(uart, x7D, sizeof(x7D)); - break; - - default: - write(uart, &value, sizeof(value)); - break; - } -} - -/** - * Sends one data id/value pair. - */ -void sPort_send_data(int uart, uint16_t id, uint32_t data) -{ - union { - uint32_t word; - uint8_t byte[4]; - } buf; - - /* send start byte */ - static const uint8_t c = 0x10; - write(uart, &c, 1); - - /* init crc */ - uint16_t crc = c; - - buf.word = id; - - for (int i = 0; i < 2; i++) { - update_crc(&crc, buf.byte[i]); - sPort_send_byte(uart, buf.byte[i]); /* LSB first */ - } - - buf.word = data; - - for (int i = 0; i < 4; i++) { - update_crc(&crc, buf.byte[i]); - sPort_send_byte(uart, buf.byte[i]); /* LSB first */ - } - - sPort_send_byte(uart, 0xFF - crc); -} - - -// scaling correct with OpenTX 2.1.7 -void sPort_send_BATV(int uart) -{ - /* get a local copy of the vehicle status data */ - struct vehicle_status_s vehicle_status; - memset(&vehicle_status, 0, sizeof(vehicle_status)); - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); - - /* send battery voltage as VFAS */ - uint32_t voltage = (int)(100 * vehicle_status.battery_voltage); - sPort_send_data(uart, SMARTPORT_ID_VFAS, voltage); -} - -// verified scaling -void sPort_send_CUR(int uart) -{ - /* get a local copy of the vehicle status data */ - struct vehicle_status_s vehicle_status; - memset(&vehicle_status, 0, sizeof(vehicle_status)); - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); - - /* send data */ - uint32_t current = (int)(10 * vehicle_status.battery_current); - sPort_send_data(uart, SMARTPORT_ID_CURR, current); -} - -// verified scaling for "custom" altitude option -// OpenTX uses the initial reading as field elevation and displays -// the difference (altitude - field) -void sPort_send_ALT(int uart) -{ - /* get a local copy of the current sensor values */ - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); - orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); - - /* send data */ - uint32_t alt = (int)(100 * raw.baro_alt_meter[0]); - sPort_send_data(uart, SMARTPORT_ID_ALT, alt); -} - -// verified scaling for "calculated" option -void sPort_send_SPD(int uart) -{ - /* get a local copy of the global position data */ - struct vehicle_global_position_s global_pos; - memset(&global_pos, 0, sizeof(global_pos)); - orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); - - /* send data for A2 */ - float speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e); - uint32_t ispeed = (int)(10 * speed); - sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed); -} - -// verified scaling -void sPort_send_FUEL(int uart) -{ - /* get a local copy of the vehicle status data */ - struct vehicle_status_s vehicle_status; - memset(&vehicle_status, 0, sizeof(vehicle_status)); - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); - - /* send data */ - uint32_t fuel = (int)(100 * vehicle_status.battery_remaining); - sPort_send_data(uart, SMARTPORT_ID_FUEL, fuel); -} diff --git a/src/drivers/sPort_telemetry/sPort_data.h b/src/drivers/sPort_telemetry/sPort_data.h deleted file mode 100644 index 895fde3465..0000000000 --- a/src/drivers/sPort_telemetry/sPort_data.h +++ /dev/null @@ -1,87 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. - * Author: Stefan Rado - * - * 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 sPort_data.h - * @author Stefan Rado - * - * FrSky SmartPort telemetry implementation. - * - */ -#ifndef _SPORT_DATA_H -#define _SPORT_DATA_H - -#include - -/* FrSky SmartPort polling IDs captured from X4R */ -#define SMARTPORT_POLL_1 0x1B -#define SMARTPORT_POLL_2 0x34 -#define SMARTPORT_POLL_3 0x95 -#define SMARTPORT_POLL_4 0x16 -#define SMARTPORT_POLL_5 0xB7 - -/* FrSky SmartPort sensor IDs */ -#define SMARTPORT_ID_RSSI 0xf101 -#define SMARTPORT_ID_RXA1 0xf102 // supplied by RX -#define SMARTPORT_ID_RXA2 0xf103 // supplied by RX -#define SMARTPORT_ID_BATV 0xf104 -#define SMARTPORT_ID_SWR 0xf105 -#define SMARTPORT_ID_T1 0x0400 -#define SMARTPORT_ID_T2 0x0410 -#define SMARTPORT_ID_RPM 0x0500 -#define SMARTPORT_ID_FUEL 0x0600 -#define SMARTPORT_ID_ALT 0x0100 -#define SMARTPORT_ID_VARIO 0x0110 -#define SMARTPORT_ID_ACCX 0x0700 -#define SMARTPORT_ID_ACCY 0x0710 -#define SMARTPORT_ID_ACCZ 0x0720 -#define SMARTPORT_ID_CURR 0x0200 -#define SMARTPORT_ID_VFAS 0x0210 -#define SMARTPORT_ID_CELLS 0x0300 -#define SMARTPORT_ID_GPS_LON_LAT 0x0800 -#define SMARTPORT_ID_GPS_ALT 0x0820 -#define SMARTPORT_ID_GPS_SPD 0x0830 -#define SMARTPORT_ID_GPS_CRS 0x0840 -#define SMARTPORT_ID_GPS_TIME 0x0850 - -// Public functions -void sPort_init(void); -void sPort_send_data(int uart, uint16_t id, uint32_t data); -void sPort_send_BATV(int uart); -void sPort_send_CUR(int uart); -void sPort_send_ALT(int uart); -void sPort_send_SPD(int uart); -void sPort_send_FUEL(int uart); - -#endif /* _SPORT_TELEMETRY_H */ diff --git a/src/drivers/sPort_telemetry/sPort_telemetry.c b/src/drivers/sPort_telemetry/sPort_telemetry.c deleted file mode 100644 index 943f3c89fc..0000000000 --- a/src/drivers/sPort_telemetry/sPort_telemetry.c +++ /dev/null @@ -1,407 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. - * Author: Stefan Rado - * - * 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 sPort_telemetry.c - * @author Mark Whitehorn - * @author Stefan Rado - * - * FrSky SmartPort telemetry implementation. - * - * This daemon emulates FrSky SmartPort sensors by responding to polling - * packets received from an attached FrSky X series receiver. - * - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "sPort_data.h" -#include "../frsky_telemetry/frsky_data.h" - - -/* thread state */ -static volatile bool thread_should_exit = false; -static volatile bool thread_running = false; -static int sPort_task; - -/* functions */ -static int sPort_open_uart(const char *uart_name, struct termios *uart_config, struct termios *uart_config_original); -static void usage(void); -static int sPort_telemetry_thread_main(int argc, char *argv[]); -__EXPORT int sPort_telemetry_main(int argc, char *argv[]); - - -/** - * Opens the UART device and sets all required serial parameters. - */ -static int sPort_open_uart(const char *uart_name, struct termios *uart_config, struct termios *uart_config_original) -{ - /* Open UART */ - const int uart = open(uart_name, O_RDWR | O_NOCTTY | O_NONBLOCK); - - if (uart < 0) { - err(1, "Error opening port: %s", uart_name); - } - - /* Back up the original UART configuration to restore it after exit */ - int termios_state; - - if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - warnx("ERR: tcgetattr%s: %d\n", uart_name, termios_state); - close(uart); - return -1; - } - - /* Fill the struct for the new configuration */ - tcgetattr(uart, uart_config); - - /* Disable output post-processing */ - uart_config->c_oflag &= ~OPOST; - - /* Set baud rate */ - static const speed_t speed = B57600; - - if (cfsetispeed(uart_config, speed) < 0 || cfsetospeed(uart_config, speed) < 0) { - warnx("ERR: %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); - close(uart); - return -1; - } - - if ((termios_state = tcsetattr(uart, TCSANOW, uart_config)) < 0) { - warnx("ERR: %s (tcsetattr)\n", uart_name); - close(uart); - return -1; - } - - return uart; -} - -/** - * Print command usage information - */ -static void usage() -{ - fprintf(stderr, - "usage: sPort_telemetry start [-d ]\n" - " sPort_telemetry stop\n" - " sPort_telemetry status\n"); - exit(1); -} - -/** - * The daemon thread. - */ -static int sPort_telemetry_thread_main(int argc, char *argv[]) -{ - /* Default values for arguments */ - char *device_name = "/dev/ttyS6"; /* USART8 */ - - /* Work around some stupidity in task_create's argv handling */ - argc -= 2; - argv += 2; - - int ch; - - while ((ch = getopt(argc, argv, "d:")) != EOF) { - switch (ch) { - case 'd': - device_name = optarg; - break; - - default: - usage(); - break; - } - } - - /* Open UART */ - warnx("opening uart"); - struct termios uart_config_original; - struct termios uart_config; - const int uart = sPort_open_uart(device_name, &uart_config, &uart_config_original); - - if (uart < 0) { - warnx("could not open %s", device_name); - err(1, "could not open %s", device_name); - } - - /* poll descriptor */ - struct pollfd fds[1]; - fds[0].fd = uart; - fds[0].events = POLLIN; - - thread_running = true; - - /* Main thread loop */ - char sbuf[20]; - - /* look for polling frames indicating SmartPort telemetry - * if not found, send D type telemetry frames instead - */ - int status = poll(fds, sizeof(fds) / sizeof(fds[0]), 3000); - - if (status < 1) { - /* timed out: reconfigure UART and send D type telemetry */ - warnx("sending FrSky D type telemetry"); - - if (cfsetispeed(&uart_config, B9600) < 0 || cfsetospeed(&uart_config, B9600) < 0) { - warnx("ERR: %s:(cfsetispeed, cfsetospeed)\n", device_name); - close(uart); - return -1; - } - - if (tcsetattr(uart, TCSANOW, &uart_config) < 0) { - warnx("ERR: %s (tcsetattr)\n", device_name); - close(uart); - return -1; - } - - int iteration = 0; - - /* Subscribe to topics */ - frsky_init(); - - while (!thread_should_exit) { - - /* Sleep 200 ms */ - usleep(200000); - - /* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */ - frsky_send_frame1(uart); - - /* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */ - if (iteration % 5 == 0) { - frsky_send_frame2(uart); - } - - /* Send frame 3 (every 5000ms): date, time */ - if (iteration % 25 == 0) { - frsky_send_frame3(uart); - - iteration = 0; - } - - iteration++; - } - - } else { - /* Subscribe to topics */ - sPort_init(); - - /* send S.port telemetry */ - while (!thread_should_exit) { - - /* wait for poll frame starting with value 0x7E - * note that only the bus master is supposed to put a 0x7E on the bus. - * slaves use byte stuffing to send 0x7E and 0x7D. - * we expect a poll frame every 12msec - */ - int status = poll(fds, sizeof(fds) / sizeof(fds[0]), 50); - - if (status < 1) { continue; } - - // read 1 byte - int newBytes = read(uart, &sbuf[0], 1); - - if (newBytes < 1 || sbuf[0] != 0x7E) { continue; } - - /* wait for ID byte */ - status = poll(fds, sizeof(fds) / sizeof(fds[0]), 50); - - if (status < 1) { continue; } - - hrt_abstime now = hrt_absolute_time(); - - newBytes = read(uart, &sbuf[1], 1); - - // allow a minimum of 500usec before reply - usleep(500); - - static hrt_abstime lastBATV = 0; - static hrt_abstime lastCUR = 0; - static hrt_abstime lastALT = 0; - static hrt_abstime lastSPD = 0; - static hrt_abstime lastFUEL = 0; - - switch (sbuf[1]) { - - case SMARTPORT_POLL_1: - - /* report BATV at 1Hz */ - if (now - lastBATV > 1000 * 1000) { - lastBATV = now; - /* send battery voltage */ - sPort_send_BATV(uart); - } - - break; - - - case SMARTPORT_POLL_2: - - /* report battery current at 5Hz */ - if (now - lastCUR > 200 * 1000) { - lastCUR = now; - /* send battery current */ - sPort_send_CUR(uart); - } - - break; - - - case SMARTPORT_POLL_3: - - /* report altitude at 5Hz */ - if (now - lastALT > 200 * 1000) { - lastALT = now; - /* send altitude */ - sPort_send_ALT(uart); - } - - break; - - - case SMARTPORT_POLL_4: - - /* report speed at 5Hz */ - if (now - lastSPD > 200 * 1000) { - lastSPD = now; - /* send speed */ - sPort_send_SPD(uart); - } - - break; - - case SMARTPORT_POLL_5: - - /* report fuel at 1Hz */ - if (now - lastFUEL > 1000 * 1000) { - lastFUEL = now; - /* send fuel */ - sPort_send_FUEL(uart); - } - - break; - - } - - /* TODO: flush the input buffer if in full duplex mode */ - read(uart, &sbuf[0], sizeof(sbuf)); - } - } - - /* Reset the UART flags to original state */ - tcsetattr(uart, TCSANOW, &uart_config_original); - close(uart); - - thread_running = false; - return 0; -} - -/** - * The main command function. - * Processes command line arguments and starts the daemon. - */ -int sPort_telemetry_main(int argc, char *argv[]) -{ - if (argc < 2) { - warnx("missing command"); - usage(); - } - - if (!strcmp(argv[1], "start")) { - - /* this is not an error */ - if (thread_running) { - errx(0, "sPort_telemetry already running"); - } - - thread_should_exit = false; - sPort_task = px4_task_spawn_cmd("sPort_telemetry", - SCHED_DEFAULT, - 200, - 2000, - sPort_telemetry_thread_main, - (char *const *)argv); - - while (!thread_running) { - usleep(200); - } - - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - - /* this is not an error */ - if (!thread_running) { - errx(0, "sPort_telemetry already stopped"); - } - - thread_should_exit = true; - - while (thread_running) { - usleep(200000); - warnx("."); - } - - warnx("terminated."); - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - errx(0, "running"); - - } else { - errx(1, "not running"); - } - } - - warnx("unrecognized command"); - usage(); - /* not getting here */ - return 0; -}