Decode LTM telemetry protocol
parent
a13227bb3b
commit
742176d7eb
@ -0,0 +1,246 @@
|
||||
'use strict';
|
||||
|
||||
var helper = helper || {};
|
||||
|
||||
helper.ltmDecoder = (function () {
|
||||
|
||||
let TELEMETRY = {
|
||||
//A frame
|
||||
pitch: null,
|
||||
roll: null,
|
||||
heading: null,
|
||||
|
||||
//S frame
|
||||
voltage: null,
|
||||
currectDrawn: null,
|
||||
rssi: null,
|
||||
airspeed: null,
|
||||
flightmode: null,
|
||||
flightmodeName: null,
|
||||
|
||||
armed: null,
|
||||
failsafe: null,
|
||||
|
||||
//G frame
|
||||
latitude: null,
|
||||
longitude: null,
|
||||
altitude: null,
|
||||
groundSpeed: null,
|
||||
gpsFix: null,
|
||||
gpsSats: null,
|
||||
|
||||
|
||||
//X frame
|
||||
hdop: null,
|
||||
sensorStatus: null,
|
||||
frameCounter: null,
|
||||
disarmReason: null,
|
||||
disarmReasonName: null
|
||||
|
||||
};
|
||||
|
||||
let publicScope = {},
|
||||
privateScope = {};
|
||||
|
||||
const LTM_TIMEOUT_MS = 5000;
|
||||
const LTM_FRAME_TIMEOUT_MS = 700;
|
||||
const LTM_HEADER_START_1 = '$';
|
||||
const LTM_HEADER_START_2 = 'T';
|
||||
const LTM_FRAMELENGTH = {
|
||||
'G': 18,
|
||||
'A': 10,
|
||||
'S': 11,
|
||||
'O': 18,
|
||||
'N': 10,
|
||||
'X': 10
|
||||
};
|
||||
|
||||
const LTM_FLIGHT_MODE_NAMES = [
|
||||
"MANUAL",
|
||||
"RATE",
|
||||
"ANGLE",
|
||||
"HORIZON",
|
||||
"ACRO",
|
||||
"STABALIZED1",
|
||||
"STABALIZED2",
|
||||
"STABILIZED3",
|
||||
"ALTHOLD",
|
||||
"GPSHOLD",
|
||||
"WAYPOINTS",
|
||||
"HEADHOLD",
|
||||
"CIRCLE",
|
||||
"RTH",
|
||||
"FOLLOWME",
|
||||
"LAND",
|
||||
"FLYBYWIRE1",
|
||||
"FLYBYWIRE2",
|
||||
"CRUISE",
|
||||
"UNKNOWN",
|
||||
"LAUNCH",
|
||||
"AUTOTUNE"
|
||||
];
|
||||
|
||||
const LTM_DISARM_REASON_NAMES = [
|
||||
"NONE",
|
||||
"TIMEOUT",
|
||||
"STICKS",
|
||||
"SWITCH_3D",
|
||||
"SWITCH",
|
||||
"KILLSWITCH",
|
||||
"FAILSAFE",
|
||||
"NAVIGATION",
|
||||
"LANDING"
|
||||
];
|
||||
|
||||
const LTM_STATE_IDLE = 0;
|
||||
const LTM_STATE_HEADER_START_1 = 1;
|
||||
const LTM_STATE_HEADER_START_2 = 2;
|
||||
const LTM_STATE_MSGTYPE = 3;
|
||||
|
||||
privateScope.protocolState = LTM_STATE_IDLE;
|
||||
privateScope.lastFrameReceivedMs = null;
|
||||
privateScope.frameType = null;
|
||||
privateScope.frameLength = null;
|
||||
privateScope.receiverIndex = 0;
|
||||
privateScope.serialBuffer = [];
|
||||
privateScope.frameProcessingStartedAtMs = 0;
|
||||
|
||||
privateScope.readByte = function (offset) {
|
||||
return privateScope.serialBuffer[offset];
|
||||
};
|
||||
|
||||
privateScope.readInt = function (offset) {
|
||||
return privateScope.serialBuffer[offset] + (privateScope.serialBuffer[offset + 1] << 8);
|
||||
}
|
||||
|
||||
privateScope.readInt32 = function (offset) {
|
||||
return privateScope.serialBuffer[offset] + (privateScope.serialBuffer[offset + 1] << 8) + (privateScope.serialBuffer[offset + 2] << 16) + (privateScope.serialBuffer[offset + 3] << 24);
|
||||
}
|
||||
|
||||
privateScope.push = function (data) {
|
||||
let charCode = String.fromCharCode(data);
|
||||
|
||||
//If frame is processed for too long, reset protocol state
|
||||
if (privateScope.protocolState != LTM_STATE_IDLE && new Date().getTime() - privateScope.frameProcessingStartedAtMs > LTM_FRAME_TIMEOUT_MS) {
|
||||
privateScope.protocolState = LTM_STATE_IDLE;
|
||||
privateScope.frameProcessingStartedAtMs = new Date().getTime();
|
||||
console.log('LTM privateScope.protocolState: TIMEOUT, forcing into IDLE, processed frame: ' + privateScope.frameType);
|
||||
}
|
||||
|
||||
if (privateScope.protocolState == LTM_STATE_IDLE) {
|
||||
if (charCode == LTM_HEADER_START_1) {
|
||||
privateScope.protocolState = LTM_STATE_HEADER_START_1;
|
||||
privateScope.frameProcessingStartedAtMs = new Date().getTime();
|
||||
}
|
||||
return;
|
||||
} else if (privateScope.protocolState == LTM_STATE_HEADER_START_1) {
|
||||
if (charCode == LTM_HEADER_START_2) {
|
||||
privateScope.protocolState = LTM_STATE_HEADER_START_2;
|
||||
} else {
|
||||
privateScope.protocolState = LTM_STATE_IDLE;
|
||||
}
|
||||
return;
|
||||
} else if (privateScope.protocolState == LTM_STATE_HEADER_START_2) {
|
||||
|
||||
//Check if incoming frame type is a known one
|
||||
if (LTM_FRAMELENGTH[charCode] == undefined) {
|
||||
//Unknown frame type, reset protocol state
|
||||
privateScope.protocolState = LTM_STATE_IDLE;
|
||||
console.log('Unknown frame type, reset protocol state');
|
||||
} else {
|
||||
//Known frame type, store it and move to next state
|
||||
privateScope.frameType = charCode;
|
||||
privateScope.frameLength = LTM_FRAMELENGTH[charCode];
|
||||
privateScope.receiverIndex = 0;
|
||||
privateScope.serialBuffer = [];
|
||||
privateScope.protocolState = LTM_STATE_MSGTYPE;
|
||||
console.log('privateScope.protocolState: LTM_STATE_MSGTYPE', 'will expext frame ' + privateScope.frameType, 'expected length: ' + privateScope.frameLength);
|
||||
}
|
||||
return;
|
||||
|
||||
} else if (privateScope.protocolState == LTM_STATE_MSGTYPE) {
|
||||
|
||||
/*
|
||||
* Check if last payload byte has been received.
|
||||
*/
|
||||
if (privateScope.receiverIndex == privateScope.frameLength - 4) {
|
||||
/*
|
||||
* If YES, check checksum and execute data processing
|
||||
*/
|
||||
if (privateScope.frameType == 'A') {
|
||||
TELEMETRY.pitch = privateScope.readInt(0);
|
||||
TELEMETRY.roll = privateScope.readInt(2);
|
||||
TELEMETRY.heading = privateScope.readInt(4);
|
||||
}
|
||||
|
||||
if (privateScope.frameType == 'S') {
|
||||
TELEMETRY.voltage = privateScope.readInt(0);
|
||||
TELEMETRY.currectDrawn = privateScope.readInt(2);
|
||||
TELEMETRY.rssi = privateScope.readByte(4);
|
||||
|
||||
TELEMETRY.airspeed = privateScope.readByte(5);
|
||||
|
||||
let fm = privateScope.readByte(6);
|
||||
TELEMETRY.flightmode = fm >> 2;
|
||||
TELEMETRY.flightmodeName = LTM_FLIGHT_MODE_NAMES[TELEMETRY.flightmode];
|
||||
|
||||
TELEMETRY.armed = (fm & 0x02) >> 1;
|
||||
TELEMETRY.failsafe = fm & 0x01;
|
||||
}
|
||||
|
||||
if (privateScope.frameType == 'G') {
|
||||
TELEMETRY.latitude = privateScope.readInt32(0);
|
||||
TELEMETRY.longitude = privateScope.readInt32(4);
|
||||
TELEMETRY.groundSpeed = privateScope.readByte(8);
|
||||
TELEMETRY.altitude = privateScope.readInt32(9);
|
||||
|
||||
let raw = privateScope.readByte(13);
|
||||
TELEMETRY.gpsSats = raw >> 2;
|
||||
TELEMETRY.gpsFix = raw & 0x03;
|
||||
}
|
||||
|
||||
if (privateScope.frameType == 'X') {
|
||||
TELEMETRY.hdop = privateScope.readInt(0);
|
||||
TELEMETRY.sensorStatus = privateScope.readByte(2);
|
||||
TELEMETRY.frameCounter = privateScope.readByte(3);
|
||||
TELEMETRY.disarmReason = privateScope.readByte(4);
|
||||
TELEMETRY.disarmReasonName = LTM_DISARM_REASON_NAMES[TELEMETRY.disarmReason];
|
||||
}
|
||||
|
||||
privateScope.protocolState = LTM_STATE_IDLE;
|
||||
privateScope.serialBuffer = [];
|
||||
privateScope.lastFrameReceivedMs = new Date().getTime();
|
||||
privateScope.receiverIndex = 0;
|
||||
|
||||
} else {
|
||||
/*
|
||||
* If no, put data into buffer
|
||||
*/
|
||||
privateScope.serialBuffer.push(data);
|
||||
privateScope.receiverIndex++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
publicScope.read = function (readInfo) {
|
||||
var data = new Uint8Array(readInfo.data);
|
||||
|
||||
for (var i = 0; i < data.length; i++) {
|
||||
privateScope.push(data[i]);
|
||||
}
|
||||
};
|
||||
|
||||
publicScope.isReceiving = function () {
|
||||
return privateScope.lastFrameReceivedMs !== null && (new Date().getTime() - privateScope.lastFrameReceivedMs) < LTM_TIMEOUT_MS;
|
||||
};
|
||||
|
||||
publicScope.wasEverReceiving = function () {
|
||||
return privateScope.lastFrameReceivedMs !== null;
|
||||
};
|
||||
|
||||
publicScope.get = function () {
|
||||
return TELEMETRY;
|
||||
};
|
||||
|
||||
return publicScope;
|
||||
})();
|
Loading…
Reference in New Issue