From 214709dc897ffe9154090931511bdb3dbde19ce3 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sun, 17 Jan 2016 15:32:47 -0700 Subject: [PATCH] move smartport telemetry to new module --- src/drivers/sPort_telemetry/CMakeLists.txt | 45 +++ src/drivers/sPort_telemetry/sPort_data.c | 311 ++++++++++++++++++ .../sPort_data.h | 14 +- .../sPort_telemetry.c | 92 +++--- 4 files changed, 415 insertions(+), 47 deletions(-) create mode 100644 src/drivers/sPort_telemetry/CMakeLists.txt create mode 100644 src/drivers/sPort_telemetry/sPort_data.c rename src/drivers/{frsky_telemetry => sPort_telemetry}/sPort_data.h (90%) rename src/drivers/{frsky_telemetry => sPort_telemetry}/sPort_telemetry.c (74%) diff --git a/src/drivers/sPort_telemetry/CMakeLists.txt b/src/drivers/sPort_telemetry/CMakeLists.txt new file mode 100644 index 0000000000..d19291720d --- /dev/null +++ b/src/drivers/sPort_telemetry/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# 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 new file mode 100644 index 0000000000..a5f4cdcbf7 --- /dev/null +++ b/src/drivers/sPort_telemetry/sPort_data.c @@ -0,0 +1,311 @@ +/**************************************************************************** + * + * 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 + +/* FrSky SmartPort sensor IDs */ +#define FRSKY_ID_GPS_ALT_BP 0x01 +#define FRSKY_ID_TEMP1 0x02 +#define FRSKY_ID_RPM 0x03 +#define FRSKY_ID_FUEL 0x04 +#define FRSKY_ID_TEMP2 0x05 +#define FRSKY_ID_VOLTS 0x06 +#define FRSKY_ID_GPS_ALT_AP 0x09 +#define FRSKY_ID_BARO_ALT_BP 0x10 +#define FRSKY_ID_GPS_SPEED_BP 0x11 +#define FRSKY_ID_GPS_LONG_BP 0x12 +#define FRSKY_ID_GPS_LAT_BP 0x13 +#define FRSKY_ID_GPS_COURS_BP 0x14 +#define FRSKY_ID_GPS_DAY_MONTH 0x15 +#define FRSKY_ID_GPS_YEAR 0x16 +#define FRSKY_ID_GPS_HOUR_MIN 0x17 +#define FRSKY_ID_GPS_SEC 0x18 +#define FRSKY_ID_GPS_SPEED_AP 0x19 +#define FRSKY_ID_GPS_LONG_AP 0x1A +#define FRSKY_ID_GPS_LAT_AP 0x1B +#define FRSKY_ID_GPS_COURS_AP 0x1C +#define FRSKY_ID_BARO_ALT_AP 0x21 +#define FRSKY_ID_GPS_LONG_EW 0x22 +#define FRSKY_ID_GPS_LAT_NS 0x23 +#define FRSKY_ID_ACCEL_X 0x24 +#define FRSKY_ID_ACCEL_Y 0x25 +#define FRSKY_ID_ACCEL_Z 0x26 +#define FRSKY_ID_CURRENT 0x28 +#define FRSKY_ID_VARIO 0x30 +#define FRSKY_ID_VFAS 0x39 +#define FRSKY_ID_VOLTS_BP 0x3A +#define FRSKY_ID_VOLTS_AP 0x3B + +#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, sizeof(c)); +} + +static void update_crc(uint16_t *crc) +{ + *crc += 0x10; + *crc += *crc >> 8; + *crc &= 0xFF; + *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 0x5E: + write(uart, x7E, sizeof(x7E)); + break; + + case 0x5D: + 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 { + uint16_t word; + uint8_t byte[2]; + } wbuf; + + uint16_t crc = 0; + sPort_send_start(uart); + + wbuf.word = id; + + for (int i = 0; i < 2; i++) { + update_crc(&crc); + sPort_send_byte(uart, wbuf.byte[i]); /* LSB first */ + } + + uint8_t *bbuf = (uint8_t *)data; + + for (int i = 0; i < 4; i++) { + update_crc(&crc); + sPort_send_byte(uart, bbuf[i]); /* LSB first */ + } +} + +#ifdef xxxx +/** + * Sends frame 1 (every 200ms): + * acceleration values, barometer altitude, temperature, battery voltage & current + */ +void sPort_send_frame1(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); + + /* get a local copy of the battery data */ + struct battery_status_s battery; + memset(&battery, 0, sizeof(battery)); + orb_copy(ORB_ID(battery_status), battery_sub, &battery); + + /* send formatted frame */ + sPort_send_data(uart, FRSKY_ID_ACCEL_X, + roundf(raw.accelerometer_m_s2[0] * 1000.0f)); + sPort_send_data(uart, FRSKY_ID_ACCEL_Y, + roundf(raw.accelerometer_m_s2[1] * 1000.0f)); + sPort_send_data(uart, FRSKY_ID_ACCEL_Z, + roundf(raw.accelerometer_m_s2[2] * 1000.0f)); + + sPort_send_data(uart, FRSKY_ID_BARO_ALT_BP, + raw.baro_alt_meter[0]); + sPort_send_data(uart, FRSKY_ID_BARO_ALT_AP, + roundf(frac(raw.baro_alt_meter[0]) * 100.0f)); + + sPort_send_data(uart, FRSKY_ID_TEMP1, + roundf(raw.baro_temp_celcius[0])); + + sPort_send_data(uart, FRSKY_ID_VFAS, + roundf(battery.voltage_v * 10.0f)); + sPort_send_data(uart, FRSKY_ID_CURRENT, + (battery.current_a < 0) ? 0 : roundf(battery.current_a * 10.0f)); + + sPort_send_start(uart); +} + +/** + * Formats the decimal latitude/longitude to the required degrees/minutes. + */ +static float sPort_format_gps(float dec) +{ + float dm_deg = (int) dec; + return (dm_deg * 100.0f) + (dec - dm_deg) * 60; +} + +/** + * Sends frame 2 (every 1000ms): + * GPS course, latitude, longitude, ground speed, GPS altitude, remaining battery level + */ +void sPort_send_frame2(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); + + /* 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 formatted frame */ + float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; + char lat_ns = 0, lon_ew = 0; + int sec = 0; + + if (global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { + time_t time_gps = global_pos.time_utc_usec / 1000000ULL; + struct tm *tm_gps = gmtime(&time_gps); + + course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f; + lat = sPort_format_gps(fabsf(global_pos.lat)); + lat_ns = (global_pos.lat < 0) ? 'S' : 'N'; + lon = sPort_format_gps(fabsf(global_pos.lon)); + lon_ew = (global_pos.lon < 0) ? 'W' : 'E'; + speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e) + * 25.0f / 46.0f; + alt = global_pos.alt; + sec = tm_gps->tm_sec; + } + + sPort_send_data(uart, FRSKY_ID_GPS_COURS_BP, course); + sPort_send_data(uart, FRSKY_ID_GPS_COURS_AP, frac(course) * 1000.0f); + + sPort_send_data(uart, FRSKY_ID_GPS_LAT_BP, lat); + sPort_send_data(uart, FRSKY_ID_GPS_LAT_AP, frac(lat) * 10000.0f); + sPort_send_data(uart, FRSKY_ID_GPS_LAT_NS, lat_ns); + + sPort_send_data(uart, FRSKY_ID_GPS_LONG_BP, lon); + sPort_send_data(uart, FRSKY_ID_GPS_LONG_AP, frac(lon) * 10000.0f); + sPort_send_data(uart, FRSKY_ID_GPS_LONG_EW, lon_ew); + + sPort_send_data(uart, FRSKY_ID_GPS_SPEED_BP, speed); + sPort_send_data(uart, FRSKY_ID_GPS_SPEED_AP, frac(speed) * 100.0f); + + sPort_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt); + sPort_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f); + + sPort_send_data(uart, FRSKY_ID_FUEL, + roundf(vehicle_status.battery_remaining * 100.0f)); + + sPort_send_data(uart, FRSKY_ID_GPS_SEC, sec); + + sPort_send_start(uart); +} + +/** + * Sends frame 3 (every 5000ms): + * GPS date & time + */ +void sPort_send_frame3(int uart) +{ + /* get a local copy of the battery 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 formatted frame */ + time_t time_gps = global_pos.time_utc_usec / 1000000ULL; + struct tm *tm_gps = gmtime(&time_gps); + uint16_t hour_min = (tm_gps->tm_min << 8) | (tm_gps->tm_hour & 0xff); + sPort_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, tm_gps->tm_mday); + sPort_send_data(uart, FRSKY_ID_GPS_YEAR, tm_gps->tm_year); + sPort_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, hour_min); + sPort_send_data(uart, FRSKY_ID_GPS_SEC, tm_gps->tm_sec); + + sPort_send_start(uart); +} +#endif // xxxx diff --git a/src/drivers/frsky_telemetry/sPort_data.h b/src/drivers/sPort_telemetry/sPort_data.h similarity index 90% rename from src/drivers/frsky_telemetry/sPort_data.h rename to src/drivers/sPort_telemetry/sPort_data.h index b5840af641..07088b16ee 100644 --- a/src/drivers/frsky_telemetry/sPort_data.h +++ b/src/drivers/sPort_telemetry/sPort_data.h @@ -39,13 +39,13 @@ * FrSky SmartPort telemetry implementation. * */ -#ifndef _FRSKY_DATA_H -#define _FRSKY_DATA_H +#ifndef _SPORT_DATA_H +#define _SPORT_DATA_H + +#include // Public functions -void frsky_init(void); -void frsky_send_frame1(int uart); -void frsky_send_frame2(int uart); -void frsky_send_frame3(int uart); +void sPort_init(void); +void sPort_send_data(int uart, uint16_t id, uint32_t data); -#endif /* _FRSKY_TELEMETRY_H */ +#endif /* _SPORT_TELEMETRY_H */ diff --git a/src/drivers/frsky_telemetry/sPort_telemetry.c b/src/drivers/sPort_telemetry/sPort_telemetry.c similarity index 74% rename from src/drivers/frsky_telemetry/sPort_telemetry.c rename to src/drivers/sPort_telemetry/sPort_telemetry.c index 3a85f12e48..dbf9d0068c 100644 --- a/src/drivers/frsky_telemetry/sPort_telemetry.c +++ b/src/drivers/sPort_telemetry/sPort_telemetry.c @@ -33,7 +33,8 @@ ****************************************************************************/ /** - * @file frsky_telemetry.c + * @file sPort_telemetry.c + * @author Mark Whitehorn * @author Stefan Rado * * FrSky SmartPort telemetry implementation. @@ -54,33 +55,44 @@ #include #include -#include "frsky_data.h" +#include "sPort_data.h" /* thread state */ static volatile bool thread_should_exit = false; static volatile bool thread_running = false; -static int frsky_task; +static int sPort_task; /* functions */ -static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original); +static int sPort_open_uart(const char *uart_name, struct termios *uart_config_original); static void usage(void); -static int frsky_telemetry_thread_main(int argc, char *argv[]); -__EXPORT int frsky_telemetry_main(int argc, char *argv[]); +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 frsky_open_uart(const char *uart_name, struct termios *uart_config_original) +static int sPort_open_uart(const char *uart_name, struct termios *uart_config_original) { /* Open UART */ - const int uart = open(uart_name, O_WRONLY | O_NOCTTY); + const int uart = open(uart_name, O_RDWR | O_NOCTTY); if (uart < 0) { err(1, "Error opening port: %s", uart_name); } + /* make sure uart FD is blocking */ + int flags = fcntl(uart, F_GETFL); + if (flags < 0) { + err(1, "Error getting FD flags: %s", uart_name); + } + flags &= ~O_NONBLOCK; + flags = fcntl(uart, F_SETFL, flags); + if (flags < 0) { + err(1, "Error setting FD flags: %s", uart_name); + } + /* Back up the original UART configuration to restore it after exit */ int termios_state; @@ -98,7 +110,7 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or uart_config.c_oflag &= ~OPOST; /* Set baud rate */ - static const speed_t speed = B9600; + 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); @@ -121,16 +133,16 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or static void usage() { fprintf(stderr, - "usage: frsky_telemetry start [-d ]\n" - " frsky_telemetry stop\n" - " frsky_telemetry status\n"); + "usage: sPort_telemetry start [-d ]\n" + " sPort_telemetry stop\n" + " sPort_telemetry status\n"); exit(1); } /** * The daemon thread. */ -static int frsky_telemetry_thread_main(int argc, char *argv[]) +static int sPort_telemetry_thread_main(int argc, char *argv[]) { /* Default values for arguments */ char *device_name = "/dev/ttyS1"; /* USART2 */ @@ -154,42 +166,42 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) } /* Open UART */ + warnx("opening uart"); struct termios uart_config_original; - const int uart = frsky_open_uart(device_name, &uart_config_original); + const int uart = sPort_open_uart(device_name, &uart_config_original); if (uart < 0) { + warnx("could not open %s", device_name); err(1, "could not open %s", device_name); } /* Subscribe to topics */ - frsky_init(); + sPort_init(); thread_running = true; /* Main thread loop */ - unsigned int iteration = 0; + char sbuf[2]; + uint8_t fiftyfive = 0x55; 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++; + /* wait for poll frame starting with value 0x7E */ + int newBytes = read(uart, &sbuf[0], 1); +// warnx("%x, %x \n", sbuf[0], sbuf[1]); + + if (newBytes < 1 || sbuf[0] != 0x7E) continue; + + /* read the ID byte */ + sbuf[1] = read(uart, &sbuf[0], 1); + +// sPort_send_data(uart, id, 7); + + /*** test ***/ + /* write single byte */ + write(uart, &fiftyfive, 1); + /* read it back */ + read(uart, &sbuf[0], 1); } /* Reset the UART flags to original state */ @@ -204,7 +216,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) * The main command function. * Processes command line arguments and starts the daemon. */ -int frsky_telemetry_main(int argc, char *argv[]) +int sPort_telemetry_main(int argc, char *argv[]) { if (argc < 2) { warnx("missing command"); @@ -215,15 +227,15 @@ int frsky_telemetry_main(int argc, char *argv[]) /* this is not an error */ if (thread_running) { - errx(0, "frsky_telemetry already running"); + errx(0, "sPort_telemetry already running"); } thread_should_exit = false; - frsky_task = px4_task_spawn_cmd("frsky_telemetry", + sPort_task = px4_task_spawn_cmd("sPort_telemetry", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, - frsky_telemetry_thread_main, + sPort_telemetry_thread_main, (char *const *)argv); while (!thread_running) { @@ -237,7 +249,7 @@ int frsky_telemetry_main(int argc, char *argv[]) /* this is not an error */ if (!thread_running) { - errx(0, "frsky_telemetry already stopped"); + errx(0, "sPort_telemetry already stopped"); } thread_should_exit = true;