From 918473d014e155f80b808f3fdfe8e9bdfac244de Mon Sep 17 00:00:00 2001 From: Stephen Oliver Date: Thu, 2 Apr 2020 00:19:15 -0400 Subject: [PATCH] Add support for Smartport telemetry Not tested with a real telemetry source yet. This does not require any setting to change the telemetry protocol in use, because the ground station uses different ports already, we just listen on each of them in the different telemetry classes and everything will just work. Ref: #17 --- QOpenHD.pro | 2 + inc/smartporttelemetry.h | 59 +++++++ qml/main.qml | 4 + src/main.cpp | 2 + src/smartporttelemetry.cpp | 307 +++++++++++++++++++++++++++++++++++++ 5 files changed, 374 insertions(+) create mode 100755 inc/smartporttelemetry.h create mode 100755 src/smartporttelemetry.cpp diff --git a/QOpenHD.pro b/QOpenHD.pro index e68b8d2e8..dd81c91f7 100755 --- a/QOpenHD.pro +++ b/QOpenHD.pro @@ -68,6 +68,7 @@ SOURCES += \ src/openhdtelemetry.cpp \ src/powermicroservice.cpp \ src/qopenhdlink.cpp \ + src/smartporttelemetry.cpp \ src/util.cpp \ src/vectortelemetry.cpp @@ -93,6 +94,7 @@ HEADERS += \ inc/openhdsettings.h \ inc/openhdtelemetry.h \ inc/qopenhdlink.h \ + inc/smartporttelemetry.h \ inc/util.h \ inc/vectortelemetry.h \ inc/wifibroadcast.h \ diff --git a/inc/smartporttelemetry.h b/inc/smartporttelemetry.h new file mode 100755 index 000000000..a3a9e35d5 --- /dev/null +++ b/inc/smartporttelemetry.h @@ -0,0 +1,59 @@ +#ifndef SMARTPORTTELEMETRY_H +#define SMARTPORTTELEMETRY_H + +#include +#include + +#include "constants.h" + + +typedef struct { + uint16_t id; + union { + uint32_t u32; + int32_t i32; + uint16_t u16; + int16_t i16; + uint8_t u8; + uint8_t b[4]; + int8_t i8; + float f; + } data; + uint8_t crc; +} tSPortData; + + +class QUdpSocket; + +class SmartportTelemetry: public QObject { + Q_OBJECT + +public: + explicit SmartportTelemetry(QObject *parent = nullptr); + + + Q_PROPERTY(QString last_heartbeat MEMBER m_last_heartbeat WRITE set_last_heartbeat NOTIFY last_heartbeat_changed) + void set_last_heartbeat(QString last_heartbeat); + +signals: + void last_heartbeat_changed(QString last_heartbeat); + + +private slots: + void processSmartportDatagrams(); + +private: + void init(); + void smartport_read(uint8_t *buf, int buflen); + uint8_t u8CheckCrcSPORT(uint8_t *t); + + void processSmartportMessage(uint8_t *b); + + QUdpSocket *smartportSocket = nullptr; + + QString m_last_heartbeat = "N/A"; + qint64 last_heartbeat_timestamp; + +}; + +#endif //SMARTPORTTELEMETRY_H diff --git a/qml/main.qml b/qml/main.qml index 0e4a7832a..b6caaf5e1 100755 --- a/qml/main.qml +++ b/qml/main.qml @@ -164,6 +164,10 @@ ApplicationWindow { // id: mspTelemetry //} + SmartportTelemetry { + id: smartportTelemetry + } + LTMTelemetry { id: ltmTelemetry } diff --git a/src/main.cpp b/src/main.cpp index f6d099f33..50be4b1fc 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -27,6 +27,7 @@ const QVector permissions({"android.permission.INTERNET", #include "msptelemetry.h" #include "ltmtelemetry.h" #include "vectortelemetry.h" +#include "smartporttelemetry.h" #include "qopenhdlink.h" @@ -137,6 +138,7 @@ int main(int argc, char *argv[]) { qmlRegisterType("OpenHD", 1, 0, "MSPTelemetry"); qmlRegisterType("OpenHD", 1, 0, "LTMTelemetry"); qmlRegisterType("OpenHD", 1, 0, "VectorTelemetry"); + qmlRegisterType("OpenHD", 1, 0, "SmartportTelemetry"); qmlRegisterType("OpenHD", 1, 0, "OpenHDRC"); diff --git a/src/smartporttelemetry.cpp b/src/smartporttelemetry.cpp new file mode 100755 index 000000000..77d11e8e1 --- /dev/null +++ b/src/smartporttelemetry.cpp @@ -0,0 +1,307 @@ +#include "smartporttelemetry.h" +#include +#include +#include +#ifndef __windows__ +#include +#endif + +#include +#include +#include +#include +#include + +#include "util.h" +#include "constants.h" + +#include "openhd.h" + + +#define START_STOP 0x7e +#define DATA_FRAME 0x10 + +//Frsky DATA ID's +#define FR_ID_ALTITUDE 0x0100 //ALT_FIRST_ID +#define FR_ID_VARIO 0x0110 //VARIO_FIRST_ID +#define FR_ID_ALTITUDE 0x0100 //ALT_FIRST_ID +#define FR_ID_VARIO 0x0110 //VARIO_FIRST_ID +#define FR_ID_VFAS 0x0210 //VFAS_FIRST_ID +#define FR_ID_CURRENT 0x0200 //CURR_FIRST_ID +#define FR_ID_CELLS 0x0300 //CELLS_FIRST_ID +#define FR_ID_CELLS_LAST 0x030F //CELLS_LAST_ID +#define FR_ID_T1 0x0400 //T1_FIRST_ID +#define FR_ID_T2 0x0410 //T2_FIRST_ID +#define FR_ID_RPM 0x0500 //RPM_FIRST_ID +#define FR_ID_FUEL 0x0600 //FUEL_FIRST_ID +#define FR_ID_ACCX 0x0700 //ACCX_FIRST_ID +#define FR_ID_ACCY 0x0710 //ACCY_FIRST_ID +#define FR_ID_ACCZ 0x0720 //ACCZ_FIRST_ID +#define FR_ID_LATLONG 0x0800 //GPS_LONG_LATI_FIRST_ID +#define FR_ID_GPS_ALT 0x0820 //GPS_ALT_FIRST_ID +#define FR_ID_SPEED 0x0830 //GPS_SPEED_FIRST_ID +#define FR_ID_GPS_COURSE 0x0840 //GPS_COURS_FIRST_ID +#define FR_ID_GPS_TIME_DATE 0x0850 //GPS_TIME_DATE_FIRST_ID +#define FR_ID_GPS_SAT 0x0860 //GPS satellite count and fix state (own definition) +#define FR_ID_A3_FIRST 0x0900 //A3_FIRST_ID +#define FR_ID_A4_FIRST 0x0910 //A4_FIRST_ID +#define FR_ID_AIR_SPEED_FIRST 0x0A00 //AIR_SPEED_FIRST_ID +#define FR_ID_RSSI 0xF101 // used by the radio system +#define FR_ID_ADC1 0xF102 //ADC1_ID +#define FR_ID_ADC2 0xF103 //ADC2_ID +#define FR_ID_RXBATT 0xF104 // used by the radio system +#define FR_ID_SWR 0xF105 // used by the radio system +#define FR_ID_FIRMWARE 0xF106 // used by the radio system +#define FR_ID_VFAS 0x0210 //VFAS_FIRST_ID + + +SmartportTelemetry::SmartportTelemetry(QObject *parent): QObject(parent) { + qDebug() << "SmartportTelemetry::SmartportTelemetry()"; + init(); +} + + +void SmartportTelemetry::init() { + qDebug() << "SmartportTelemetry::init()"; + smartportSocket = new QUdpSocket(this); + smartportSocket->bind(QHostAddress::Any, 5010); + connect(smartportSocket, &QUdpSocket::readyRead, this, &SmartportTelemetry::processSmartportDatagrams); +} + + +void SmartportTelemetry::processSmartportDatagrams() { + QByteArray datagram; + + while (smartportSocket->hasPendingDatagrams()) { + datagram.resize(int(smartportSocket->pendingDatagramSize())); + smartportSocket->readDatagram(datagram.data(), datagram.size()); + smartport_read((uint8_t*)datagram.data(), datagram.length()); + } +} + + +void SmartportTelemetry::processSmartportMessage(uint8_t *b) { + tSPortData tel; + + tel.id = (uint16_t)b[1]; + tel.id <<= 8u; + tel.id += (uint16_t)b[0]; + + tel.data.b[0] = b[2]; + tel.data.b[1] = b[3]; + tel.data.b[2] = b[4]; + tel.data.b[3] = b[5]; + + tel.crc = b[6]; + + switch (tel.id) { + case FR_ID_VFAS: { + auto battery_voltage = (float)tel.data.u16 / 100.0; + OpenHD::instance()->set_battery_voltage(battery_voltage); + QSettings settings; + auto battery_cells = settings.value("battery_cells", QVariant(3)).toInt(); + int battery_percent = lipo_battery_voltage_to_percent(battery_cells, battery_voltage); + OpenHD::instance()->set_battery_percent(battery_percent); + QString battery_gauge_glyph = battery_gauge_glyph_from_percentage(battery_percent); + OpenHD::instance()->set_battery_gauge(battery_gauge_glyph); + break; + } + case FR_ID_LATLONG: { + if (tel.data.u32 & 0x80000000) { + float longitude = (float)(tel.data.u32 & 0x3fffffff); + longitude /= 600000; + if (tel.data.u32 & 0x40000000) { + longitude = -longitude; + } + OpenHD::instance()->set_lon(longitude); + } else { + float latitude = (float)(tel.data.u32 & 0x3fffffff); + latitude /= 600000; + if (tel.data.u32 & 0x40000000) { + latitude = -latitude; + } + OpenHD::instance()->set_lat(latitude); + } + break; + } + case FR_ID_GPS_ALT: { + auto msl_altitude = (float)(tel.data.i32) / 100.0; + OpenHD::instance()->set_alt_msl(msl_altitude); + break; + } + case FR_ID_SPEED: { + auto speed = (float)(tel.data.u32) / 2000.0; + OpenHD::instance()->set_speed(speed); + break; + } + case FR_ID_GPS_COURSE: { + auto heading = (float)( tel.data.u32 ) / 100.0; + OpenHD::instance()->set_hdg(heading); + break; + } + case FR_ID_T1: { + // iNac, CF flight modes / arm + //u16Modes = tel->d.data.u16; // see inav smartport.c //printf( "T1: %x", tel->d.data.u32 ); + break; + } + case FR_ID_T2: { + // iNav, CF sat fix / home + //auto fix = (uint8_t)( tel.data.u32 / 1000 ); + auto sats = (uint8_t)( tel.data.u32 % 1000 ); + OpenHD::instance()->set_satellites_visible(sats); + break; + } + case FR_ID_GPS_SAT: { + // car ctrl sat fix + //auto fix = (uint8_t)( tel.data.u16 % 10 ); + auto sats = (uint8_t)( tel.data.u16 / 10 ); //printf( "Sat: %x", tel->d.data.u32 ); + OpenHD::instance()->set_satellites_visible(sats); + break; + } + case FR_ID_RSSI: { + //auto rssi = (uint8_t)tel.data.u8; //printf( "RSSI: %x - %x", u8UavRssi, tel->d.data.u32 ); + break; + } + case FR_ID_RXBATT: { + //auto rx_batt = (float)(tel.data.u8); + //rx_batt *= 3.3 / 255.0 * 4.0; + break; + } + case FR_ID_SWR: { + //auto swr = (uint8_t)(tel.data.u32); + break; + } + case FR_ID_ADC1: { + //double adc1 = (float)tel.data.u8; + //adc1 *= 3.3 / 255.0; + break; + } + case FR_ID_ADC2: { + //double adc2 = (float)tel.data.u8; + //adc2 = 3.3 / 255.0; + break; + } + case FR_ID_ALTITUDE: { + auto rel_altitude = (float)(tel.data.i32) / 100.0; + OpenHD::instance()->set_alt_rel(rel_altitude); + break; + } + case FR_ID_VARIO: { + //auto vario = (float)( tel.data.i32 ) / 100; + break; + } + case FR_ID_ACCX: { + auto x = tel.data.i16; + OpenHD::instance()->set_vx(x); + break; + } + case FR_ID_ACCY: { + auto y = tel.data.i16; + OpenHD::instance()->set_vy(y); + break; + } + case FR_ID_ACCZ: { + auto z = tel.data.i16; + OpenHD::instance()->set_vz(z); + break; + } + case FR_ID_CURRENT: { + auto ampere = (float)tel.data.u16 / 10.0; // this is guessed + OpenHD::instance()->set_battery_current(ampere); + break; + } + case FR_ID_CELLS: { + break; + } + case FR_ID_CELLS_LAST: { + break; + } + case FR_ID_RPM: { + break; + } + case FR_ID_FUEL: { + break; + } + case FR_ID_GPS_TIME_DATE: { + break; + } + case FR_ID_A3_FIRST: { + break; + } + case FR_ID_A4_FIRST: { + break; + } + case FR_ID_AIR_SPEED_FIRST: { + break; + } + case FR_ID_FIRMWARE: { + break; + } + default: { + //printf( "\r\nsmartport unknown id: %x , %x", (uint16_t)tel.id, tel.data.u32 ); + break; + } + } +} + + +void SmartportTelemetry::set_last_heartbeat(QString last_heartbeat) { + m_last_heartbeat = last_heartbeat; + emit last_heartbeat_changed(m_last_heartbeat); +} + + +void SmartportTelemetry::smartport_read(uint8_t *buf, int buflen) { + static uint8_t s = 0; + static uint8_t e = 0; + static uint8_t tBuffer[7]; + uint8_t b; + int i; + + for (i = 0; i < buflen; i++) { + b = *buf++; + if ((e == 0) && (b == 0x7d)) { + e = 1; + continue; + } + if (e == 1) { + e = 0; + b = b + 0x20; + } + + if (s == 0) { + if (b == DATA_FRAME) { + s++; + } + } + else if (s <= 6) { + tBuffer[s-1] = b; + s++; + } else { + tBuffer[6] = b; + if (0 != u8CheckCrcSPORT(tBuffer)) { + processSmartportMessage(tBuffer); + } + s = 0; + } + } +} + + +uint8_t SmartportTelemetry::u8CheckCrcSPORT(uint8_t *b) { + uint16_t u16Crc = DATA_FRAME; + + int i; + + for (i = 0; i < 6; i++) { + u16Crc += b[i]; + u16Crc += u16Crc >> 8; + u16Crc &= 0x00ff; + } + + if ((uint8_t)(0xFF - u16Crc) == b[6]) { + return 1u; + } else { + return 0u; + } +}