Skip to content

Commit

Permalink
Add support for LTM telemetry
Browse files Browse the repository at this point in the history 
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
  • Loading branch information
@steveatinfincia
steveatinfincia committed Apr 2, 2020
1 parent 18f7fbf commit 8764206
Show file tree
Hide file tree
Showing 5 changed files with 279 additions and 61 deletions.
46 changes: 40 additions & 6 deletions inc/ltmtelemetry.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,32 @@

#include "constants.h"

#define LIGHTTELEMETRY_START1 0x24 //$ Header byte 1
#define LIGHTTELEMETRY_START2 0x54 //T Header byte 2
#define LIGHTTELEMETRY_GFRAME 0x47 //G GPS frame: GPS + Baro altitude data (Lat, Lon, Speed, Alt, Sats, Sat fix)
#define LIGHTTELEMETRY_AFRAME 0x41 //A Attitude frame: Attitude data (Roll, Pitch, Heading)
#define LIGHTTELEMETRY_SFRAME 0x53 //S Status frame: Sensors/Status data (VBat, Consumed current, Rssi, Airspeed, Arm status, Failsafe status, Flight mode)
#define LIGHTTELEMETRY_OFRAME 0x4f //O Origin frame: (Lat, Lon, Alt, OSD on, home fix)
#define LIGHTTELEMETRY_NFRAME 0x53 //N Navigation frame
#define LIGHTTELEMETRY_XFRAME 0x4f //X GPS eXtra frame: (GPS HDOP value, hw_status (failed sensor))

// complete length including headers and checksum
#define LIGHTTELEMETRY_GFRAMELENGTH 18
#define LIGHTTELEMETRY_AFRAMELENGTH 10
#define LIGHTTELEMETRY_SFRAMELENGTH 11
#define LIGHTTELEMETRY_OFRAMELENGTH 18
#define LIGHTTELEMETRY_NFRAMELENGTH 10
#define LIGHTTELEMETRY_XFRAMELENGTH 10


enum ltm_serial_state {
LTM_IDLE,
LTM_HEADER_START1,
LTM_HEADER_START2,
LTM_HEADER_MSGTYPE,
LTM_HEADER_DATA
};


class QUdpSocket;

Expand Down Expand Up @@ -33,20 +59,28 @@ private slots:

private:
void init();

int ltm_read(uint8_t *buf, int buflen);
uint8_t ltmread_u8();
uint16_t ltmread_u16();
uint32_t ltmread_u32();

void processLTMMessage();

#if defined(__rasp_pi__)
QFuture<void> fifoFuture;
QFutureWatcher<void> watcher;
#else
QUdpSocket *ltmSocket = nullptr;
#endif

QString m_last_heartbeat = "N/A";
qint64 last_heartbeat_timestamp;


enum ltm_serial_state c_state = LTM_IDLE;

uint8_t LTMserialBuffer[LIGHTTELEMETRY_GFRAMELENGTH-4];
uint8_t LTMreceiverIndex;
uint8_t LTMcmd;
uint8_t LTMrcvChecksum;
uint8_t LTMreadIndex;
uint8_t LTMframelength;

};

#endif //LTMTELEMETRY_H
2 changes: 2 additions & 0 deletions inc/util.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,8 @@ QString tracker_mode_from_enum(TRACKER_MODE mode);

QString vot_mode_from_telemetry(uint8_t mode);

QString ltm_mode_from_telem(int mode);

uint map(double input, double input_start, double input_end, uint16_t output_start, uint16_t output_end);

