diff --git a/Chat/Readme.md b/Chat/Readme.md new file mode 100644 index 00000000..25dea3f9 --- /dev/null +++ b/Chat/Readme.md @@ -0,0 +1,3 @@ +## Chat + +OpenAI based Chat module diff --git a/Chat/index.html b/Chat/index.html new file mode 100644 index 00000000..460cd30f --- /dev/null +++ b/Chat/index.html @@ -0,0 +1,2126 @@ + + + + + + + Chat + + + + + + + + + + + + + + +
+

ArduPilot AI Chat Control

+ +
+ +
+
+

AI Chat Interface

+ +
+ +
+
+

Welcome! I'm your ArduPilot AI assistant. You can control the drone using + natural language commands like:

+
    +
  • "Arm the drone"
    • +
  • "Take off to 10 meters"
    • +
  • "Fly north for 50 meters"
    • +
  • "Return to launch"
    • +
  • "Change to guided mode"
    • +
  • "Emergency stop"
    • +
+
+
+ +
+ + + + + +
+ + + +
+ + + +
+ + +
+ + +
+

AI Configuration

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

Debug Output

+ + +
+ +
+
+
+ + + + + + + + + + + + + \ No newline at end of file diff --git a/Chat/shared.js b/Chat/shared.js new file mode 100644 index 00000000..054120ab --- /dev/null +++ b/Chat/shared.js @@ -0,0 +1,11 @@ +// shared.js +export let MAVLink = null; +export let mavlink_ws = null; + +export function setMAVLink(instance) { + MAVLink = instance; +} + +export function setMavlinkWS(ws) { + mavlink_ws = ws; +} diff --git a/Chat/style.css b/Chat/style.css new file mode 100644 index 00000000..b20deff8 --- /dev/null +++ b/Chat/style.css @@ -0,0 +1,24 @@ +/* Chat messages */ +.user-text { + align-self: flex-end; + background-color: #2563eb; /* blue-600 */ + color: #fff; + padding: 10px 16px; + border-radius: 18px 18px 4px 18px; + max-width: 70%; + margin: 4px 0; + font-size: 1rem; + box-shadow: 0 1px 2px rgba(0,0,0,0.07); +} + +.assistant-text { + align-self: flex-start; + background-color: #f3f4f6; /* gray-100 */ + color: #222; + padding: 10px 16px; + border-radius: 18px 18px 18px 4px; + max-width: 70%; + margin: 4px 0; + font-size: 1rem; + box-shadow: 0 1px 2px rgba(0,0,0,0.07); +} \ No newline at end of file diff --git a/Dev/index.html b/Dev/index.html index 618af763..44107d76 100644 --- a/Dev/index.html +++ b/Dev/index.html @@ -108,6 +108,17 @@

Thrust Expo

+ + + + + +

Chat

