1
0
Fork 0
mirror of https://github.com/iNavFlight/inav-configurator.git synced 2025-07-24 00:35:20 +03:00

Merge pull request #1988 from iNavFlight/dzikuvx-ltm-decoder

Decode LTM telemetry protocol
This commit is contained in:
Paweł Spychalski 2024-03-23 18:47:40 +01:00 committed by GitHub
commit f93fba9c36
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
8 changed files with 596 additions and 2 deletions

View file

@ -5603,5 +5603,38 @@
},
"ezTuneNote": {
"message": "<strong>Important</strong> Ez Tune is enabled. All settings on this tab are set and controlled by the Ez Tune. To use PID Tuning tab you have to disable Ez Tune. To do it, uncheck the <strong>Enabled</strong> checkbox on the Ez Tune tab."
},
"gsActivated": {
"message": "Ground station mode activated"
},
"gsDeactivated": {
"message": "Ground station mode deactivated"
},
"gsTelemetry": {
"message": "Telemetry"
},
"gsTelemetryLatitude": {
"message": "Latitude"
},
"gsTelemetryLongitude": {
"message": "Longitude"
},
"gsTelemetryAltitude": {
"message": "Altitude"
},
"gsTelemetryAltitudeShort": {
"message": "Alt"
},
"gsTelemetryVoltageShort": {
"message": "Vbat"
},
"gsTelemetrySats": {
"message": "Sats"
},
"gsTelemetryFix": {
"message": "Fix"
},
"gsTelemetrySpeed": {
"message": "Speed"
}
}

View file