#if defined(__android__)
Expand Down
6 changes: 3 additions & 3 deletions qml/main.qml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -164,9 +164,9 @@ ApplicationWindow {
// id: mspTelemetry
//}

//LTMTelemetry {
// id: ltmTelemetry
//}
LTMTelemetry {
id: ltmTelemetry
}

VectorTelemetry {
id: vectorTelemetry
Expand Down
237 changes: 185 additions & 52 deletions src/ltmtelemetry.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,3 +1,20 @@
/* #################################################################################################################
* LightTelemetry protocol (LTM)
*
* Ghettostation one way telemetry protocol for really low bitrates (1200/2400 bauds).
*
* Protocol details: 3 different frames, little endian.
* G Frame (GPS position) (2hz @ 1200 bauds , 5hz >= 2400 bauds): 18BYTES
* 0x24 0x54 0x47 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xC0
* $ T G --------LAT-------- -------LON--------- SPD --------ALT-------- SAT/FIX CRC
* A Frame (Attitude) (5hz @ 1200bauds , 10hz >= 2400bauds): 10BYTES
* 0x24 0x54 0x41 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xC0
* $ T A --PITCH-- --ROLL--- -HEADING- CRC
* S Frame (Sensors) (2hz @ 1200bauds, 5hz >= 2400bauds): 11BYTES
* 0x24 0x54 0x53 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xC0
* $ T S VBAT(mv) Current(ma) RSSI AIRSPEED ARM/FS/FMOD CRC
* ################################################################################################################# */

#include "ltmtelemetry.h"
#include <sys/stat.h>
#include <sys/types.h>
Expand All @@ -17,6 +34,7 @@

#include "openhd.h"


LTMTelemetry::LTMTelemetry(QObject *parent): QObject(parent) {
qDebug() << "LTMTelemetry::LTMTelemetry()";
init();
Expand All @@ -25,76 +43,191 @@ LTMTelemetry::LTMTelemetry(QObject *parent): QObject(parent) {

void LTMTelemetry::init() {
qDebug() << "LTMTelemetry::init()";
#if defined(__rasp_pi__)
restartFifo();
#else
ltmSocket = new QUdpSocket(this);
ltmSocket->bind(QHostAddress::Any, 14550);
ltmSocket->bind(QHostAddress::Any, 5001);
connect(ltmSocket, &QUdpSocket::readyRead, this, &LTMTelemetry::processLTMDatagrams);
#endif
}


#if defined(__rasp_pi__)
void LTMTelemetry::restartFifo() {
qDebug() << "LTMTelemetry::restartFifo()";
fifoFuture = QtConcurrent::run(this, &LTMTelemetry::processLTMFifo);
watcher.cancel();
disconnect(&watcher, &QFutureWatcherBase::finished, this, &LTMTelemetry::restartFifo);
connect(&watcher, &QFutureWatcherBase::finished, this, &LTMTelemetry::restartFifo);
watcher.setFuture(fifoFuture);
}

void LTMTelemetry::processLTMFifo() {
uint8_t buf[4096];

int fifoFP = open(LTM_FIFO, O_RDONLY);
if (fifoFP < 0) {
QThread::msleep(1000);
return;
}

while (true) {
int received = read(fifoFP, buf, sizeof(buf));
if (received < 0) {
qDebug() << "LTM fifo returned -1";

close(fifoFP);
QThread::msleep(1000);
return;
}
for (int i = 0; i < received; i++) {
//if (res) {
// processLTMMessage(msg);
//}
}
}
QThread::msleep(1000);
close(fifoFP);
}
#else
void LTMTelemetry::processLTMDatagrams() {
QByteArray datagram;

while (ltmSocket->hasPendingDatagrams()) {
datagram.resize(int(ltmSocket->pendingDatagramSize()));
ltmSocket->readDatagram(datagram.data(), datagram.size());
typedef QByteArray::Iterator Iterator;

for (Iterator i = datagram.begin(); i != datagram.end(); i++) {
//if (res) {
// processLTMMessage(msg);
//}
}
ltm_read((uint8_t *)datagram.data(), datagram.length());
}
}
#endif

void LTMTelemetry::processLTMMessage() {

void LTMTelemetry::processLTMMessage() {
LTMreadIndex = 0;

if (LTMcmd == LIGHTTELEMETRY_GFRAME) {
auto latitude = (double)((int32_t)ltmread_u32())/10000000;
OpenHD::instance()->set_lat(latitude);

auto longitude = (double)((int32_t)ltmread_u32())/10000000;
OpenHD::instance()->set_lon(longitude);

uint8_t uav_groundspeedms = ltmread_u8();
auto speed = (float)(uav_groundspeedms * 3.6f); // convert to kmh
OpenHD::instance()->set_speed(speed);

auto rel_altitude = (float)((int32_t)ltmread_u32())/100.0f;
OpenHD::instance()->set_alt_rel(rel_altitude);

uint8_t ltm_satsfix = ltmread_u8();
auto sats = (ltm_satsfix >> 2) & 0xFF;
OpenHD::instance()->set_satellites_visible(sats);

//auto fix = ltm_satsfix & 0b00000011;
} else if (LTMcmd == LIGHTTELEMETRY_AFRAME) {
auto pitch = (int16_t)ltmread_u16();
OpenHD::instance()->set_pitch(pitch);
auto roll = (int16_t)ltmread_u16();
OpenHD::instance()->set_roll(roll);
auto heading = (float)((int16_t)ltmread_u16());
if (heading < 0 ) heading = heading + 360; //convert from -180/180 to 0/360°
OpenHD::instance()->set_hdg(heading);
} else if (LTMcmd == LIGHTTELEMETRY_OFRAME) {
//auto ltm_home_latitude = (double)((int32_t)ltmread_u32())/10000000;
//auto ltm_home_longitude = (double)((int32_t)ltmread_u32())/10000000;
//auto ltm_home_altitude = (float)((int32_t)ltmread_u32())/100.0f;
//auto ltm_osdon = ltmread_u8();
//auto ltm_homefix = ltmread_u8();
} else if (LTMcmd == LIGHTTELEMETRY_XFRAME) {
//HDOP uint16 HDOP * 100
//hw status uint8
//LTM_X_counter uint8
//Disarm Reason uint8
//(unused) 1byte
auto hdop = (float)((uint16_t)ltmread_u16())/10000.0f;
OpenHD::instance()->set_gps_hdop(hdop);
} else if (LTMcmd == LIGHTTELEMETRY_SFRAME) {
//Vbat uint16, mV
//Battery Consumption uint16, mAh
//RSSI uchar
//Airspeed uchar, m/s
//Status uchar

auto battery_voltage = (float)ltmread_u16()/1000.0f;
//td->mah = (float)ltmread_u16()/1000.0f;
OpenHD::instance()->set_battery_voltage(battery_voltage);

// no current provided?
//OpenHD::instance()->set_battery_current(ampere);

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);

//td->rssi = ltmread_u8();

uint8_t uav_airspeedms = ltmread_u8();
auto airspeed = (float)(uav_airspeedms * 3.6f); // convert to kmh
OpenHD::instance()->set_airspeed(airspeed);

uint8_t ltm_armfsmode = ltmread_u8();
auto armed = ltm_armfsmode & 0b00000001;
OpenHD::instance()->set_armed(armed);

//td->ltm_failsafe = (ltm_armfsmode >> 1) & 0b00000001;

auto _flightmode = (ltm_armfsmode >> 2) & 0b00111111;
auto flightmode = ltm_mode_from_telem(_flightmode);
OpenHD::instance()->set_flight_mode(flightmode);
}
}


void LTMTelemetry::set_last_heartbeat(QString last_heartbeat) {
m_last_heartbeat = last_heartbeat;
emit last_heartbeat_changed(m_last_heartbeat);
}


uint8_t LTMTelemetry::ltmread_u8() {
return LTMserialBuffer[LTMreadIndex++];
}


uint16_t LTMTelemetry::ltmread_u16() {
uint16_t t = ltmread_u8();
t |= (uint16_t)ltmread_u8()<<8;
return t;
}


uint32_t LTMTelemetry::ltmread_u32() {
uint32_t t = ltmread_u16();
t |= (uint32_t)ltmread_u16()<<16;
return t;
}


int LTMTelemetry::ltm_read(uint8_t *buf, int buflen) {
int i;

for (i = 0; i < buflen; ++i) {
uint8_t c = buf[i];

if (c_state == LTM_IDLE) {
c_state = (c=='$') ? LTM_HEADER_START1 : LTM_IDLE;
} else if (c_state == LTM_HEADER_START1) {
c_state = (c=='T') ? LTM_HEADER_START2 : LTM_IDLE;
} else if (c_state == LTM_HEADER_START2) {
switch (c) {
case 'G':
LTMframelength = LIGHTTELEMETRY_GFRAMELENGTH;
c_state = LTM_HEADER_MSGTYPE;
break;
case 'A':
LTMframelength = LIGHTTELEMETRY_AFRAMELENGTH;
c_state = LTM_HEADER_MSGTYPE;
break;
case 'S':
LTMframelength = LIGHTTELEMETRY_SFRAMELENGTH;
c_state = LTM_HEADER_MSGTYPE;
break;
case 'O':
LTMframelength = LIGHTTELEMETRY_OFRAMELENGTH;
c_state = LTM_HEADER_MSGTYPE;
break;
case 'N':
LTMframelength = LIGHTTELEMETRY_NFRAMELENGTH;
c_state = LTM_HEADER_MSGTYPE;
break;
case 'X':
LTMframelength = LIGHTTELEMETRY_XFRAMELENGTH;
c_state = LTM_HEADER_MSGTYPE;
break;
default:
c_state = LTM_IDLE;
}
LTMcmd = c;
LTMreceiverIndex=0;
} else if (c_state == LTM_HEADER_MSGTYPE) {

if (LTMreceiverIndex == 0) {
LTMrcvChecksum = c;
} else {
LTMrcvChecksum ^= c;
}

if (LTMreceiverIndex == LTMframelength-4) { // received checksum byte
if (LTMrcvChecksum == 0) {
processLTMMessage();
c_state = LTM_IDLE;
}
else { // wrong checksum, drop packet
c_state = LTM_IDLE;
}
}
else LTMserialBuffer[LTMreceiverIndex++]=c;
}
}
}
Loading

0 comments on commit 8764206

Please sign in to comment.