From c81c34b147ee0f654a241d23b5444f948854bc38 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 23 Jan 2016 09:57:28 -0700 Subject: [PATCH] add sPort telemetry handling into frsky_telemetry daemon --- src/drivers/frsky_telemetry/sPort_data.c | 217 +++++++++++++++++++++++ src/drivers/frsky_telemetry/sPort_data.h | 87 +++++++++ 2 files changed, 304 insertions(+) create mode 100644 src/drivers/frsky_telemetry/sPort_data.c create mode 100644 src/drivers/frsky_telemetry/sPort_data.h diff --git a/src/drivers/frsky_telemetry/sPort_data.c b/src/drivers/frsky_telemetry/sPort_data.c new file mode 100644 index 0000000000..6af5ecc09e --- /dev/null +++ b/src/drivers/frsky_telemetry/sPort_data.c @@ -0,0 +1,217 @@ +/**************************************************************************** + * + * 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 + * @author Mark Whitehorn + * + * 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/frsky_telemetry/sPort_data.h b/src/drivers/frsky_telemetry/sPort_data.h new file mode 100644 index 0000000000..8173780b88 --- /dev/null +++ b/src/drivers/frsky_telemetry/sPort_data.h @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * 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 Mark Whitehorn + * + * 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 */