@ -55,6 +55,7 @@ sources.css = [
'./node_modules/openlayers/dist/ol.css',
'./src/css/logic.css',
'./src/css/defaults_dialog.css',
'./src/css/groundstation.css',
];
sources.js = [
@ -139,7 +140,9 @@ 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',
'./js/groundstation.js'
];
sources.receiverCss = [

194
js/groundstation.js Normal file
View file

@ -0,0 +1,194 @@
'use strict';
var helper = helper || {};
helper.groundstation = (function () {
let publicScope = {},
privateScope = {};
privateScope.activated = false;
privateScope.$viewport = null;
privateScope.$gsViewport = null;
privateScope.mapHandler = null;
privateScope.mapLayer = null;
privateScope.mapView = null;
privateScope.cursorStyle = null;
privateScope.cursorPosition = null;
privateScope.cursorFeature = null;
privateScope.cursorVector = null;
privateScope.cursorLayer = null;
privateScope.textGeometry = null;
privateScope.textFeature = null;
privateScope.textVector = null;
privateScope.textSource = null;
privateScope.mapInitiated = false;
publicScope.isActivated = function () {
return privateScope.activated;
};
publicScope.activate = function ($viewport) {
if (privateScope.activated) {
return;
}
helper.interval.add('gsUpdateGui', privateScope.updateGui, 200);
privateScope.$viewport = $viewport;
privateScope.$viewport.find(".tab_container").hide();
privateScope.$viewport.find('#content').hide();
privateScope.$viewport.find('#status-bar').hide();
privateScope.$viewport.find('#connectbutton a.connect_state').text(chrome.i18n.getMessage('disconnect')).addClass('active');
privateScope.$gsViewport = $viewport.find('#view-groundstation');
privateScope.$gsViewport.show();
privateScope.mapInitiated = false;
setTimeout(privateScope.initMap, 100);
privateScope.activated = true;
GUI.log(chrome.i18n.getMessage('gsActivated'));
}
privateScope.initMap = function () {
//initialte layers
if (globalSettings.mapProviderType == 'bing') {
privateScope.mapLayer = new ol.source.BingMaps({
key: globalSettings.mapApiKey,
imagerySet: 'AerialWithLabels',
maxZoom: 19
});
} else if (globalSettings.mapProviderType == 'mapproxy') {
privateScope.mapLayer = new ol.source.TileWMS({
url: globalSettings.proxyURL,
params: { 'LAYERS': globalSettings.proxyLayer }
})
} else {
privateScope.mapLayer = new ol.source.OSM();
}
//initiate view
privateScope.mapView = new ol.View({
center: ol.proj.fromLonLat([0, 0]),
zoom: 3
});
//initiate map handler
privateScope.mapHandler = new ol.Map({
target: document.getElementById('groundstation-map'),
layers: [
new ol.layer.Tile({
source: privateScope.mapLayer
})
],
view: privateScope.mapView
});
};
publicScope.deactivate = function () {
if (!privateScope.activated) {
return;
}
helper.interval.remove('gsUpdateGui');
if (privateScope.$viewport !== null) {
privateScope.$viewport.find(".tab_container").show();
privateScope.$viewport.find('#content').show();
privateScope.$viewport.find('#status-bar').show();
}
if (privateScope.$gsViewport !== null) {
privateScope.$gsViewport.hide();
}
privateScope.activated = false;
GUI.log(chrome.i18n.getMessage('gsDeactivated'));
}
privateScope.updateGui = function () {
let telemetry = helper.ltmDecoder.get();
if (telemetry.gpsFix && telemetry.gpsFix > 1) {
let lat = telemetry.latitude / 10000000;
let lon = telemetry.longitude / 10000000;
//On first initiation, set zoom to 15
if (!privateScope.mapInitiated) {
//Place UAV on the map
privateScope.cursorStyle = new ol.style.Style({
image: new ol.style.Icon(({
anchor: [0.5, 0.5],
opacity: 1,
scale: 0.6,
src: '../images/icons/icon_mission_airplane.png'
}))
});
privateScope.cursorPosition = new ol.geom.Point(ol.proj.fromLonLat([lon, lat]));
privateScope.cursorFeature = new ol.Feature({
geometry: privateScope.cursorPosition
});
privateScope.cursorFeature.setStyle(privateScope.cursorStyle);
privateScope.cursorVector = new ol.source.Vector({
features: [privateScope.cursorFeature]
});
privateScope.cursorLayer = new ol.layer.Vector({
source: privateScope.cursorVector
});
privateScope.mapHandler.addLayer(privateScope.cursorLayer);
privateScope.mapView.setZoom(17);
privateScope.mapInitiated = true;
}
//Update map center
let position = ol.proj.fromLonLat([lon, lat]);
privateScope.mapView.setCenter(position);
//Update position of cursor
privateScope.cursorPosition.setCoordinates(position);
//Update orientation of cursor
privateScope.cursorStyle.getImage().setRotation((telemetry.heading / 360.0) * 6.28318);
//Update text
privateScope.$viewport.find("#gs-telemetry-latitude").html(lat);
privateScope.$viewport.find("#gs-telemetry-longitude").html(lon);
}
privateScope.$viewport.find("#gs-telemetry-altitude").html(telemetry.altitude / 100.0 + 'm');
privateScope.$viewport.find("#gs-telemetry-voltage").html(telemetry.voltage / 100.0 + 'V');
privateScope.$viewport.find("#gs-telemetry-sats").html(telemetry.gpsSats);
privateScope.$viewport.find("#gs-telemetry-speed").html(telemetry.groundSpeed * 100 + 'm/s');
let fixText = '';
if (telemetry.gpsFix == 3) {
fixText = '3D';
} else if (telemetry.gpsFix == 2) {
fixText = '2D';
} else {
fixText = 'No fix';
}
privateScope.$viewport.find("#gs-telemetry-fix").html(fixText);
};
return publicScope;
})();

260
js/ltmDecoder.js Normal file
View file

@ -0,0 +1,260 @@
'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('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
*/
let checksum = 0;
for (let i = 0; i < privateScope.serialBuffer.length; i++) {
checksum ^= privateScope.serialBuffer[i];
}
if (checksum != data) {
console.log('LTM checksum error, frame type: ' + privateScope.frameType + ' rejected');
privateScope.protocolState = LTM_STATE_IDLE;
privateScope.serialBuffer = [];
privateScope.receiverIndex = 0;
return;
}
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;
})();

View file

