Decode LTM telemetry protocol

gulpfile.js

@@ -139,7 +139,8 @@ sources.js = [
     './js/libraries/plotly-latest.min.js',
     './js/sitl.js',
     './js/CliAutoComplete.js',
-    './node_modules/jquery-textcomplete/dist/jquery.textcomplete.js'
+    './node_modules/jquery-textcomplete/dist/jquery.textcomplete.js',
+    './js/ltmDecoder.js',
 ];
 
 sources.receiverCss = [

js/ltmDecoder.js

@@ -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;
+})();

js/serial_backend.js

@@ -322,6 +322,7 @@ function onOpen(openInfo) {
         chrome.storage.local.set({wireless_mode_enabled: $('#wireless-mode').is(":checked")});
 
         CONFIGURATOR.connection.addOnReceiveListener(read_serial);
+        CONFIGURATOR.connection.addOnReceiveListener(helper.ltmDecoder.read);
 
         // disconnect after 10 seconds with error if we don't get IDENT data
         helper.timeout.add('connecting', function () {