+ OpenAI Chat module + Use an AI chat to control your vehicle + + + diff --git a/images/Chat_Icon.png b/images/Chat_Icon.png new file mode 100644 index 00000000..208bae54 Binary files /dev/null and b/images/Chat_Icon.png differ diff --git a/modules/MAVLink/local_modules/README.md b/modules/MAVLink/local_modules/README.md new file mode 100644 index 00000000..ad5a1382 --- /dev/null +++ b/modules/MAVLink/local_modules/README.md @@ -0,0 +1,6 @@ +This folder is a locally modified copy of some Node/npm packages 'jspack' and 'long'. we have copied them here and tweaked them to be compatible with our needs, please see their respective README.md file for their original info, which we have not changed. + +This README.md serves to make you aware that these two packages as stored here in the 'jspack' and 'long' folders ARE MODIFIED from the originals. +By placing this statement here, and putting a notice in long.js as well, we feel are in compliance with the LICENSE file of 'long' , which requires us to tell you they are modified. + +We have included their original license files, in compliance with them, as both license/s permit distribution of derived works in source and/or binary form. diff --git a/modules/MAVLink/local_modules/jspack/.npmignore b/modules/MAVLink/local_modules/jspack/.npmignore new file mode 100644 index 00000000..be1ea81a --- /dev/null +++ b/modules/MAVLink/local_modules/jspack/.npmignore @@ -0,0 +1,19 @@ +lib-cov +*.seed +*.log +*.csv +*.dat +*.out +*.pid +*.gz +*.pyc + +pids +logs +results + +npm-debug.log +node_modules + +**~ +**.swp diff --git a/modules/MAVLink/local_modules/jspack/LICENSE b/modules/MAVLink/local_modules/jspack/LICENSE new file mode 100644 index 00000000..d646dd72 --- /dev/null +++ b/modules/MAVLink/local_modules/jspack/LICENSE @@ -0,0 +1,26 @@ +Copyright (c) 2008, Fair Oaks Labs, Inc. +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are +permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this list + of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, this + list of conditions and the following disclaimer in the documentation and/or other + materials provided with the distribution. + + * Neither the name of Fair Oaks Labs, Inc. nor the names of its contributors may be + used to endorse or promote products derived from this software without specific + prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL +THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF +THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/modules/MAVLink/local_modules/jspack/README.md b/modules/MAVLink/local_modules/jspack/README.md new file mode 100644 index 00000000..fee0cdc3 --- /dev/null +++ b/modules/MAVLink/local_modules/jspack/README.md @@ -0,0 +1,147 @@ +jspack - library to pack primitives to octet arrays +==================================================== + +[![Build status](https://travis-ci.org/birchroad/node-jspack.svg?branch=master)](https://travis-ci.org/birchroad/node-jspack) + +## Disclaimer +The jspack module and documentation are essentially ports of the +Python struct module and documentation, with such changes as were necessary. The port was originally made by Fair Oaks Labs, Inc. and published at http://code.google.com/p/jspack/ +If any Python people are miffed that their documentation got ripped off, let me know, +and I'll gladly revise them. + +This module performs conversions between JavaScript values and C structs +represented as octet arrays (i.e. JavaScript arrays of integral numbers +between 0 and 255, inclusive). It uses format strings (explained below) as +compact descriptions of the layout of the C structs and the intended conversion +to/from JavaScript values. This can be used to handle binary data stored in +files, or received from network connections or other sources. + +## Install + npm install jspack + +## Reference + +The module defines the following functions: + +### Unpack(fmt, a, p) +Return an array containing values unpacked from the octet array a, +beginning at position p, according to the supplied format string. If there +are more octets in a than required by the format string, the excess is +ignored. If there are fewer octets than required, Unpack() will return +undefined. If no value is supplied for the p argument, zero is assumed. + +### PackTo(fmt, a, p, values) +Pack and store the values array into the supplied octet array a, beginning +at position p. If there are more values supplied than are specified in the +format string, the excess is ignored. If there are fewer values supplied, +PackTo() will return false. If there is insufficient space in a to store +the packed values, PackTo() will return false. On success, PackTo() returns +the a argument. If any value is of an inappropriate type, the results are +undefined. + +### Pack(fmt, values) +Return an octet array containing the packed values array. If there are +more values supplied than are specified in the format string, the excess is +ignored. If there are fewer values supplied, Pack() will return false. If +any value is of an inappropriate type, the results are undefined. + +### CalcLength(fmt) +Return the number of octets required to store the given format string. + + +## Formats +Format characters have the following meanings; the conversion between C and +JavaScript values should be obvious given their types: + + Format | C Type | JavaScript Type | Size (octets) | Notes + ------------------------------------------------------------------- + A | char[] | Array | Length | (1) + x | pad byte | N/A | 1 | + c | char | string (length 1) | 1 | (2) + b | signed char | number | 1 | (3) + B | unsigned char | number | 1 | (3) + h | signed short | number | 2 | (3) + H | unsigned short | number | 2 | (3) + i | signed int | number | 4 | (3) + I | unsigned int | number | 4 | (3) + l | signed long | number | 4 | (3) + L | unsigned long | number | 4 | (3) + q | signed long | number | 8 | (6) + Q | unsigned long | number | 8 | (6) + s | char[] | string | Length | (2) + f | float | number | 4 | (4) + d | double | number | 8 | (5) + +*Notes:* + + **(1)** The "A" code simply returns a slice of the source octet array. This is + primarily useful when a data structure contains bytes which are subject to + multiple interpretations (e.g. unions), and the data structure is being + decoded in multiple passes. + + **(2)** The "c" and "s" codes handle strings with codepoints between 0 and 255, + inclusive. The data are not bounds-checked, so strings containing characters + with codepoints outside this range will encode to "octet" arrays that contain + values outside the range of an octet. Furthermore, since these codes decode + octet arrays by assuming the octets represent UNICODE codepoints, they may + not "correctly" decode bytes in the range 128-255, since that range is subject + to multiple interpretations. Caveat coder! + + **(3)** The 8 "integer" codes clip their encoded values to the minima and maxmima + of their respective types: If you invoke Struct.Pack('b', [-129]), for + instance, the result will be [128], which is the octet encoding of -128, + which is the minima of a signed char. Similarly, Struct.Pack('h', [-32769]) + returns [128, 0]. Fractions are truncated. + + **(4)** Since JavaScript doesn't natively support 32-bit floats, whenever a float + is stored, the source JavaScript number must be rounded. This module applies + correct rounding during this process. Numbers with magnitude greater than or + equal to 2^128-2^103 round to either positive or negative Infinity. The + rounding algorithm assumes that JavaScript is using exactly 64 bits of + floating point precision; 128-bit floating point will result in subtle errors. + + **(5)** This module assumes that JavaScript is using 64 bits of floating point + precision, so the "d" code performs no rounding. 128-bit floating point will + cause the "d" code to simply truncate significands to 52 bits. + + **(6)** Since 64bit longs cannot be represented by numbers JavaScript, this version of + jspack will process longs as arrays in the form: ```[lowBits, hightBits]```. The + decoded long array contains a third element, the unsigned flag, which is ```false``` for signed + and ```true``` for unsigned values. + This representation is similar to what [Long.js](https://github.com/dcodeIO/Long.js), and + therefore the [Google Closure Libaray](https://github.com/google/closure-library), uses. + See [test/int64.js](test/int64.js) for examples how to work with Long.js. + +A format character may be preceded by an integral repeat count. For example, +the format string "4h" means exactly the same thing as "hhhh". + +Whitespace characters between formats are ignored; a count and its format must +not be separated by whitespace, however. + +For the "A" format character, the count is interpreted as the size of the +array, not a repeat count as for the other format characters; for example, "10A" +means a single 10-octet array. When packing, the Array is truncated or padded +with 0 bytes as appropriate to make it conform to the specified length. When +unpacking, the resulting Array always has exactly the specified number of bytes. +As a special case, "0A" means a single, empty Array. + +For the "s" format character, the count is interpreted as the size of the +string, not a repeat count as for the other format characters; for example, +"10s" means a single 10-byte string, while "10c" means 10 characters. When +packing, the string is truncated or padded with 0 bytes as appropriate to make +it conform to the specified length. When unpacking, the resulting string always +has exactly the specified number of bytes. As a special case, "0s" means a +single, empty string (while "0c" means 0 characters). + + +By default, C numbers are represented in network (or big-endian) byte order. +Alternatively, the first character of the format string can be used to indicate +byte order of the packed data, according to the following table: + + Character | Byte Order + ---------------------------------- + < | little-endian + > | big-endian + ! | network (= big-endian) + +If the first character is not one of these, "!" is assumed. diff --git a/modules/MAVLink/local_modules/jspack/jspack.js b/modules/MAVLink/local_modules/jspack/jspack.js new file mode 100644 index 00000000..20095fed --- /dev/null +++ b/modules/MAVLink/local_modules/jspack/jspack.js @@ -0,0 +1,891 @@ +/** + * @license + + Copyright © 2008 Fair Oaks Labs, Inc. + All rights reserved. + + This file is Modified from the original, by buzz 2020: + - ran thru http://www.jsnice.org/ and manually renamed the variables to be clearer + - added optionally enabled debugging/verbose/printfs throughout + - bugfixes and integration so it now passes our mavlink.js testsuite/s + - please see README.md in the upper level folder. +*/ +'use strict'; + +//var Long = require('long'); + +let DEBUG = false; + +/** + * @return {undefined} + */ +function JSPack() { + var el; + /** @type {boolean} */ + var booleanIsBigEndian = false; + var m = this; + + + /** + * @param {!Object} octet_array_a + * @param {number} offset_p + * @param {number} len + * @return {?} + */ + //Raw byte arrays + // m._DeArray = function(octet_array_a, offset_p, len) { + // if (DEBUG) console.log("zzz1"); + // return [octet_array_a.slice(offset_p, offset_p + len)]; + //}; + + /** + * @param {!Array} to_octet_array_a + * @param {number} offset_p + * @param {number} len + * @param {!NodeList} from_array_v + * @return {undefined} + */ + // m._EnArray = function(to_octet_array_a, offset_p, len, from_array_v) { + // if (DEBUG) console.log("zzz2"); + /** @type {number} */ + // var i = 0; + // for (; i < len; to_octet_array_a[offset_p + i] = from_array_v[i] ? from_array_v[i] : 0, i++) { + // } + //}; + + + /** + * @param {!Object} octet_array_a + * @param {number} offset_p + * @return {?} + */ + // ASCII characters + m._DeChar = function(octet_array_a, offset_p) { + if (DEBUG) console.log("zzz3"); + return String.fromCharCode(octet_array_a[offset_p]); + }; + /** + * @param {!Array} to_octet_array_a + * @param {number} offset_p + * @param {string} from_str_array_v + * @return {undefined} + */ + // m._EnChar = function(to_octet_array_a, offset_p, from_str_array_v) { + // if (DEBUG) console.log("zzz4"); + // /** @type {number} */ + // to_octet_array_a[offset_p] = from_str_array_v.charCodeAt(0); + // }; + + + /** + * @param {!Object} octet_array_a + * @param {number} offset_p + * @return {?} + */ + //Little-endian (un)signed N-byte integers + m._DeInt = function(octet_array_a, offset_p) { + if (DEBUG) console.log("zzz5"); + /** @type {number} */ + var lsb = booleanIsBigEndian ? el.len - 1 : 0; + /** @type {number} */ + var nsb = booleanIsBigEndian ? -1 : 1; + /** @type {number} */ + var stop = lsb + nsb * el.len; + var rv; + var i; + var f; + /** @type {number} */ + rv = 0; + /** @type {number} */ + i = lsb; + /** @type {number} */ + f = 1; + for (; i != stop; rv = rv + octet_array_a[offset_p + i] * f, i = i + nsb, f = f * 256) { + } + if (el.bSigned && rv & Math.pow(2, el.len * 8 - 1)) { + /** @type {number} */ + rv = rv - Math.pow(2, el.len * 8); + } + return rv; + }; + + + /** + * @param {!Array} octet_array_a + * @param {number} offset_p + * @param {number} val + * @return {undefined} + */ + m._EnInt = function(octet_array_a, offset_p, val) { + if (DEBUG) console.log("chunk-from: "+val); + /** @type {number} */ + var lsb = booleanIsBigEndian ? el.len - 1 : 0; + /** @type {number} */ + var nsb = booleanIsBigEndian ? -1 : 1; + /** @type {number} */ + var stop = lsb + nsb * el.len; + var i; + // range limit: + if (val < el.min ) { + val = el.min; + console.log("value limited to MIN:"+val); + } + if (val > el.max ) { + val = el.max; + console.log("value limited to MAX:"+val); + } + /** @type {number} */ + i = lsb; + if (DEBUG) console.log("booleanIsBigEndian:"+booleanIsBigEndian); + if (DEBUG) console.log("el.len:"+el.len); + if (DEBUG) console.log("lsb:"+lsb); + if (DEBUG) console.log("nsb:"+nsb); + if (DEBUG) console.log("i:"+i); + if (DEBUG) console.log("stop:"+stop); + for (; i != stop; ) { + + var to = JSON.stringify(val&255); + if (DEBUG) console.log("chunk as bytes: "+to); + + octet_array_a[offset_p + i] = val & 255; + i = i + nsb; + val = val >> 8; + + + } + }; + + + /** + * @param {!Object} octet_array_a + * @param {number} offset_p + * @param {number} len + * @return {?} + */ + // ASCII character strings + m._DeString = function(octet_array_a, offset_p, len) { + if (DEBUG) console.log("zzz7"); + /** @type {!Array} */ + var retval = new Array(len); + /** @type {number} */ + var i = 0; + for (; i < len; retval[i] = String.fromCharCode(octet_array_a[offset_p + i]), i++) { + } + return retval.join(""); + }; + /** + * @param {!Array} octet_array_a + * @param {number} offset_p + * @param {number} len + * @param {string} strval + * @return {undefined} + */ + m._EnString = function(octet_array_a, offset_p, len, strval) { + if (DEBUG) console.log("zzz8"); + var t; + /** @type {number} */ + if ( DEBUG ) console.log("strencode before: "+octet_array_a+"\np:"+offset_p+" len:"+len+" strval:"+strval) + var i = 0; + //if (DEBUG) console.log("strval:"+strval); +//console.trace("Here I am!") + + // we all strings to be passed in as a string of characters, or a an array or buffer of them is ok too + + if (typeof strval.charCodeAt === "function") { + for (; i < len; octet_array_a[offset_p + i] = (t = strval.charCodeAt(i)) ? t : 0, i++) { + if ( t > 255 ) console.log("ERROR ERROR ERROR ERROR ERROR ERROR - It seems u passed unicode/utf-8/etc to jspack, not 8 bit ascii. please use .toString('binary'); not .toString();"); + } + if ( DEBUG ) console.log("strencode from CHAR-string."); + + } else if (Array.isArray(strval)) { + for (; i < len; octet_array_a[offset_p + i] = (t = strval[i]) ? t : 0, i++) { + // referring directly to 't' inside this loop is bad, seems delayed by an iteration, but strval[i] is ok. + if ( strval[i] > 255 ) console.log("ERROR ERROR ERROR ERROR ERROR ERROR - It seems u passed unicode/utf-8/etc, or array data with values > 255, to jspack, not 8 bit ascii.\n(bad Array data)"+strval[i]); + } + if ( DEBUG ) console.log("strencode from ARRAY."); + + } else if (Buffer.isBuffer(strval)) { + for (; i < len; octet_array_a[offset_p + i] = (t = strval[i]) ? t : 0, i++) { + if ( strval[i] > 255 ) console.log("ERROR ERROR ERROR ERROR ERROR ERROR - It seems u passed unicode/utf-8/etc to jspack, not 8 bit ascii. \n(bad Buffer data)"+strval[i]); + } + if ( DEBUG ) console.log("strencode from Buffer."); + + } else { + console.log("ERROR encoding string _EnString: array:"+octet_array_a+" p:"+offset_p+" len:"+len+" strval:"+JSON.stringify(strval)) +} + }; + + + /** + * @param {!Object} octet_array_a + * @param {number} offset_p + * @return {?} + */ + // Little-endian N-bit IEEE 754 floating point + m._De754 = function(octet_array_a, offset_p) { + if (DEBUG) console.log("zzz9"); + var bool_s; + var exponent; + var mantissa; + var i; + var d; + var nBits; + var mantissaLen; + var exponentLen; + var eBias; + var eMax; + mantissaLen = el.mLen; + /** @type {number} */ + exponentLen = el.len * 8 - el.mLen - 1; + /** @type {number} */ + eMax = (1 << exponentLen) - 1; + /** @type {number} */ + eBias = eMax >> 1; + /** @type {number} */ + i = booleanIsBigEndian ? 0 : el.len - 1; + /** @type {number} */ + d = booleanIsBigEndian ? 1 : -1; + bool_s = octet_array_a[offset_p + i]; + /** @type {number} */ + i = i + d; + /** @type {number} */ + nBits = -7; + /** @type {number} */ + exponent = bool_s & (1 << -nBits) - 1; + /** @type {number} */ + bool_s = bool_s >> -nBits; + /** @type {number} */ + nBits = nBits + exponentLen; + for (; nBits > 0; exponent = exponent * 256 + octet_array_a[offset_p + i], i = i + d, nBits = nBits - 8) { + } + /** @type {number} */ + mantissa = exponent & (1 << -nBits) - 1; + /** @type {number} */ + exponent = exponent >> -nBits; + nBits = nBits + mantissaLen; + for (; nBits > 0; mantissa = mantissa * 256 + octet_array_a[offset_p + i], i = i + d, nBits = nBits - 8) { + } + switch(exponent) { + case 0: + /** @type {number} */ + // Zero, or denormalized number + exponent = 1 - eBias; + break; + case eMax: + // NaN, or +/-Infinity + return mantissa ? NaN : (bool_s ? -1 : 1) * Infinity; + default: + // Normalized number + mantissa = mantissa + Math.pow(2, mantissaLen); + /** @type {number} */ + exponent = exponent - eBias; + break; + } + return (bool_s ? -1 : 1) * mantissa * Math.pow(2, exponent - mantissaLen); + }; + /** + * @param {!Array} octet_array_a + * @param {number} offset_p + * @param {number} v + * @return {undefined} + */ + m._En754 = function(octet_array_a, offset_p, v) { + if (DEBUG) console.log("zzz_10"); + var bool_s; + var exponent; + var mantissa; + var i; + var d; + var c; + var mantissaLen; + var exponentLen; + var eBias; + var eMax; + mantissaLen = el.mLen; + /** @type {number} */ + exponentLen = el.len * 8 - el.mLen - 1; + /** @type {number} */ + eMax = (1 << exponentLen) - 1; + /** @type {number} */ + eBias = eMax >> 1; + /** @type {number} */ + bool_s = v < 0 ? 1 : 0; + /** @type {number} */ + v = Math.abs(v); + if (isNaN(v) || v == Infinity) { + /** @type {number} */ + mantissa = isNaN(v) ? 1 : 0; + /** @type {number} */ + exponent = eMax; + } else { + /** @type {number} */ + exponent = Math.floor(Math.log(v) / Math.LN2);// Calculate log2 of the value + if (v * (c = Math.pow(2, -exponent)) < 1) { // Math.log() isn't 100% reliable + exponent--; + /** @type {number} */ + c = c * 2; + } + // Round by adding 1/2 the significand's LSD + if (exponent + eBias >= 1) { + /** @type {number} */ + v = v + el.rt / c; // Normalized: mLen significand digits + } else { + /** @type {number} */ + v = v + el.rt * Math.pow(2, 1 - eBias);// Denormalized: <= mLen significand digits + } + if (v * c >= 2) { + exponent++; + /** @type {number} */ + c = c / 2; // Rounding can increment the exponent + } + if (exponent + eBias >= eMax) { + // Overflow + /** @type {number} */ + mantissa = 0; + /** @type {number} */ + exponent = eMax; + } else { + if (exponent + eBias >= 1) { + // Normalized - term order matters, as Math.pow(2, 52-e) and v*Math.pow(2, 52) can overflow + /** @type {number} */ + mantissa = (v * c - 1) * Math.pow(2, mantissaLen); + /** @type {number} */ + exponent = exponent + eBias; + } else { + // Denormalized - also catches the '0' case, somewhat by chance + /** @type {number} */ + mantissa = v * Math.pow(2, eBias - 1) * Math.pow(2, mantissaLen); + /** @type {number} */ + exponent = 0; + } + } + } + /** @type {number} */ + i = booleanIsBigEndian ? el.len - 1 : 0; + /** @type {number} */ + d = booleanIsBigEndian ? -1 : 1; + for (; mantissaLen >= 8; octet_array_a[offset_p + i] = mantissa & 255, i = i + d, mantissa = mantissa / 256, mantissaLen = mantissaLen - 8) { + } + /** @type {number} */ + exponent = exponent << mantissaLen | mantissa; + exponentLen = exponentLen + mantissaLen; + for (; exponentLen > 0; octet_array_a[offset_p + i] = exponent & 255, i = i + d, exponent = exponent / 256, exponentLen = exponentLen - 8) { + } + octet_array_a[offset_p + i - d] |= bool_s * 128; + }; + + + /** + * @param {!Object} octet_array_a + * @param {number} offset_p + * @return {?} + */ + // Convert int64 to array with 3 elements: [lowBits, highBits, unsignedFlag] + // '>>>' trick to convert signed 32bit int to unsigned int (because << always results in a signed 32bit int) + m._DeInt64 = function(octet_array_a, offset_p) { + if (DEBUG) console.log("zzz_11"); + /** @type {number} */ + var lsb = booleanIsBigEndian ? 0 : 7; + /** @type {number} */ + var nsb = booleanIsBigEndian ? 1 : -1; + /** @type {number} */ + var stop = lsb + nsb * 8; + /** @type {!Array} */ + var nextIdLookup = [0, 0, !el.bSigned]; + var i; + var f; + var indexLookupKey; + /** @type {number} */ + i = lsb; + /** @type {number} */ + indexLookupKey = 1; + /** @type {number} */ + f = 0; + for (; i != stop; nextIdLookup[indexLookupKey] = (nextIdLookup[indexLookupKey] << 8 >>> 0) + octet_array_a[offset_p + i], i = i + nsb, f++, indexLookupKey = f < 4 ? 1 : 0) { + + if ( DEBUG ) console.log("jsPacking int64:"+octet_array_a[offset_p + i]); + + } + return nextIdLookup; + }; + /** + * @param {!Array} octet_array_a + * @param {number} offset_p + * @param {!Object} v + * @return {undefined} + */ + m._EnInt64 = function(octet_array_a, offset_p, v) { + + if (v.length != 2) { //todo put this error back + console.log("ERROR ERROR: jspack needs an array of at least length TWO to pack an int64 "+v+' len:'+v.length); + } +// if (DEBUG) console.log("zzz_12 v:"+v); + /** @type {number} */ + var lsb = booleanIsBigEndian ? 0 : 7; + /** @type {number} */ + var nsb = booleanIsBigEndian ? 1 : -1; + /** @type {number} */ + var stop = lsb + nsb * 8; + var i; + var f; + var j; + var shift; + /** @type {number} */ + i = lsb; + /** @type {number} */ + j = 1; + /** @type {number} */ + f = 0; + /** @type {number} */ + shift = 24; + + for (; i != stop; octet_array_a[offset_p + i] = v[j] >> shift & 255, i = i + nsb, f++, j = f < 4 ? 1 : 0, shift = 24 - 8 * (f % 4)) { + var x = v[j] >> shift & 255 ; + var vj = v[j]; + + if ( DEBUG ) console.log('js qqqq vj:'+vj+' j:'+j+' x:'+x+' a:'+octet_array_a+' i:'+i+" offset_p:"+offset_p+" v:"+v); + } + }; + + + + // Class data + /** @type {string} */ + m._sPattern = "(\\d+)?([AxcbBhHsfdiIlLqQ])"; + + m._lenLut = {'A':1, 'x':1, 'c':1, 'b':1, 'B':1, 'h':2, 'H':2, 's':1, 'f':4, 'd':8, 'i':4, 'I':4, 'l':4, 'L':4, 'q':8, 'Q':8}; + + m._elLookUpTable = { 'A': {en:m._EnArray, de:m._DeArray}, + 's': {en:m._EnString, de:m._DeString}, + 'c': {en:m._EnChar, de:m._DeChar}, + 'b': {en:m._EnInt, de:m._DeInt, len:1, bSigned:true, min:-Math.pow(2, 7), max:Math.pow(2, 7)-1}, + 'B': {en:m._EnInt, de:m._DeInt, len:1, bSigned:false, min:0, max:Math.pow(2, 8)-1}, + 'h': {en:m._EnInt, de:m._DeInt, len:2, bSigned:true, min:-Math.pow(2, 15), max:Math.pow(2, 15)-1}, + 'H': {en:m._EnInt, de:m._DeInt, len:2, bSigned:false, min:0, max:Math.pow(2, 16)-1}, + 'i': {en:m._EnInt, de:m._DeInt, len:4, bSigned:true, min:-Math.pow(2, 31), max:Math.pow(2, 31)-1}, + 'I': {en:m._EnInt, de:m._DeInt, len:4, bSigned:false, min:0, max:Math.pow(2, 32)-1}, + 'l': {en:m._EnInt, de:m._DeInt, len:4, bSigned:true, min:-Math.pow(2, 31), max:Math.pow(2, 31)-1}, + 'L': {en:m._EnInt, de:m._DeInt, len:4, bSigned:false, min:0, max:Math.pow(2, 32)-1}, + 'f': {en:m._En754, de:m._De754, len:4, mLen:23, rt:Math.pow(2, -24)-Math.pow(2, -77)}, + 'd': {en:m._En754, de:m._De754, len:8, mLen:52, rt:0}, + 'q': {en:m._EnInt64, de:m._DeInt64, bSigned:true, len:8 }, // 64bit fields need 8 bytes.. + 'Q': {en:m._EnInt64, de:m._DeInt64, bSigned:false, len:8 }}; // quirk of longs is they come in with a length of 2 in an array + + + /** + * @param {number} num_elements_n + * @param {number} size_s + * @param {!Object} octet_array_a + * @param {number} offset_p + * @return {?} + */ + // Unpack a series of n elements of size s from array a at offset p with fxn + m._UnpackSeries = function(num_elements_n, size_s, octet_array_a, offset_p) { + if (DEBUG) console.log("zzz_13"); + var fxn = el.de; + /** @type {!Array} */ + var rv = []; + /** @type {number} */ + var o = 0; + for (; o < num_elements_n; rv.push(fxn(octet_array_a, offset_p + o * size_s)), o++) { + } + return rv; + }; + /** + * @param {number} num_elements_n + * @param {number} size_s + * @param {!Array} to_octet_array_a + * @param {number} array_a_offset_p + * @param {(Array|NodeList|null)} from_array_v + * @param {number} array_v_offset_i + * @return {undefined} + */ + // Pack a series of n elements of size s from array v at offset i to array a at offset p with fxn + + m._PackSeries = function(num_elements_n, size_s, to_octet_array_a, array_a_offset_p, from_array_v, array_v_offset_i) { + if (DEBUG) console.log("pack-series: "); + + + if ( DEBUG ) console.log('js before 0:'+0+' num_elements_n:'+num_elements_n+' size_s:'+size_s+' to_a:'+to_octet_array_a+' i:'+array_v_offset_i+" offset_p:"+array_a_offset_p+" v:"+from_array_v); + var fxn = el.en; + /** @type {number} */ + var o = 0; + for (; o < num_elements_n; o++) { + //if (DEBUG) console.log("14 called fxn with o:"+o); + var z = from_array_v[array_v_offset_i + o]; + var to = JSON.stringify(z); + var too = JSON.stringify(from_array_v); + if (DEBUG) console.log('js pre-ffff z:'+z+' to:'+to+' too:'+too+''); + // handle flattened arrays - non-array things don't have a .length + try { + if (z.length == undefined ) { + //from_array_v = [ from_array_v ] ; + if (DEBUG) console.log('Z FIX'); + }} catch (e){} + var z = from_array_v[array_v_offset_i + o]; + var to = JSON.stringify(z); + var too = JSON.stringify(from_array_v); + + // if we only have one thing to back and its got an 8 byte target len ( it's a 64bit long), and length of source array is 2 ( low and high bits ) + // we treat it as a singular thing... we use this for Q type, which gets passed in as [lowBits, hightBits] + if (( num_elements_n == 1 ) && (size_s == 8) && (from_array_v.length == 2) ) { + z = from_array_v; + if (DEBUG) console.log("js handling Q 64bit array"); + } + + + if (DEBUG) console.log('js partial z:'+z+' to:'+to+' too:'+too+' num_elements_n:'+num_elements_n+' size_s:'+size_s+' to_a:'+to_octet_array_a+' v_offset_i:'+array_v_offset_i+" a_offset_p:"+array_a_offset_p+" from_v:"+from_array_v); + + fxn(to_octet_array_a, array_a_offset_p + o * size_s, z); + + } + if (DEBUG) console.log('js after to_a:'+to_octet_array_a); + }; + + + /** + * @param {string} fmt + * @param {!Object} octet_array_a + * @param {number} offset_p + * @return {?} + */ + // Unpack the octet array a, beginning at offset p, according to the fmt string + m.Unpack = function(fmt, octet_array_a, offset_p) { + if (DEBUG) console.log("zzz_15"); + /** @type {boolean} */ + // Set the private bBE flag based on the format string - assume big-endianness + booleanIsBigEndian = fmt.charAt(0) != "<"; + /** @type {number} */ + offset_p = offset_p ? offset_p : 0; + /** @type {!RegExp} */ + var re = new RegExp(this._sPattern, "g"); + var re_match; + var repeat_count_n; + var element_size_s; + /** @type {!Array} */ + var rv = []; + + //loop over chars in the format string with regex due to optional digits + for (; re_match = re.exec(fmt);) { + /** @type {number} */ + repeat_count_n = re_match[1] == undefined || re_match[1] == "" ? 1 : parseInt(re_match[1]); + element_size_s = this._lenLut[re_match[2]]; + if (offset_p + repeat_count_n * element_size_s > octet_array_a.length) { + return undefined; + } + switch(re_match[2]) { + case "A": + case "s": + rv.push(this._elLookUpTable[re_match[2]].de(octet_array_a, offset_p, repeat_count_n)); + break; + case "c": + case "b": + case "B": + case "h": + case "H": + case "i": + case "I": + case "l": + case "L": + case "f": + case "d": + case "q": + case "Q": + el = this._elLookUpTable[re_match[2]]; + + //rv.push(this._UnpackSeries(repeat_count_n, element_size_s, octet_array_a, offset_p)); + + // unpack arrays to an actual array type within the field array result: + // https://github.com/AndreasAntener/node-jspack/commit/4f16680101303a6b4a1b0deba8cf7d20fc68213e + if (repeat_count_n > 1) { + // Field is array, unpack into separate array and push as such + var arr = []; + arr.push(this._UnpackSeries(repeat_count_n, element_size_s, octet_array_a, offset_p)); + rv.push(arr); + } else { + rv.push(this._UnpackSeries(repeat_count_n, element_size_s, octet_array_a, offset_p)); + } + + break; + } + /** @type {number} */ + offset_p = offset_p + repeat_count_n * element_size_s; + } + return Array.prototype.concat.apply([], rv); + }; + + + // cross check the list of input data matches the size of bytes we'll be assembling + // this is a slightly tweaked implementation of the previous 'PackTo' commented out below. + // it has a more-consistent approach to input and output arrays, paying particular attention to Q,q, long, etc + m.WouldPack = function(fmt, octet_array_a, offset_p, values) { + //if (DEBUG) console.log("zzz_16 fmt:"+JSON.stringify(fmt)+" values:"+JSON.stringify(values)); + // @type {boolean} / + // Set the private bBE flag based on the format string - assume big-endianness + booleanIsBigEndian = fmt.charAt(0) != "<"; + // @type {!RegExp} / + var re = new RegExp(this._sPattern, "g"); + + var m; + var n; + var s; + var values_i = 0; // current index into the values[] + + var j; + for (; m = re.exec(fmt);) { + + // leading optional prefix num or 1 + n = m[1] == undefined || m[1] == "" ? 1 : parseInt(m[1]); + + s = this._lenLut[m[2]]; + + + if (DEBUG) console.log("character: "+m[2]+" how many(n)?: "+n); + el = this._elLookUpTable[m[2]]; + + //if (DEBUG) console.log("using lookup table:"+JSON.stringify(el)); + var bytes_consumed_per_element = el["len"]; + bytes_consumed_per_element = bytes_consumed_per_element == undefined ? 1 : bytes_consumed_per_element ; // undefined means 1 + if (DEBUG) console.log("bytes_consumed_per_element:"+JSON.stringify(bytes_consumed_per_element)); + if (DEBUG) console.log("current_values_idx:"+JSON.stringify(values_i) +" values:"+JSON.stringify(values[values_i]) ) ; + + + // do per-case behaviours 'A' , 's' and 'x' are special, everything else gets the same + switch(m[2]) { + //------------------------------------------ + case "A": + case "s": + if (values_i + 1 > values.length) { + console.log("JSPACK-ERROR: values_i + 1 > values.length values_i:"+values_i+" values.length:"+values.length); + //return false; + } + if (DEBUG) console.log("all values:"+JSON.stringify(values)); + this._elLookUpTable[m[2]].en(octet_array_a, offset_p, n, values[values_i]); + // @type {number} / + values_i = values_i + 1; + break; + //------------------------------------------ + case "x": + // @type {number} / + j = 0; + for (; j < n; j++) { + // @type {number} / + octet_array_a[offset_p + j] = 0; + } + break; + //------------------------------------------ + // everything else + default: + + // if n > 1 , ie it's multiple occurrences of a 'thing' + if (n > 1 ) { + + // if we were handed an array at this idx, we need the array to be the same length as n + if (Array.isArray(values[values_i])) { + + // Value series is array, iterate through that, only increment by 1 + if ((values_i + 1) > values.length) { + if (DEBUG) console.log("JSPACK-ERROR: value series is array but (values_i + 1) > values.length. i:"+values_i+" values.length:"+values.length); + //return false; + } + if (DEBUG) console.log("(dst IS array) (source IS array)"); + this._PackSeries(n, s, octet_array_a, offset_p, values[values_i], 0); + values_i += 1; + } + else { + if (DEBUG) console.log("ERROR: (dst IS array) (source is not array)"); + } + } + + // if n == 1, it just one of a thing + if (n == 1 ) { + + // type Q can have the source as an array when there is only 1 of them. + if (Array.isArray(values[values_i]) ) { + + if (( m[2] == 'Q' ) || ( m[2] == 'q' ) ) { + this._PackSeries(n, s, octet_array_a, offset_p, values[values_i], 0); + values_i += 1; + } + if (DEBUG) console.log("(dst is not array) (source IS array)"); + + } else { + if ((values_i + n ) > values.length) { + if (DEBUG) console.log("JSPACK-ERROR: value series NOT array but (values_i + n ) > values.length. i:"+values_i+" n:"+n+" values.length:"+values.length+" values:"+JSON.stringify(values)); + //return false; + } + if (DEBUG) console.log("(dst is not array) (source is not array)"); + this._PackSeries(n, s, octet_array_a, offset_p, values, values_i); + values_i += n; + } + } + + if (DEBUG) console.log(""); + break; + //------------------------------------------ + } + + offset_p = offset_p + n * s; + + } + if (DEBUG) console.log("wouldpack completed, result array_a is:"+JSON.stringify(octet_array_a)); + return octet_array_a + } + + + + /** + * @param {string} fmt + * @param {!Array} octet_array_a + * @param {number} offset_p + * @param {!NodeList} values + * @return {?} + */ +/* + // Pack the supplied values into the octet array a, beginning at offset p, according to the fmt string + m.PackTo = function(fmt, octet_array_a, offset_p, values) { + if (DEBUG) console.log("zzz_16 fmt:"+JSON.stringify(fmt)+" values:"+JSON.stringify(values)); + // @type {boolean} / + // Set the private bBE flag based on the format string - assume big-endianness + booleanIsBigEndian = fmt.charAt(0) != "<"; + // @type {!RegExp} / + var re = new RegExp(this._sPattern, "g"); + var m; + var n; + var s; + // @type {number} / + var i = 0; + var j; + for (; m = re.exec(fmt);) { + // @type {number} / + n = m[1] == undefined || m[1] == "" ? 1 : parseInt(m[1]); + s = this._lenLut[m[2]]; + if (offset_p + n * s > octet_array_a.length) { + console.log("JSPACK-ERROR: offset_p + n * s > octet_array_a.length offset_p:"+offset_p+" n:"+n+" s:"+s+" octet_array_a.length:"+octet_array_a.length+" octet_array_a:"+JSON.stringify(octet_array_a)); + return false; + } + if (DEBUG) console.log("\n---------------------------------------------\n"); + if (DEBUG) console.log("handling format specifier:"+m[2]+" how many:"+n); + switch(m[2]) { + case "A": + case "s": + if (i + 1 > values.length) { + console.log("JSPACK-ERROR: i + 1 > values.length i:"+i+" values.length:"+values.length); + return false; + } + if (DEBUG) console.log("zzz_16A values:"+JSON.stringify(values)); + this._elLookUpTable[m[2]].en(octet_array_a, offset_p, n, values[i]); + // @type {number} / + i = i + 1; + break; + case "c": + case "b": + case "B": + case "h": + case "H": + case "i": + case "I": + case "l": + case "L": + case "f": + case "d": + case "q": + case "Q": + if (DEBUG) console.log("16 blerg"); + el = this._elLookUpTable[m[2]]; + if (DEBUG) console.log("using lookup table:"+JSON.stringify(el)); + //if (i + n > values.length) { return false; } + //this._PackSeries(n, s, octet_array_a, offset_p, values, i); + //i = i + n; + //added support for packing value series when they are supplied as arrays within the values array + // https://github.com/AndreasAntener/node-jspack/commit/8de80d20aa06dea15527b3073c6c8631abda0f17 + if (n > 1 && Array.isArray(values[i])) { + // Value series is array, iterate through that, only increment by 1 + if ((i + 1) > values.length) { + console.log("JSPACK-ERROR: value series is array but (i + 1) > values.length. i:"+i+" values.length:"+values.length); + return false; + } + if (DEBUG) console.log("zzz_16 option 1 (source is array)"); + this._PackSeries(n, s, octet_array_a, offset_p, values[i], 0); + i += 1; + } else { + if ((i + n) > values.length) { + console.log("JSPACK-ERROR: value series NOT array but (i + n) > values.length. i:"+i+" n:"+n+" values.length:"+values.length+" values:"+JSON.stringify(values)); + //return false; + } + if (DEBUG) console.log("zzz_16 option 2 (source is not array)"); + this._PackSeries(n, s, octet_array_a, offset_p, values, i); + i += n; + } + + if (DEBUG) console.log("end case"); + break; + case "x": + // @type {number} / + j = 0; + for (; j < n; j++) { + // @type {number} / + octet_array_a[offset_p + j] = 0; + } + break; + } + // @type {number} / + offset_p = offset_p + n * s; + } + console.log("pack completed, result array_a is:"+JSON.stringify(octet_array_a)); + return octet_array_a; + }; + */ + + /** + * @param {string} fmt + * @param {(Node|NodeList|null|string)} values + * @return {?} + */ + // Pack the supplied values into a new octet array, according to the fmt string + m.Pack = function(fmt, values) { + if (DEBUG) console.log("\n\n------------------------------------------------------------------------------------------------------------\n\n"); + if (DEBUG) console.log("initial unpacked values:"+JSON.stringify(values)); + if (DEBUG) console.log("initial format string:"+JSON.stringify(fmt)); + if (DEBUG) console.log("\n\nwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwww\n\n"); + return this.WouldPack(fmt, new Array(this.CalcLength(fmt)), 0, values); + //if (DEBUG) console.log("\n\nmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm\n\n"); + // return this.PackTo(fmt, new Array(this.CalcLength(fmt)), 0, values); + }; + + /** + * @param {string} fmt + * @param {(Node|NodeList|null|string)} values + * @return {?} + */ + // Pack the supplied values into a new octet array, according to the fmt string + m.oldPack = function(fmt, values) { + if (DEBUG) console.log("\n\n------------------------------------------------------------------------------------------------------------\n\n"); + if (DEBUG) console.log("initial unpacked values:"+JSON.stringify(values)); + if (DEBUG) console.log("initial format string:"+JSON.stringify(fmt)); + return this.PackTo(fmt, new Array(this.CalcLength(fmt)), 0, values); + }; + + /** + * @param {string} fmt + * @return {?} + */ + // Determine the number of bytes represented by the format string + m.CalcLength = function(fmt) { + + /** @type {!RegExp} */ + var re = new RegExp(this._sPattern, "g"); + var m; + /** @type {number} */ + var value = 0; + for (; m = re.exec(fmt);) { + /** @type {number} */ + value = value + (m[1] == undefined || m[1] == "" ? 1 : parseInt(m[1])) * this._lenLut[m[2]]; + } + if (DEBUG) console.log("number of bytes in format string?: "+value+"\n"); + return value; + }; +} + +export default JSPack + diff --git a/modules/MAVLink/local_modules/jspack/package.json b/modules/MAVLink/local_modules/jspack/package.json new file mode 100644 index 00000000..f9c36896 --- /dev/null +++ b/modules/MAVLink/local_modules/jspack/package.json @@ -0,0 +1,64 @@ +{ + "_from": "jspack@0.0.4", + "_id": "jspack@0.0.4", + "_inBundle": false, + "_integrity": "sha1-Mt01x/3LPjRWwY+7fvntC8YjgXc=", + "_location": "/jspack", + "_phantomChildren": {}, + "_requested": { + "type": "version", + "registry": false, + "raw": "jspack@0.0.4", + "name": "jspack", + "escapedName": "jspack", + "rawSpec": "0.0.4", + "saveSpec": null, + "fetchSpec": "0.0.4" + }, + "_requiredBy": [ + "#USER", + "/" + ], + "_resolved": "local_modules/jspack", + "_shasum": "32dd35c7fdcb3e3456c18fbb7ef9ed0bc6238177", + "_spec": "jspack@0.0.4", + "_where": "/home/buzz/GCS/mavlink/pymavlink/generator/javascript", + "author": { + "name": "https://github.com/pgriess" + }, + "bugs": { + "url": "https://github.com/birchroad/node-jspack/issues" + }, + "bundleDependencies": false, + "deprecated": false, + "description": "JavaScript library to pack primitives to octet arrays, including int64 support, packaged for NodeJS.", + "devDependencies": { + "long": "", + "mocha": "", + "should": "", + "sinon": "" + }, + "homepage": "https://github.com/birchroad/node-jspack", + "main": "./jspack.js", + "maintainers": [ + { + "name": "Peter Magnusson", + "email": "peter@birchroad.net", + "url": "http://github.com/birchroad/node-jspack" + }, + { + "name": "Andreas Antener", + "url": "https://github.com/AndreasAntener/node-jspack" + } + ], + "name": "jspack", + "repository": { + "type": "git", + "url": "https://github.com/birchroad/node-jspack.git" + }, + "scripts": { + "pretest": "npm install", + "test": "mocha test" + }, + "version": "0.0.4" +} diff --git a/modules/MAVLink/local_modules/jspack/test/int64.js b/modules/MAVLink/local_modules/jspack/test/int64.js new file mode 100644 index 00000000..fc72eb49 --- /dev/null +++ b/modules/MAVLink/local_modules/jspack/test/int64.js @@ -0,0 +1,456 @@ +// This file is MODIFIED from the original, by buzz 2020, please see README.md in the upper level folder for more details. +var should = require('should'); +var jspack = require('../jspack.js').jspack; +var Long = require('long'); + +describe('Test long integration (examples):', function() { + + // Demonstrating the use together with Long.js (https://github.com/dcodeIO/Long.js) + // + // Packing a long requires the input of a 2 part array containing the [low, high] bits + // of the specific long value. + // Unpacking a long results in a 3 part array containing [low, high, unsigned] bits and flag. + // The decoded value can be applied directly to Long.fromBits() + // + // Test number u 228290380562207 (BE: 0x00, 0x00, 0xcf, 0xa0, 0xff, 0x09, 0xff, 0x1f) + // (LE: 0x1f, 0xff, 0x09, 0xff, 0xa0, 0xcf, 0x00, 0x00) + // Test number s -228290380562207 (BE: 0xff, 0xff, 0x30, 0x5f, 0x00, 0xf6, 0x00, 0xe1) + // (LE: 0xe1, 0x00, 0xf6, 0x00, 0x5f, 0x30, 0xff, 0xff) + + it('pack Q', function() { + var buf = jspack.Pack('>Q', [[0xffe1ffff, 0xffa0]]); + buf.should.be.eql([0x00, 0x00, 0xff, 0xa0, 0xff, 0xe1, 0xff, 0xff]); + }); + + it('unpack Q', function() { + var buf = jspack.Unpack('>Q', [0x00, 0x00, 0xff, 0xa0, 0xff, 0xe1, 0xff, 0xff]); + buf.length.should.be.eql(1); + buf[0].length.should.be.eql(3); + buf[0][0].should.be.eql(0xffe1ffff); + buf[0][1].should.be.eql(0xffa0); + buf[0][2].should.be.true; + }); + + // Test lower-case q as well. This only test the matching of the character and the unsigned bit, + // the parsing is the same as for upper-case Q (since we don't actually convert to a number). + it('pack >q (signed)', function() { + var buf = jspack.Pack('>q', [[0xffe1ffff, 0xffa0]]); + buf.should.be.eql([0x00, 0x00, 0xff, 0xa0, 0xff, 0xe1, 0xff, 0xff]); + }); + + it('unpack >> 0).toString(2); + y = ("00000000000000000000000000000000" + x).slice(-32) + y1 = y.substring(0,8); + y2 = y.substring(8,16); + y3 = y.substring(16,24); + y4 = y.substring(24,32); + return [y,y1,y2,y3,y4]; +} +function dec2bin_ws(dec) { + var str = dec2bin(dec); + var bb = str.slice(1); //1-4 skipping zero + var bbj = bb.join(' '); + return bbj; +} + +describe('ASCII Boundary tests:', function() { + + it('pack <4s correctly over the ascii 127->128->129 boundary', function() { // should work in range 0-255 if u use 'binary' encoding + + this.format = '<4s'; + + this.ascii_bytes = new Buffer.from([ 126, 127, 128, 129]).toString('binary'); // 'binary' encoding is important here, as without it values above 128 are treated as unicode. + var buf = jspack.Pack(this.format, [ this.ascii_bytes]); + body = [ 0x7e, 0x7f, 0x80, 0x81]; // expected result + buf.should.be.eql(body); + + }); + + it('long Q buzz', function() { // should work in range 0-255 if u use 'binary' encoding + +//from aoa_ssa + + this.format = '> 8) & 0xFF), this.msgId>>16]; + + this.msgId = 130; + + var v1 = ((this.msgId & 0xFF) << 8) | ((this.msgId >> 8) & 0xFF); + var v2 = this.msgId>>16; + + v1.should.be.eql(33280); + v2.should.be.eql(0); + + var orderedfields = [253,13,0,0,40,11,10,33280,0]; + + console.log("------------------------------------------------------------------------\nmavheader:"+JSON.stringify(orderedfields)); + var hdr = jspack.Pack('BBBBBBBHB',orderedfields); + + buf = [0xfd, 0x0d, 0x00, 0x00, 0x28, 0x0b, 0x0a, 0x82, 0x00, 0x00]; + + buf.should.be.eql(hdr); + }); + +}); + +describe('Q Boundary tests:', function() { + + it('unpack >Q full', function() { + var buf = jspack.Unpack('>Q', [0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff]); + buf.length.should.be.eql(1); + buf[0].length.should.be.eql(3); + buf[0][0].should.be.eql(0xffffffff); + buf[0][1].should.be.eql(0xffffffff); + buf[0][2].should.be.true; + }); + + it('pack >Q full', function() { + var buf = jspack.Pack('>Q', [[0xffffffff, 0xffffffff]]); + buf.should.be.eql([0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff]); + }); + + it('unpack Q zero', function() { + var buf = jspack.Unpack('>Q', [0, 0, 0, 0, 0, 0, 0, 0]); + buf.length.should.be.eql(1); + buf[0].length.should.be.eql(3); + buf[0][0].should.be.eql(0); + buf[0][1].should.be.eql(0); + buf[0][2].should.be.true; + }); + + it('pack >Q zero', function() { + var buf = jspack.Pack('>Q', [[0, 0]]); + buf.should.be.eql([0, 0, 0, 0, 0, 0, 0, 0]); + }); + + it('unpack Q one', function() { + var buf = jspack.Unpack('>Q', [1, 1, 1, 1, 1, 1, 1, 1]); + buf.length.should.be.eql(1); + buf[0].length.should.be.eql(3); + buf[0][0].should.be.eql(0x01010101); + buf[0][1].should.be.eql(0x01010101); + buf[0][2].should.be.true; + }); + + it('pack >Q one', function() { + var buf = jspack.Pack('>Q', [[0x01010101, 0x01010101]]); + buf.should.be.eql([1, 1, 1, 1, 1, 1, 1, 1]); + }); + + it('unpack Q 0xfe', function() { + var buf = jspack.Unpack('>Q', [0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe]); + buf.length.should.be.eql(1); + buf[0].length.should.be.eql(3); + buf[0][0].should.be.eql(0xfefefefe); + buf[0][1].should.be.eql(0xfefefefe); + buf[0][2].should.be.true; + }); + + it('pack >Q 0xfe', function() { + var buf = jspack.Pack('>Q', [[0xfefefefe, 0xfefefefe]]); + buf.should.be.eql([0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe]); + }); + + it('unpack c.charCodeAt(0))); + } + + // Ensure we have at least the header + if (payload.length < this.HDR_LEN) { + console.error(`FTP: Payload too short (${payload.length} bytes)`); + return null; + } + + const view = new DataView(payload.buffer, payload.byteOffset, payload.byteLength); + + const op = { + seq: view.getUint16(0, true), + session: view.getUint8(2), + opcode: view.getUint8(3), + size: view.getUint8(4), + req_opcode: view.getUint8(5), + burst_complete: view.getUint8(6), + offset: view.getUint32(8, true), + payload: null + }; + + // Extract payload if present and size > 0 + if (op.size > 0 && payload.length > this.HDR_LEN) { + const payloadStart = this.HDR_LEN; + const payloadLength = Math.min(op.size, payload.length - this.HDR_LEN); + op.payload = new Uint8Array(payload.buffer, payload.byteOffset + payloadStart, payloadLength); + } + + return op; + } + + // Send FTP operation + sendOp(opcode, size, req_opcode, burst_complete, offset, payload) { + const packedOp = this.packOp(this.seq, this.session, opcode, size, req_opcode, burst_complete, offset, payload); + + // Send via FILE_TRANSFER_PROTOCOL message + const msg = new mavlink20.messages.file_transfer_protocol( + 0, // network (0 for Mavlink 2.0) + this.targetSystem, + this.targetComponent, + Array.from(packedOp) + ); + + const pkt = msg.pack(this.MAVLink); + this.ws.send(Uint8Array.from(pkt)); + + this.lastOp = { opcode, size, req_opcode, burst_complete, offset, payload }; + + console.log(`FTP: Sent op ${opcode} (${this.getOpName(opcode)}) seq=${this.seq} session=${this.session} offset=${offset}`); + this.seq = (this.seq + 1) % 256; + } + + // Helper to get operation name for debugging + getOpName(opcode) { + for (let [name, code] of Object.entries(this.OP)) { + if (code === opcode) return name; + } + return `Unknown(${opcode})`; + } + + // Terminate current session + terminateSession() { + this.sendOp(this.OP.TerminateSession, 0, 0, 0, 0, null); + this.session = (this.session + 1) % 256; + this.currentFile = null; + this.fileBuffer = null; + this.readGaps = []; + this.reachedEOF = false; + console.log("FTP: Session terminated"); + } + + // Get file from vehicle + getFile(filename, callback) { + console.log(`FTP: Getting file ${filename}`); + + this.terminateSession(); + this.callback = callback; + this.currentFile = filename; + this.fileBuffer = new Uint8Array(0); + this.readGaps = []; + this.reachedEOF = false; + this.opStartTime = Date.now(); + + // Encode filename + const encoder = new TextEncoder(); + const filenameBytes = encoder.encode(filename); + + // Send OpenFileRO + this.sendOp(this.OP.OpenFileRO, filenameBytes.length, 0, 0, 0, filenameBytes); + } + + // Handle incoming FILE_TRANSFER_PROTOCOL message + handleMessage(m) { + if (m._name !== "FILE_TRANSFER_PROTOCOL") return; + if (m.target_system !== this.MAVLink.srcSystem || m.target_component !== this.MAVLink.srcComponent) return; + + // m.payload is already a Uint8Array, pass it directly + const op = this.parseOp(m.payload); + if (!op) { + console.error("FTP: Failed to parse operation"); + return; + } + + console.log(`FTP: Received ${this.getOpName(op.opcode)} for req=${this.getOpName(op.req_opcode)} size=${op.size} offset=${op.offset} seq=${op.seq}`); + + // Handle different response types + if (op.req_opcode === this.OP.OpenFileRO) { + this.handleOpenResponse(op); + } else if (op.req_opcode === this.OP.BurstReadFile) { + this.handleBurstReadResponse(op); + } else if (op.req_opcode === this.OP.ReadFile) { + this.handleReadResponse(op); + } else if (op.req_opcode === this.OP.TerminateSession) { + console.log("FTP: Session terminated ACK"); + } else { + console.log(`FTP: Unhandled response for ${this.getOpName(op.req_opcode)}`); + } + } + + // Handle OpenFileRO response + handleOpenResponse(op) { + if (op.opcode === this.OP.Ack) { + console.log("FTP: File opened, starting burst read"); + // Start burst read from offset 0 + this.sendOp(this.OP.BurstReadFile, this.burstSize, 0, 0, 0, null); + } else if (op.opcode === this.OP.Nack) { + console.error("FTP: Failed to open file - NACK received"); + if (this.callback) this.callback(null); + this.terminateSession(); + } else { + console.error(`FTP: Unexpected response to OpenFileRO: opcode ${op.opcode}`); + } + } + + // Handle BurstReadFile response + handleBurstReadResponse(op) { + if (op.opcode === this.OP.Ack && op.payload) { + // Expand buffer if needed + const newSize = op.offset + op.size; + if (newSize > this.fileBuffer.length) { + const newBuffer = new Uint8Array(newSize); + newBuffer.set(this.fileBuffer); + this.fileBuffer = newBuffer; + } + + // Write data at offset + this.fileBuffer.set(op.payload, op.offset); + + console.log(`FTP: Read ${op.size} bytes at offset ${op.offset}`); + + // Check if we need to continue + if (op.burst_complete) { + if (op.size > 0 && op.size < this.burstSize) { + // EOF reached + this.reachedEOF = true; + this.finishTransfer(); + } else { + // Continue reading + const nextOffset = op.offset + op.size; + this.sendOp(this.OP.BurstReadFile, this.burstSize, 0, 0, nextOffset, null); + } + } + } else if (op.opcode === this.OP.Nack) { + const errorCode = op.payload ? op.payload[0] : 0; + if (errorCode === this.ERR.EndOfFile || errorCode === 0) { + console.log("FTP: EOF reached"); + this.reachedEOF = true; + this.finishTransfer(); + } else { + console.error(`FTP: Read failed with error ${errorCode}`); + if (this.callback) this.callback(null); + this.terminateSession(); + } + } + } + + // Handle ReadFile response (for gap filling) + handleReadResponse(op) { + if (op.opcode === this.OP.Ack && op.payload) { + // Fill gap + this.fileBuffer.set(op.payload, op.offset); + + // Remove from gaps list + this.readGaps = this.readGaps.filter(g => g.offset !== op.offset); + + console.log(`FTP: Filled gap at ${op.offset}, ${this.readGaps.length} gaps remaining`); + + if (this.readGaps.length === 0 && this.reachedEOF) { + this.finishTransfer(); + } + } + } + + // Finish file transfer + finishTransfer() { + if (!this.fileBuffer) { + return; + } + const dt = (Date.now() - this.opStartTime) / 1000; + const size = this.fileBuffer.length; + const rate = (size / dt) / 1024; + + console.log(`FTP: Transfer complete - ${size} bytes in ${dt.toFixed(2)}s (${rate.toFixed(1)} KB/s)`); + + if (this.callback) { + this.callback(this.fileBuffer); + } + + this.terminateSession(); + } +} + +// Fence data parser +class FenceParser { + constructor() { + } + + parseMissionItems(data) { + var header = data.buffer.slice(0,10); + var hdr = jspack.Unpack(" { + 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(){}; + +// Implement the CRC-16/MCRF4XX function (present in the Python version through the mavutil.py package) +mavlink20.x25Crc = function(buffer, crcIN) { + + var bytes = buffer; + var crcOUT = crcIN === undefined ? 0xffff : crcIN; + bytes.forEach(function(e) { + var tmp = e ^ (crcOUT & 0xff); + tmp = (tmp ^ (tmp << 4)) & 0xff; + crcOUT = (crcOUT >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4); + crcOUT = crcOUT & 0xffff; + }); + return crcOUT; + +} + +mavlink20.WIRE_PROTOCOL_VERSION = "2.0"; +mavlink20.PROTOCOL_MARKER_V1 = 0xFE +mavlink20.PROTOCOL_MARKER_V2 = 0xFD +mavlink20.HEADER_LEN_V1 = 6 +mavlink20.HEADER_LEN_V2 = 10 +mavlink20.HEADER_LEN = 10; + +mavlink20.MAVLINK_TYPE_CHAR = 0 +mavlink20.MAVLINK_TYPE_UINT8_T = 1 +mavlink20.MAVLINK_TYPE_INT8_T = 2 +mavlink20.MAVLINK_TYPE_UINT16_T = 3 +mavlink20.MAVLINK_TYPE_INT16_T = 4 +mavlink20.MAVLINK_TYPE_UINT32_T = 5 +mavlink20.MAVLINK_TYPE_INT32_T = 6 +mavlink20.MAVLINK_TYPE_UINT64_T = 7 +mavlink20.MAVLINK_TYPE_INT64_T = 8 +mavlink20.MAVLINK_TYPE_FLOAT = 9 +mavlink20.MAVLINK_TYPE_DOUBLE = 10 + +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) { + + this.mlen = ( typeof mlen === 'undefined' ) ? 0 : mlen; + this.seq = ( typeof seq === 'undefined' ) ? 0 : seq; + this.srcSystem = ( typeof srcSystem === 'undefined' ) ? 0 : srcSystem; + this.srcComponent = ( typeof srcComponent === 'undefined' ) ? 0 : srcComponent; + this.msgId = msgId + this.incompat_flags = incompat_flags + this.compat_flags = compat_flags + +} +mavlink20.header.prototype.pack = function() { + return jspack.Pack('BBBBBBBHB', [253, this.mlen, this.incompat_flags, this.compat_flags, this.seq, this.srcSystem, this.srcComponent, ((this.msgId & 0xFF) << 8) | ((this.msgId >> 8) & 0xFF), this.msgId>>16]); +} + +// Base class declaration: mavlink.message will be the parent class for each +// concrete implementation in mavlink.messages. +mavlink20.message = function() {}; + +// Convenience setter to facilitate turning the unpacked array of data into member properties +mavlink20.message.prototype.set = function(args,verbose) { + // inspect + this.fieldnames.forEach(function(e, i) { + var num = parseInt(i,10); + if (this.hasOwnProperty(e) && isNaN(num) ){ // asking for an attribute that's non-numeric is ok unless its already an attribute we have + if ( verbose >= 1) { console.log("WARNING, overwriting an existing property is DANGEROUS:"+e+" ==>"+i+"==>"+args[i]+" -> "+JSON.stringify(this)); } + } + }, this); + + // then modify + this.fieldnames.forEach(function(e, i) { + this[e] = args[i]; + }, this); +}; + +/* + 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; + } + + 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; +} + +// 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); + + 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)); + + // first add the linkid(1 byte) and timestamp(6 bytes) that start the signature + this._msgbuf = this._msgbuf.concat([mav.signing.link_id]) + this._msgbuf = this._msgbuf.concat(tsbuf); + + sig = mavlink20.create_signature(mav.signing.secret_key, this._msgbuf); + this._msgbuf = this._msgbuf.concat( ... sig ); + + mav.signing.timestamp += 1; +} + +// This pack function builds the header and produces a complete MAVLink message, +// including header and message CRC. +mavlink20.message.prototype.pack = function(mav, crc_extra, payload) { + + this._payload = payload; + var plen = this._payload.length; + //in MAVLink2 we can strip trailing zeros off payloads. This allows for simple + // variable length arrays and smaller packets + while ((plen > 1) && ( (this._payload[plen-1] == 0) || (this._payload[plen-1] == null) ) ) { + plen = plen - 1; + } + this._payload = this._payload.slice(0, plen); + // signing is our first incompat flag. + var incompat_flags = 0; + if (mav.signing.sign_outgoing){ + incompat_flags |= mavlink20.MAVLINK_IFLAG_SIGNED + } + // header + this._header = new mavlink20.header(this._id, this._payload.length, mav.seq, mav.srcSystem, mav.srcComponent, incompat_flags, 0,); + // payload + this._msgbuf = this._header.pack().concat(this._payload); + // crc - for now, assume always using crc_extra = True. TODO: check/fix this. + var crc = mavlink20.x25Crc(this._msgbuf.slice(1)); + crc = mavlink20.x25Crc([crc_extra], crc); + this._msgbuf = this._msgbuf.concat(jspack.Pack(' MAV. Also used to +return a point from MAV -> GCS. + + target_system : System ID. (uint8_t) + target_component : Component ID. (uint8_t) + idx : Point index (first point is 1, 0 is for return point). (uint8_t) + count : Total number of points (for sanity checking). (uint8_t) + lat : Latitude of point. (float) + lng : Longitude of point. (float) + +*/ + mavlink20.messages.fence_point = function( ...moreargs ) { + [ this.target_system , this.target_component , this.idx , this.count , this.lat , this.lng ] = moreargs; + + this._format = ' MAV. Also used to +return a point from MAV -> GCS. + + target_system : System ID. (uint8_t) + target_component : Component ID. (uint8_t) + idx : Point index (first point is 0). (uint8_t) + count : Total number of points (for sanity checking). (uint8_t) + lat : Latitude of point. (int32_t) + lng : Longitude of point. (int32_t) + alt : Transit / loiter altitude relative to home. (int16_t) + break_alt : Break altitude relative to home. (int16_t) + land_dir : Heading to aim for when landing. (uint16_t) + flags : Configuration flags. (uint8_t) + +*/ + mavlink20.messages.rally_point = function( ...moreargs ) { + [ this.target_system , this.target_component , this.idx , this.count , this.lat , this.lng , this.alt , this.break_alt , this.land_dir , this.flags ] = moreargs; + + this._format = ' value[float]. +This allows to send a parameter to any other component (such as the +GCS) without the need of previous knowledge of possible parameter +names. Thus the same GCS can store different parameters for different +autopilots. See also https://mavlink.io/en/services/parameter.html for +a full documentation of QGroundControl and IMU code. + + target_system : System ID (uint8_t) + target_component : Component ID (uint8_t) + param_id : Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string (char) + param_index : Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) (int16_t) + +*/ + mavlink20.messages.param_request_read = function( ...moreargs ) { + [ this.target_system , this.target_component , this.param_id , this.param_index ] = moreargs; + + this._format = ' 0 indicates the interval at which it is sent. (int32_t) + +*/ + mavlink20.messages.message_interval = function( ...moreargs ) { + [ this.message_id , this.interval_us ] = moreargs; + + this._format = '= 1 && + this.buf[0] != mavlink20.PROTOCOL_MARKER_V2 && + this.buf[0] != mavlink20.PROTOCOL_MARKER_V1) { + + // Strip the offending initial bytes and throw an error. + var badPrefix = this.buf[0]; + 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+")"); + } + +} + +// Determine the length. Leaves buffer untouched. +// Although the 'len' of a packet is available as of the second byte, the third byte with 'incompat_flags' lets +// 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; + } + } + +} + +// 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) { + + var m = null; + + try { + if (c != null) { + this.pushBuffer(c); + } + this.parsePrefix(); + this.parseLength(); + m = this.parsePayload(); + + } catch(e) { + this.log('error', e.message); + this.total_receive_errors += 1; + m = new mavlink20.messages.bad_data(this.bufInError, e.message); + this.bufInError = new Uint8Array(); + + } + + // emit a packet-specific message as well as a generic message, user/s can choose to use either or both of these. + if (isNode && null != m) { + this.emit(m._name, m); + this.emit('message', m); + } + + return m; + +} + +// continuation of python's __parse_char_legacy +MAVLink20Processor.prototype.parsePayload = function() { + + var m = null; + + // tip: this.expected_length and this.incompat_flags both already set correctly by parseLength(..) above + + // If we have enough bytes to try and read it, read it. + // shortest packet is header+checksum(2) with no payload, so we need at least that many + // but once we have a longer 'expected length' we have to read all of it. + if(( this.expected_length >= mavlink20.HEADER_LEN+2) && (this.buf.length >= this.expected_length) ) { + + // Slice off the expected packet length, reset expectation to be to find a header. + var mbuf = this.buf.slice(0, this.expected_length); + + // TODO: slicing off the buffer should depend on the error produced by the decode() function + // - if we find a well formed message, cut-off the expected_length + // - if the message is not well formed (correct prefix by accident), cut-off 1 char only + this.buf = this.buf.slice(this.expected_length); + this.expected_length = mavlink20.HEADER_LEN; // after attempting a parse, we'll next expect to find just a header. + + try { + m = this.decode(mbuf); + this.total_packets_received += 1; + } + catch(e) { + // Set buffer in question and re-throw to generic error handling + this.bufInError = mbuf; + throw e; + } + } + + return m; + +} + +// 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); + + // No messages available, bail. + 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 = []; + ret.push(m); + while(true) { + m = this.parseChar(null); + if ( null === m ) { + // No more messages left. + return ret; + } + ret.push(m); + } + +} + +//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) { + var timestamp_buf = msgbuf.slice(-12,-6); + + var link_id; + if (isNode) { + var link_id_buf = Buffer.from ? Buffer.from(msgbuf.slice(-13,-12)) : new Buffer(msgbuf.slice(-13,-12)); + link_id = link_id_buf[0]; // get the first byte. + } else { + // Browser-compatible buffer handling + link_id = msgbuf.slice(-13,-12)[0]; + } + + function unpackUint48LE(bytes) { + let value = 0n; + for (let i = 5; i >= 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, header_len; + + // decode the header + try { + 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]; + compat_flags = unpacked[3]; + seq = unpacked[4]; + srcSystem = unpacked[5]; + 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 + 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); + } + + 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 - (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 - (header_len + 2)) + " expected " + mlen + ", msgId=" + msgId); + + } + + if (!(msgId in mavlink20.map)) { + throw new Error("Unknown MAVLink message ID (" + msgId + ")"); + } + + // 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 { + var crcBuf1 = msgbuf.slice(-2); + receivedChecksum = jspack.Unpack(' payloadBuf.length) { + payloadBuf = this.concat_buffer(payloadBuf, new Uint8Array(paylen - payloadBuf.length).fill(0)); + } + // Decode the payload and reorder the fields to match the order map. + try { + var t = jspack.Unpack(decoder.format, payloadBuf); + } + catch (e) { + throw new Error('Unable to unpack MAVLink payload type='+decoder.type+' format='+decoder.format+' payloadLength='+ payloadBuf +': '+ e.message); + } + + // Need to check if the message contains arrays + var args = {}; + const elementsInMsg = decoder.order_map.length; + const actualElementsInMsg = JSON.parse(JSON.stringify(t)).length; + + if (elementsInMsg == actualElementsInMsg) { + // Reorder the fields to match the order map + t.forEach(function(e, i, l) { + args[i] = t[decoder.order_map[i]] + }); + } else { + // This message contains arrays + var typeIndex = 1; + var orderIndex = 0; + var memberIndex = 0; + var tempArgs = {}; + + // Walk through the fields + for(var i = 0, size = decoder.format.length-1; i <= size; ++i) { + var order = decoder.order_map[orderIndex]; + var currentType = decoder.format[typeIndex]; + + if (isNaN(parseInt(currentType))) { + // This field is not an array check the type and add it to the args + tempArgs[orderIndex] = t[memberIndex]; + memberIndex++; + } else { + // This field is part of an array, need to find the length of the array + var arraySize = '' + var newArray = [] + while (!isNaN(decoder.format[typeIndex])) { + arraySize = arraySize + decoder.format[typeIndex]; + typeIndex++; + } + + // Now that we know how long the array is, create an array with the values + for(var j = 0, size = parseInt(arraySize); j < size; ++j){ + newArray.push(t[j+orderIndex]); + memberIndex++; + } + + // Add the array to the args object + arraySize = arraySize + decoder.format[typeIndex]; + currentType = arraySize; + tempArgs[orderIndex] = newArray; + } + orderIndex++; + typeIndex++; + } + + // Finally reorder the fields to match the order map + t.forEach(function(e, i, l) { + args[i] = tempArgs[decoder.order_map[i]] + }); + } + + // 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, + 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._msgbuf = msgbuf; + m._payload = payloadBuf; + m.crc = receivedChecksum; + m._header = new mavlink20.header(msgId, mlen, seq, srcSystem, srcComponent, incompat_flags, compat_flags); + this.log(m); + return m; +} + + +// 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/modules/MAVLink/mavlink_store.js b/modules/MAVLink/mavlink_store.js new file mode 100644 index 00000000..f24a9b5e --- /dev/null +++ b/modules/MAVLink/mavlink_store.js @@ -0,0 +1,94 @@ +/* + MAVLink store of recently received messages + use the mavlink_store object to store and retrieve messages +*/ + +class MAVLinkMessageStore { + static _instance = null; // holds the one and only instance + + constructor() { + // return the existing instance if it already exists + if (MAVLinkMessageStore._instance) { + return MAVLinkMessageStore._instance; + } + + // first construction, do normal init + this.store = {}; + + // default system and component ids + this.sysid_default = 1; + this.compid_default = 1; + + // remember this instance for future constructions + MAVLinkMessageStore._instance = this; + } + + // optional helper, some teams prefer this call site + static getInstance() { + return new MAVLinkMessageStore(); + } + + // store message in the message store + // indexed by system id, component id and message id + store_message(msg) { + // sanity check msg + if ( + msg == null || + msg._id == null || + msg._header == null || + msg._header.srcSystem == null || + msg._header.srcComponent == null + ) { + return; + } + + // retrieve sender system id and component id + const sysid = msg._header.srcSystem; + const compid = msg._header.srcComponent; + + // store message + if (this.store[sysid] == null) { + this.store[sysid] = {}; + } + if (this.store[sysid][compid] == null) { + this.store[sysid][compid] = {}; + } + this.store[sysid][compid][msg._id] = msg; + } + + // retrieve latest message, returns null if not found + get_latest_message(msgid, sysid = this.sysid_default, compid = this.compid_default) { + if (this.store[sysid] == null) return null; + if (this.store[sysid][compid] == null) return null; + return this.store[sysid][compid][msgid]; + } + + find_message_by_name(name, sysid = this.sysid_default, compid = this.compid_default) { + if (!this.store[sysid] || !this.store[sysid][compid]) return null; + for (const id in this.store[sysid][compid]) { + const msg = this.store[sysid][compid][id]; + if (msg._name === name) return msg; + } + return null; + } + + // get available message names, returns array or null + get_available_message_names(sysid = this.sysid_default, compid = this.compid_default) { + if (this.store[sysid] == null) return null; + if (this.store[sysid][compid] == null) return null; + + const msg_ids = Object.keys(this.store[sysid][compid]); + if (msg_ids.length === 0) return null; + + const msg_names = msg_ids.map(id => this.store[sysid][compid][id]._name); + if (msg_names.length === 0) return null; + return msg_names; + } +} + +// export the class +export default MAVLinkMessageStore; + +// export the singleton instance +const mavlink_store = MAVLinkMessageStore.getInstance(); +export { mavlink_store };