@ -78,6 +78,8 @@ var MSP = {
last_received_timestamp: null,
analog_last_received_timestamp: null,
lastFrameReceivedMs: 0,
read: function (readInfo) {
var data = new Uint8Array(readInfo.data);
@ -236,6 +238,7 @@ var MSP = {
if (this.message_checksum == expected_checksum) {
// message received, process
mspHelper.processData(this);
this.lastFrameReceivedMs = Date.now();
} else {
console.log('code: ' + this.code + ' - crc failed');
this.packet_error++;
@ -378,6 +381,12 @@ var MSP = {
this.packet_error = 0; // reset CRC packet error counter for next session
this.callbacks_cleanup();
},
isReceiving: function () {
return Date.now() - this.lastFrameReceivedMs < 5000;
},
wasEverReceiving: function () {
return this.lastFrameReceivedMs > 0;
}
};

View file

@ -129,6 +129,11 @@ $(document).ready(function () {
});
$('div.connect_controls a.connect').click(function () {
if (helper.groundstation.isActivated()) {
helper.groundstation.deactivate();
}
if (GUI.connect_lock != true) { // GUI control overrides the user control
var clicks = $(this).data('clicks');
@ -322,10 +327,13 @@ 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 () {
if (!CONFIGURATOR.connectionValid) {
//As we add LTM listener, we need to invalidate connection only when both protocols are not listening!
if (!CONFIGURATOR.connectionValid && !helper.ltmDecoder.isReceiving()) {
GUI.log(chrome.i18n.getMessage('noConfigurationReceived'));
helper.mspQueue.flush();
@ -337,6 +345,13 @@ function onOpen(openInfo) {
}
}, 10000);
//Add a timer that every 1s will check if LTM stream is receiving data and display alert if so
helper.interval.add('ltm-connection-check', function () {
if (helper.ltmDecoder.isReceiving()) {
helper.groundstation.activate($('#main-wrapper'));
}
}, 1000);
FC.resetState();
// request configuration data. Start with MSPv1 and

View file

@ -174,6 +174,49 @@
<div id="scrollicon"></div>
<div class="wrapper"></div>
</div>
<div id="view-groundstation" style="display: none;">
<div id="groundstation-telemetry">
<h2 class="groundstation-telemetry__header" data-i18n="gsTelemetry"></h2>
<div class="groundstation-telemetry__row groundstation-telemetry__row--big">
<label for="gs-telemetry-voltage" class="groundstation-telemetry__label" data-i18n="gsTelemetryVoltageShort"></label>
<div class="groundstation-telemetry__value" id="gs-telemetry-voltage">-</div>
</div>
<div class="groundstation-telemetry__row groundstation-telemetry__row--big">
<label for="gs-telemetry-altitude" class="groundstation-telemetry__label" data-i18n="gsTelemetryAltitudeShort"></label>
<div class="groundstation-telemetry__value" id="gs-telemetry-altitude">-</div>
</div>
<div class="groundstation-telemetry__row groundstation-telemetry__row--big">
<label for="gs-telemetry-speed" class="groundstation-telemetry__label" data-i18n="gsTelemetrySpeed"></label>
<div class="groundstation-telemetry__value" id="gs-telemetry-speed">-</div>
</div>
<div class="groundstation-telemetry__row">
<label for="gs-telemetry-latitude" class="groundstation-telemetry__label" data-i18n="gsTelemetryLatitude"></label>
<div class="groundstation-telemetry__value" id="gs-telemetry-latitude">-</div>
</div>
<div class="groundstation-telemetry__row">
<label class="groundstation-telemetry__label" for="gs-telemetry-longitude" data-i18n="gsTelemetryLongitude"></label>
<div class="groundstation-telemetry__value" id="gs-telemetry-longitude">-</div>
</div>
<div class="groundstation-telemetry__row">
<label class="groundstation-telemetry__label" for="gs-telemetry-longitude" data-i18n="gsTelemetrySats"></label>
<div class="groundstation-telemetry__value" id="gs-telemetry-sats">-</div>
</div>
<div class="groundstation-telemetry__row">
<label class="groundstation-telemetry__label" for="gs-telemetry-longitude" data-i18n="gsTelemetryFix"></label>
<div class="groundstation-telemetry__value" id="gs-telemetry-fix">-</div>
</div>
</div>
<div id="groundstation-map"></div>
</div>
<div class="tab_container">
<div id="tabs">
<ul class="mode-disconnected">

37
src/css/groundstation.css Normal file
View file

@ -0,0 +1,37 @@
#view-groundstation {
background-color: #2e2e2e;
width: 100%;
height: 100%;
display: flex;
}
#groundstation-map {
width: 100%;
height: 100%;
}
#groundstation-telemetry {
width: 20%;
color: #ddd;
}
.groundstation-telemetry__header {
margin: 1em;
text-align: center;
}
.groundstation-telemetry__row {
display: flex;
justify-content: space-between;
margin: 1em;
font-size: 1.1em;
}
.groundstation-telemetry__row--big {
font-size: 2.2em;
}
.groundstation-telemetry__label {
color: #aaa;
font-weight: bold;
}