diff --git a/TelemetryDashboard/MAVLink/mavlink.js b/TelemetryDashboard/MAVLink/mavlink.js index 5e3e2824..2a29f743 100644 --- a/TelemetryDashboard/MAVLink/mavlink.js +++ b/TelemetryDashboard/MAVLink/mavlink.js @@ -6,14 +6,72 @@ Generated from: all.xml,ardupilotmega.xml,ASLUAV.xml,common.xml,development.xml, Note: this file has been auto-generated. DO NOT EDIT */ -var jspack -import("./local_modules/jspack/jspack.js").then((mod) => { - jspack = new mod.default() -}).catch((e) => { - // Discard annoying error. - // This fails in the sandbox because of CORS - // Only really want the message definitions in that case, not the parser, so its OK. -}) +// Detect environment +const isNode = typeof process !== 'undefined' && + process.versions && + process.versions.node; + +// Handle jspack dependency +let jspack; +if (isNode) { + jspack = (global && global.jspack) || require("jspack").jspack; +} else { + import("./local_modules/jspack/jspack.js").then((mod) => { + jspack = new mod.default() + }).catch((e) => { + }); +} + +// Handle Node.js specific modules +let events, util; +if (isNode) { + events = require("events"); + util = require("util"); +} else { + // Browser polyfills for Node.js modules + util = { + inherits: function(constructor, superConstructor) { + constructor.prototype = Object.create(superConstructor.prototype); + constructor.prototype.constructor = constructor; + } + }; + + // Simple EventEmitter polyfill for browsers + events = { + EventEmitter: function() { + this._events = {}; + + this.on = function(event, listener) { + if (!this._events[event]) { + this._events[event] = []; + } + this._events[event].push(listener); + }; + + this.emit = function(event, ...args) { + if (this._events[event]) { + this._events[event].forEach(listener => { + try { + listener.apply(this, args); + } catch (e) { + console.error('Error in event listener:', e); + } + }); + } + }; + + this.removeListener = function(event, listener) { + if (this._events[event]) { + const index = this._events[event].indexOf(listener); + if (index > -1) { + this._events[event].splice(index, 1); + } + } + }; + } + }; +} + mavlink20 = function(){}; @@ -55,7 +113,7 @@ mavlink20.MAVLINK_IFLAG_SIGNED = 0x01 mavlink20.MAVLINK_SIGNATURE_BLOCK_LEN = 13 // Mavlink headers incorporate sequence, source system (platform) and source component. -mavlink20.header = function(msgId, mlen, seq, srcSystem, srcComponent, incompat_flags=0, compat_flags=0,) { +mavlink20.header = function(msgId, mlen, seq, srcSystem, srcComponent, incompat_flags=0, compat_flags=0) { this.mlen = ( typeof mlen === 'undefined' ) ? 0 : mlen; this.seq = ( typeof seq === 'undefined' ) ? 0 : seq; @@ -90,36 +148,140 @@ mavlink20.message.prototype.set = function(args,verbose) { }, this); }; -// trying to be the same-ish as the python function of the same name -mavlink20.message.prototype.sign_packet = function( mav) { - var crypto= require('crypto'); - var h = crypto.createHash('sha256'); +/* + sha256 implementation + embedded to avoid async issues in web browsers with crypto library + with thanks to https://geraintluff.github.io/sha256/ +*/ +mavlink20.sha256 = function(inputBytes) { + const K = new Uint32Array([ + 0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5, 0x3956c25b, + 0x59f111f1, 0x923f82a4, 0xab1c5ed5, 0xd807aa98, 0x12835b01, + 0x243185be, 0x550c7dc3, 0x72be5d74, 0x80deb1fe, 0x9bdc06a7, + 0xc19bf174, 0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc, + 0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da, 0x983e5152, + 0xa831c66d, 0xb00327c8, 0xbf597fc7, 0xc6e00bf3, 0xd5a79147, + 0x06ca6351, 0x14292967, 0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, + 0x53380d13, 0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85, + 0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3, 0xd192e819, + 0xd6990624, 0xf40e3585, 0x106aa070, 0x19a4c116, 0x1e376c08, + 0x2748774c, 0x34b0bcb5, 0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, + 0x682e6ff3, 0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208, + 0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2 + ]); + + function ROTR(n, x) { return (x >>> n) | (x << (32 - n)); } + + function Σ0(x) { return ROTR(2, x) ^ ROTR(13, x) ^ ROTR(22, x); } + function Σ1(x) { return ROTR(6, x) ^ ROTR(11, x) ^ ROTR(25, x); } + function σ0(x) { return ROTR(7, x) ^ ROTR(18, x) ^ (x >>> 3); } + function σ1(x) { return ROTR(17, x) ^ ROTR(19, x) ^ (x >>> 10); } + + function Ch(x, y, z) { return (x & y) ^ (~x & z); } + function Maj(x, y, z) { return (x & y) ^ (x & z) ^ (y & z); } + + const H = new Uint32Array([ + 0x6a09e667, 0xbb67ae85, 0x3c6ef372, 0xa54ff53a, + 0x510e527f, 0x9b05688c, 0x1f83d9ab, 0x5be0cd19 + ]); + + const l = inputBytes.length; + const bitLen = l * 8; + + const withPadding = new Uint8Array(((l + 9 + 63) >> 6) << 6); // pad to multiple of 64 bytes + withPadding.set(inputBytes); + withPadding[l] = 0x80; + withPadding.set([ + 0, 0, 0, 0, + (bitLen >>> 24) & 0xff, + (bitLen >>> 16) & 0xff, + (bitLen >>> 8) & 0xff, + bitLen & 0xff + ], withPadding.length - 8); + + const w = new Uint32Array(64); + for (let i = 0; i < withPadding.length; i += 64) { + for (let j = 0; j < 16; j++) { + w[j] = ( + (withPadding[i + 4 * j] << 24) | + (withPadding[i + 4 * j + 1] << 16) | + (withPadding[i + 4 * j + 2] << 8) | + (withPadding[i + 4 * j + 3]) + ) >>> 0; + } + for (let j = 16; j < 64; j++) { + w[j] = (σ1(w[j - 2]) + w[j - 7] + σ0(w[j - 15]) + w[j - 16]) >>> 0; + } - //mav.signing.timestamp is a 48bit number, or 6 bytes. + let [a, b, c, d, e, f, g, h] = H; + + for (let j = 0; j < 64; j++) { + const T1 = (h + Σ1(e) + Ch(e, f, g) + K[j] + w[j]) >>> 0; + const T2 = (Σ0(a) + Maj(a, b, c)) >>> 0; + h = g; + g = f; + f = e; + e = (d + T1) >>> 0; + d = c; + c = b; + b = a; + a = (T1 + T2) >>> 0; + } + + H[0] = (H[0] + a) >>> 0; + H[1] = (H[1] + b) >>> 0; + H[2] = (H[2] + c) >>> 0; + H[3] = (H[3] + d) >>> 0; + H[4] = (H[4] + e) >>> 0; + H[5] = (H[5] + f) >>> 0; + H[6] = (H[6] + g) >>> 0; + H[7] = (H[7] + h) >>> 0; + } + + const output = new Uint8Array(32); + for (let i = 0; i < 8; i++) { + output[i * 4 + 0] = (H[i] >>> 24) & 0xff; + output[i * 4 + 1] = (H[i] >>> 16) & 0xff; + output[i * 4 + 2] = (H[i] >>> 8) & 0xff; + output[i * 4 + 3] = H[i] & 0xff; + } + + return output; +} - // due to js not being able to shift numbers more than 32, we'll use this instead.. - // js stores all its numbers as a 64bit float with 53 bits of mantissa, so have room for 48 ok. - // positive shifts left, negative shifts right - function shift(number, shift) { - return number * Math.pow(2, shift); - } +// create a message signature +mavlink20.create_signature = function(key, msgbuf) { + const input = new Uint8Array(32 + msgbuf.length); + input.set(key, 0); + input.set(msgbuf, 32); - var thigh = shift(mav.signing.timestamp,-32) // 2 bytes from the top, shifted right by 32 bits - var tlow = (mav.signing.timestamp & 0xfffffff ) // 4 bytes from the bottom + const hash = mavlink20.sha256(input); + const sig = hash.slice(0, 6); + + return sig; +} + +// sign outgoing packet +mavlink20.message.prototype.sign_packet = function( mav) { + function packUint48LE(value) { + const bytes = [] + for (let i = 0; i < 6; i++) { + bytes.push(Number((value >> BigInt(8 * i)) & 0xFFn)); + } + return bytes; + } + + var tsbuf = packUint48LE(BigInt(mav.signing.timestamp)); - // I means unsigned 4bytes, H means unsigned 2 bytes // first add the linkid(1 byte) and timestamp(6 bytes) that start the signature - this._msgbuf = this._msgbuf.concat(jspack.Pack(' 0 indicates the interval at which it is sent. (int32_t) @@ -13702,7 +13974,7 @@ for the missing image. relative_alt : Altitude above ground (int32_t) q : Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) (float) image_index : Zero based index of this image (i.e. a new image will have index CAMERA_CAPTURE_STATUS.image count -1) (int32_t) - capture_result : Boolean indicating success (1) or failure (0) while capturing this image. (int8_t) + capture_result : Image was captured successfully (MAV_BOOL_TRUE). Values not equal to 0 or 1 are invalid. (int8_t) file_url : URL of image taken. Either local storage or http://foo.jpg if camera provides an HTTP interface. (char) */ @@ -13813,7 +14085,7 @@ A message containing logged data (see also MAV_CMD_LOGGING_START) target_component : component ID of the target (uint8_t) sequence : sequence number (can wrap) (uint16_t) length : data length (uint8_t) - first_message_offset : offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). (uint8_t) + first_message_offset : offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to UINT8_MAX if no start exists). (uint8_t) data : logged data (uint8_t) */ @@ -13852,7 +14124,7 @@ sent back target_component : component ID of the target (uint8_t) sequence : sequence number (can wrap) (uint16_t) length : data length (uint8_t) - first_message_offset : offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). (uint8_t) + first_message_offset : offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to UINT8_MAX if no start exists). (uint8_t) data : logged data (uint8_t) */ @@ -14138,6 +14410,54 @@ mavlink20.messages.camera_tracking_geo_status.prototype.pack = function(mav) { } +/* +Camera absolute thermal range. This can be streamed when the +associated `VIDEO_STREAM_STATUS.flag` bit +`VIDEO_STREAM_STATUS_FLAGS_THERMAL_RANGE_ENABLED` is set, but a GCS +may choose to only request it for the current active stream. Use +MAV_CMD_SET_MESSAGE_INTERVAL to define message interval (param3 +indicates the stream id of the current camera, or 0 for all streams, +param4 indicates the target camera_device_id for autopilot-attached +cameras or 0 for MAVLink cameras). + + time_boot_ms : Timestamp (time since system boot). (uint32_t) + stream_id : Video Stream ID (1 for first, 2 for second, etc.) (uint8_t) + camera_device_id : Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). (uint8_t) + max : Temperature max. (float) + max_point_x : Temperature max point x value (normalized 0..1, 0 is left, 1 is right), NAN if unknown. (float) + max_point_y : Temperature max point y value (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown. (float) + min : Temperature min. (float) + min_point_x : Temperature min point x value (normalized 0..1, 0 is left, 1 is right), NAN if unknown. (float) + min_point_y : Temperature min point y value (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown. (float) + +*/ + mavlink20.messages.camera_thermal_range = function( ...moreargs ) { + [ this.time_boot_ms , this.stream_id , this.camera_device_id , this.max , this.max_point_x , this.max_point_y , this.min , this.min_point_x , this.min_point_y ] = moreargs; + + this._format = '= 1 && this.buf[0] != this.protocol_marker ) { + if( this.buf.length >= 1 && + this.buf[0] != mavlink20.PROTOCOL_MARKER_V2 && + this.buf[0] != mavlink20.PROTOCOL_MARKER_V1) { - // Strip the offending initial byte and throw an error. + // Strip the offending initial bytes and throw an error. var badPrefix = this.buf[0]; - this.bufInError = this.buf.slice(0,1); - this.buf = this.buf.slice(1); + var idx1 = this.buf.indexOf(mavlink20.PROTOCOL_MARKER_V1); + var idx2 = this.buf.indexOf(mavlink20.PROTOCOL_MARKER_V2); + if (idx1 == -1) { + idx1 = idx2; + } + if (idx1 == -1 && idx2 == -1) { + this.bufInError = this.buf; + this.buf = new Uint8Array(); + } else { + this.bufInError = this.buf.slice(0,idx1); + this.buf = this.buf.slice(idx1); + } this.expected_length = mavlink20.HEADER_LEN; //initially we 'expect' at least the length of the header, later parseLength corrects for this. throw new Error("Bad prefix ("+badPrefix+")"); } @@ -18403,31 +19112,30 @@ MAVLink20Processor.prototype.parsePrefix = function() { // us know if we have signing enabled, which affects the real-world length by the signature-block length of 13 bytes. // once successful, 'this.expected_length' is correctly set for the whole packet. MAVLink20Processor.prototype.parseLength = function() { - - if( this.buf.length >= 3 ) { - var unpacked = jspack.Unpack('BBB', this.buf.slice(0, 3)); - var magic = unpacked[0]; // stx ie fd or fe etc - this.expected_length = unpacked[1] + mavlink20.HEADER_LEN + 2 // length of message + header + CRC (ie non-signed length) - this.incompat_flags = unpacked[2]; - // mavlink2 only.. in mavlink1, incompat_flags var above is actually the 'seq', but for this test its ok. - if ((magic == mavlink20.PROTOCOL_MARKER_V2 ) && ( this.incompat_flags & mavlink20.MAVLINK_IFLAG_SIGNED )){ - this.expected_length += mavlink20.MAVLINK_SIGNATURE_BLOCK_LEN; - } + + if( this.buf.length >= 3 ) { + var unpacked = jspack.Unpack('BBB', this.buf.slice(0, 3)); + var magic = unpacked[0]; // stx ie fd or fe etc + this.expected_length = unpacked[1] + mavlink20.HEADER_LEN + 2 // length of message + header + CRC (ie non-signed length) + this.incompat_flags = unpacked[2]; + // mavlink2 only.. in mavlink1, incompat_flags var above is actually the 'seq', but for this test its ok. + if ((magic == mavlink20.PROTOCOL_MARKER_V2 ) && ( this.incompat_flags & mavlink20.MAVLINK_IFLAG_SIGNED )){ + this.expected_length += mavlink20.MAVLINK_SIGNATURE_BLOCK_LEN; + } } } -// input some data bytes, possibly returning a new message - python equiv function is called parse_char / __parse_char_legacy +// input some data bytes, possibly returning a new message - python equiv function is called parse_char / __parse_char_legacy +// c can be null to process any remaining data in the input buffer from a previous call MAVLink20Processor.prototype.parseChar = function(c) { - if (c == null) { - return - } - var m = null; try { - this.pushBuffer(c); + if (c != null) { + this.pushBuffer(c); + } this.parsePrefix(); this.parseLength(); m = this.parsePayload(); @@ -18441,10 +19149,10 @@ MAVLink20Processor.prototype.parseChar = function(c) { } // emit a packet-specific message as well as a generic message, user/s can choose to use either or both of these. - //if(null != m) { - // this.emit(m._name, m); - // this.emit('message', m); - //} + if (isNode && null != m) { + this.emit(m._name, m); + this.emit('message', m); + } return m; @@ -18488,7 +19196,7 @@ MAVLink20Processor.prototype.parsePayload = function() { // input some data bytes, possibly returning an array of new messages MAVLink20Processor.prototype.parseBuffer = function(s) { - + // Get a message, if one is available in the stream. var m = this.parseChar(s); @@ -18496,12 +19204,13 @@ MAVLink20Processor.prototype.parseBuffer = function(s) { if ( null === m ) { return null; } - + // While more valid messages can be read from the existing buffer, add // them to the array of new messages and return them. - var ret = [m]; + var ret = []; + ret.push(m); while(true) { - m = this.parseChar(); + m = this.parseChar(null); if ( null === m ) { // No more messages left. return ret; @@ -18513,94 +19222,84 @@ MAVLink20Processor.prototype.parseBuffer = function(s) { //check signature on incoming message , many of the comments in this file come from the python impl MAVLink20Processor.prototype.check_signature = function(msgbuf, srcSystem, srcComponent) { - - //timestamp_buf = msgbuf[-12:-6] - var timestamp_buf= msgbuf.slice(-12,-6); - - //link_id = msgbuf[-13] - var link_id= new Buffer.from(msgbuf.slice(-13,-12)); // just a single byte really, but returned as a buffer - link_id = link_id[0]; // get the first byte. + var timestamp_buf = msgbuf.slice(-12,-6); - //self.mav_sign_unpacker = jspack.Unpack('= 0; i--) { + value = (value << 8n) | BigInt(bytes[i]); + } + return value; + } + var timestamp = Number(unpackUint48LE(timestamp_buf)); + + // see if the timestamp is acceptable + + // we'll use a STRING containing these three things in it as a unique key eg: '0,1,1' + stream_key = new Array(link_id,srcSystem,srcComponent).toString(); + + if (stream_key in this.signing.stream_timestamps){ + if (timestamp <= this.signing.stream_timestamps[stream_key]){ + //# reject old timestamp + //console.log('old timestamp') + return false + } + }else{ + //# a new stream has appeared. Accept the timestamp if it is at most + //# one minute behind our current timestamp + if (timestamp + 6000*1000 < this.signing.timestamp){ + //console.log('bad new stream ', timestamp/(100.0*1000*60*60*24*365), this.signing.timestamp/(100.0*1000*60*60*24*365)) + return false + } + this.signing.stream_timestamps[stream_key] = timestamp; + //console.log('new stream',this.signing.stream_timestamps) + } + + // just the last 6 of 13 available are the actual sig . ie excluding the linkid(1) and timestamp(6) + var sigpart = msgbuf.slice(-6); + sigpart = Uint8Array.from(sigpart); + // not sig part 0- end-minus-6 + var notsigpart = msgbuf.slice(0,-6); + notsigpart = Uint8Array.from(notsigpart); + + var sig1 = mavlink20.create_signature(this.signing.secret_key, notsigpart); + + // Browser-compatible buffer comparison + var signaturesMatch; + if (isNode) { + signaturesMatch = Buffer.from(sig1).equals(Buffer.from(sigpart)); + } else { + // Compare arrays element by element in browser + signaturesMatch = sig1.length === sigpart.length && + sig1.every((val, index) => val === sigpart[index]); + } + if (!signaturesMatch) { + return false; + } + //# the timestamp we next send with is the max of the received timestamp and + //# our current timestamp + this.signing.timestamp = Math.max(this.signing.timestamp, timestamp+1); + return true +} /* decode a buffer as a MAVLink message */ MAVLink20Processor.prototype.decode = function(msgbuf) { - var magic, incompat_flags, compat_flags, mlen, seq, srcSystem, srcComponent, unpacked, msgId, signature_len; + var magic, incompat_flags, compat_flags, mlen, seq, srcSystem, srcComponent, unpacked, msgId, signature_len, header_len; // decode the header try { - unpacked = jspack.Unpack('BBBBBBBHB', msgbuf.slice(0, 10)); // the H in here causes msgIDlow to takeup 2 bytes, the rest 1 + if (msgbuf[0] == 253) { + var unpacked = jspack.Unpack('BBBBBBBHB', msgbuf.slice(0, 10)); // the H in here causes msgIDlow to takeup 2 bytes, the rest 1 magic = unpacked[0]; mlen = unpacked[1]; incompat_flags = unpacked[2]; @@ -18610,64 +19309,51 @@ MAVLink20Processor.prototype.decode = function(msgbuf) { srcComponent = unpacked[6]; var msgIDlow = ((unpacked[7] & 0xFF) << 8) | ((unpacked[7] >> 8) & 0xFF); // first-two msgid bytes var msgIDhigh = unpacked[8]; // the 3rd msgid byte - msgId = msgIDlow | (msgIDhigh<<16); // combined result. 0 - 16777215 24bit number + msgId = msgIDlow | (msgIDhigh<<16); // combined result. 0 - 16777215 24bit number + header_len = 10; +} else { + var unpacked = jspack.Unpack('BBBBBB', msgbuf.slice(0, 6)); + magic = unpacked[0]; + mlen = unpacked[1]; + seq = unpacked[2]; + srcSystem = unpacked[3]; + srcComponent = unpacked[4]; + msgID = unpacked[5]; + incompat_flags = 0; + compat_flags = 0; + header_len = 6; +} } catch(e) { throw new Error('Unable to unpack MAVLink header: ' + e.message); } - // TODO allow full parsing of 1.0 inside the 2.0 parser, this is just a start - if (magic == mavlink20.PROTOCOL_MARKER_V1){ - //headerlen = 6; - - // these two are in the same place in both v1 and v2 so no change needed: - //magic = magic; - //mlen = mlen; - - // grab mavlink-v1 header position info from v2 unpacked position - seq1 = incompat_flags; - srcSystem1 = compat_flags; - srcComponent1 = seq; - msgId1 = srcSystem; - // override the v1 vs v2 offsets so we get the correct data either way... - seq = seq1; - srcSystem = srcSystem1; - srcComponent = srcComponent1; - msgId = msgId1; - // don't exist in mavlink1, so zero-them - incompat_flags = 0; - compat_flags = 0; - signature_len = 0; - // todo add more v1 here and don't just return - return; - } - if (magic != this.protocol_marker) { throw new Error("Invalid MAVLink prefix ("+magic+")"); } - // is packet supposed to be signed? - if ( incompat_flags & mavlink20.MAVLINK_IFLAG_SIGNED ){ - signature_len = mavlink20.MAVLINK_SIGNATURE_BLOCK_LEN; - } else { - signature_len = 0; - } - - // header's declared len compared to packets actual len - var actual_len = (msgbuf.length - (mavlink20.HEADER_LEN + 2 + signature_len)); - var actual_len_nosign = (msgbuf.length - (mavlink20.HEADER_LEN + 2 )); - - if ((mlen == actual_len) && (signature_len > 0)){ - var len_if_signed = mlen+signature_len; - //console.log("Packet appears signed && labeled as signed, OK. msgId=" + msgId); - - } else if ((mlen == actual_len_nosign) && (signature_len > 0)){ - - var len_if_signed = mlen+signature_len; + // is packet supposed to be signed? + if ( incompat_flags & mavlink20.MAVLINK_IFLAG_SIGNED ){ + signature_len = mavlink20.MAVLINK_SIGNATURE_BLOCK_LEN; + } else { + signature_len = 0; + } + + // header's declared len compared to packets actual len + var actual_len = (msgbuf.length - (header_len + 2 + signature_len)); + var actual_len_nosign = (msgbuf.length - (header_len + 2 )); + + if ((mlen == actual_len) && (signature_len > 0)){ + var len_if_signed = mlen+signature_len; + //console.log("Packet appears signed && labeled as signed, OK. msgId=" + msgId); + + } else if ((mlen == actual_len_nosign) && (signature_len > 0)){ + + var len_if_signed = mlen+signature_len; throw new Error("Packet appears unsigned when labeled as signed. Got actual_len "+actual_len_nosign+" expected " + len_if_signed + ", msgId=" + msgId); - - } else if( mlen != actual_len) { - throw new Error("Invalid MAVLink message length. Got " + (msgbuf.length - (mavlink20.HEADER_LEN + 2)) + " expected " + mlen + ", msgId=" + msgId); + + } else if( mlen != actual_len) { + throw new Error("Invalid MAVLink message length. Got " + (msgbuf.length - (header_len + 2)) + " expected " + mlen + ", msgId=" + msgId); } @@ -18675,83 +19361,81 @@ MAVLink20Processor.prototype.decode = function(msgbuf) { throw new Error("Unknown MAVLink message ID (" + msgId + ")"); } - // here's the common chunks of packet we want to work with below.. - var headerBuf= msgbuf.slice(mavlink20.HEADER_LEN); // first10 - var sigBuf = msgbuf.slice(-signature_len); // last 13 or nothing - var crcBuf1 = msgbuf.slice(-2); // either last-2 or last-2-prior-to-signature - var crcBuf2 = msgbuf.slice(-15,-13); // either last-2 or last-2-prior-to-signature - var payloadBuf = msgbuf.slice(mavlink20.HEADER_LEN, -(signature_len+2)); // the remaining bit between the header and the crc - var crcCheckBuf = msgbuf.slice(1, -(signature_len+2)); // the part uses to calculate the crc - ie between the magic and signature, + // here's the common chunks of packet we want to work with below.. + var payloadBuf = msgbuf.slice(mavlink20.HEADER_LEN, -(signature_len+2)); // the remaining bit between the header and the crc + var crcCheckBuf = msgbuf.slice(1, -(signature_len+2)); // the part uses to calculate the crc - ie between the magic and signature, // decode the payload // refs: (fmt, type, order_map, crc_extra) = mavlink20.map[msgId] var decoder = mavlink20.map[msgId]; // decode the checksum - var receivedChecksum = undefined; - if ( signature_len == 0 ) { // unsigned - try { - receivedChecksum = jspack.Unpack(' payloadBuf.length) { payloadBuf = this.concat_buffer(payloadBuf, new Uint8Array(paylen - payloadBuf.length).fill(0)); } @@ -18822,18 +19506,18 @@ MAVLink20Processor.prototype.decode = function(msgbuf) { // construct the message object try { // args at this point might look like: { '0': 6, '1': 8, '2': 0, '3': 0, '4': 3, '5': 3 } - var m = new decoder.type; // make a new 'empty' instance of the right class, + var m = new decoder.type(); // make a new 'empty' instance of the right class, m.set(args,false); // associate ordered-field-numbers to names, after construction not during. } catch (e) { throw new Error('Unable to instantiate MAVLink message of type '+decoder.type+' : ' + e.message); } - m._signed = sig_ok; - if (m._signed) { m._link_id = msgbuf[-13]; } + m._signed = sig_ok; + if (m._signed) { m._link_id = msgbuf[-13]; } m._msgbuf = msgbuf; - m._payload = payloadBuf + m._payload = payloadBuf; m.crc = receivedChecksum; m._header = new mavlink20.header(msgId, mlen, seq, srcSystem, srcComponent, incompat_flags, compat_flags); this.log(m); @@ -18841,8 +19525,22 @@ MAVLink20Processor.prototype.decode = function(msgbuf) { } -// allow loading as both common.js (Node), and/or vanilla javascript in-browser -if(typeof module === "object" && module.exports) { - module.exports = {mavlink20, MAVLink20Processor}; +// Browser and Node.js compatible module exports +if (!isNode) { + // For browsers, attach to window or use global namespace + if (typeof window !== 'undefined') { + window.mavlink20 = mavlink20; + window.MAVLink20Processor = MAVLink20Processor; + } + // Also support global assignment + if (typeof global !== 'undefined') { + global.mavlink20 = mavlink20; + global.MAVLink20Processor = MAVLink20Processor; + } +} else { + // For Node.js, use module.exports + if (typeof module === "object" && module.exports) { + module.exports = {mavlink20, MAVLink20Processor}; + } } diff --git a/TelemetryDashboard/TelemetryDashboard.js b/TelemetryDashboard/TelemetryDashboard.js index 79f45f5b..4c8227d3 100644 --- a/TelemetryDashboard/TelemetryDashboard.js +++ b/TelemetryDashboard/TelemetryDashboard.js @@ -43,9 +43,15 @@ function setup_connect(button_svg, button_color) { const url_input = tip_div.querySelector(`input[id="target_url"]`) + const heartbeat_checkbox = tip_div.querySelector(`input[id="send_heartbeat"]`) + const passphrase_input = tip_div.querySelector(`input[id="signing_passphrase"]`) + const connect_button = tip_div.querySelector(`input[id="connection_button"]`) const disconnect_button = tip_div.querySelector(`input[id="disconnection_button"]`) + const system_id_input = tip_div.querySelector(`input[id="system_id"]`); + const component_id_input = tip_div.querySelector(`input[id="component_id"]`); + // Websocket object let ws = null let expecting_close = false @@ -60,6 +66,17 @@ function setup_connect(button_svg, button_color) { } set_inputs(false) + function apply_ids() { + // clamp to valid ranges; fall back to GCS-like defaults 255/190 + let sid = parseInt(system_id_input?.value || "255", 10); + let cid = parseInt(component_id_input?.value || "190", 10); + if (!Number.isFinite(sid) || sid < 1 || sid > 255) sid = 255; + if (!Number.isFinite(cid) || cid < 0 || cid > 255) cid = 190; + + MAVLink.srcSystem = sid; + MAVLink.srcComponent = cid; + } + // Connect to WebSocket server function connect(target, auto_connect) { // Make sure we are not connected to something else @@ -82,6 +99,9 @@ function setup_connect(button_svg, button_color) { ws.onopen = () => { button_color("green") + // setup system IDs + apply_ids(); + // Hide tip tip.hide() @@ -92,8 +112,47 @@ function setup_connect(button_svg, button_color) { url_input.value = target // Have been connected - been_connected = true - } + been_connected = true; + setup_passphase = false; + + heartbeat_interval = setInterval(() => { + try { + if (!setup_passphase) { + const passphrase = passphrase_input.value.trim(); + if (passphrase.length > 0) { + setup_passphase = true; + const enc = new TextEncoder(); + const data = enc.encode(passphrase); + const hash = mavlink20.sha256(data); + MAVLink.signing.secret_key = new Uint8Array(hash); + MAVLink.signing.sign_outgoing = true; + } + } + + if (heartbeat_checkbox.checked) { + const msg = new mavlink20.messages.heartbeat( + 6, // MAV_TYPE_GCS + 8, // MAV_AUTOPILOT_INVALID + 0, // base_mode + 0, // custom_mode + 4 // MAV_STATE_ACTIVE + ); + const pkt = msg.pack(MAVLink); + ws.send(Uint8Array.from(pkt)); + //console.log("Sent HEARTBEAT", pkt); + } + } catch (e) { + console.error("Error sending HEARTBEAT:", e.message); + console.error(e.stack); + + if (heartbeat_interval !== null) { + clearInterval(heartbeat_interval) + heartbeat_interval = null + console.warn("Heartbeat disabled; will retry on reconnect.") + } + } + }, 1000); + } ws.onclose = () => { if ((auto_connect === true) && !been_connected) { @@ -115,10 +174,15 @@ function setup_connect(button_svg, button_color) { } ws.onmessage = (msg) => { - // Feed data to MAVLink parser and forward messages - for (const char of new Uint8Array(msg.data)) { - const m = MAVLink.parseChar(char) - if ((m != null) && (m._id != -1)) { + // Feed data to MAVLink parser and forward messages + MAVLink.pushBuffer(new Uint8Array(msg.data)); + while (true) { + const m = MAVLink.parseChar(null); + if (m === null) { + break; + } + if (m._id != -1) { + //console.log(m); // Got message with known ID // Sent to each Widget for (const widget of grid.getGridItems()) { @@ -130,7 +194,6 @@ function setup_connect(button_svg, button_color) { } } } - } // Disconnect from WebSocket server diff --git a/TelemetryDashboard/cli_test.js b/TelemetryDashboard/cli_test.js new file mode 100644 index 00000000..4c8532e0 --- /dev/null +++ b/TelemetryDashboard/cli_test.js @@ -0,0 +1,113 @@ +//!/usr/bin/env node + +const WebSocket = require('ws'); + +global.jspack = new (require('./MAVLink/local_modules/jspack/jspack.js')).default; + +const mavlib = require('./MAVLink/mavlink.js'); // Use local MAVLink definition + +if (process.argv.length < 3) { + console.error("Usage: node ws_mavlink.js "); + process.exit(1); +} + +//process.env.NODE_TLS_REJECT_UNAUTHORIZED = "0"; + +const url = process.argv[2]; +const ws = new WebSocket(url); +ws.binaryType = "arraybuffer"; + +// Create a MAVLink v2 parser +parser = new MAVLink20Processor() + +let signing_passphrase = null; +if (process.argv.length > 3) { + signing_passphrase = process.argv[3]; +} + +const crypto = require('crypto'); + +function passphrase_to_key(passphrase) { + return crypto.createHash('sha256').update(Buffer.from(passphrase, 'ascii')).digest(); +} + +if (signing_passphrase) { + parser.signing.secret_key = passphrase_to_key(signing_passphrase); + parser.signing.sign_outgoing = true; +} + +let heartbeat_interval; + +ws.on('open', () => { + console.log(`Connected to ${url}`); + + heartbeat_interval = setInterval(() => { + try { + const msg = new mavlib.mavlink20.messages.heartbeat( + 6, // MAV_TYPE_GCS + 8, // MAV_AUTOPILOT_INVALID + 0, // base_mode + 0, // custom_mode + 4 // MAV_STATE_ACTIVE + ); + const pkt = msg.pack(parser); + ws.send(pkt); + console.log("Sent HEARTBEAT"); + } catch (e) { + console.error("Error sending HEARTBEAT:", e.message); + console.error(e.stack); + } + }, 1000); +}); + +function mav_pretty(msg) { + // console.log(JSON.stringify(msg, null, 2)); + + if (!msg || !msg._name || !msg.fieldnames) { + return ""; + } + + const name = msg._name; + const fields = msg.fieldnames + .map(fn => { + let val = msg[fn]; + if (typeof val === "string") { + val = `"${val}"`; + } else if (Array.isArray(val)) { + val = "[" + val.join(", ") + "]"; + } + return `${fn}=${val}`; + }) + .join(", "); + + return `${name} { ${fields} }`; +} + + +ws.on('message', (data) => { + const buf = (data instanceof ArrayBuffer) ? Buffer.from(data) : + (Buffer.isBuffer(data) ? data : Buffer.from(data.buffer || data)); + + console.log(`Received ${buf.length} bytes: [${buf.slice(0, 10).toString('hex')}...]`); + + for (const b of buf) { + try { + const msg = parser.parseChar(b); + if (msg) { + console.log(`MAVLink message ID: ${msg._id}`); + console.log(mav_pretty(msg)); + } + } catch (e) { + console.warn(`Parser error on byte 0x${byte.toString(16)}: ${e.message}`); + } + } +}); + +ws.on('close', () => { + console.log("WebSocket closed"); + clearInterval(heartbeat_interval); +}); + +ws.on('error', (err) => { + console.error("WebSocket error:", err.message); +}); diff --git a/TelemetryDashboard/index.html b/TelemetryDashboard/index.html index 1c4fe8ee..d2c229d9 100644 --- a/TelemetryDashboard/index.html +++ b/TelemetryDashboard/index.html @@ -176,6 +176,21 @@
Dashboard settings


+
+ + +
+
+ + + + +
+
+
+ + +

@@ -200,20 +215,21 @@
Dashboard settings
}) // Load grid - let grid - let grid_changed = false - load_default_grid() + let grid; + let grid_changed = false; // MAVLink parsing - MAVLink = new MAVLink20Processor() + let MAVLink = new MAVLink20Processor(); + + load_default_grid(); // Setup editor for use later - init_editor() + init_editor(); // Setup widget pallet - init_pallet() + init_pallet(); // Bind unload event to allow prompt for save - window.addEventListener('beforeunload', handle_unload) + window.addEventListener('beforeunload', handle_unload);