node-mavlink 1.2.1 → 1.3.2
Sign up to get free protection for your applications and to get access to all the features.
- package/dist/index.d.ts +1 -0
- package/dist/index.d.ts.map +1 -0
- package/dist/index.js +21 -1
- package/dist/lib/logger.d.ts +2 -1
- package/dist/lib/logger.d.ts.map +1 -0
- package/dist/lib/logger.js +119 -1
- package/dist/lib/mavesp.d.ts +3 -1
- package/dist/lib/mavesp.d.ts.map +1 -0
- package/dist/lib/mavesp.js +101 -1
- package/dist/lib/mavlink.d.ts +14 -12
- package/dist/lib/mavlink.d.ts.map +1 -0
- package/dist/lib/mavlink.js +704 -1
- package/dist/lib/serialization.d.ts +9 -49
- package/dist/lib/serialization.d.ts.map +1 -0
- package/dist/lib/serialization.js +184 -1
- package/dist/lib/utils.d.ts +5 -4
- package/dist/lib/utils.d.ts.map +1 -0
- package/dist/lib/utils.js +77 -1
- package/package.json +11 -9
- package/sanity-check-minimal.cjs +9 -0
- package/sanity-check-minimal.mjs +11 -0
- package/sanity-check.cjs +24 -0
- package/sanity-check.mjs +24 -0
- package/.vscode/launch.json +0 -19
- package/.vscode/settings.json +0 -3
- package/index.ts +0 -5
- package/jest.config.js +0 -5
- package/lib/logger.ts +0 -128
- package/lib/mavesp.ts +0 -112
- package/lib/mavlink.ts +0 -806
- package/lib/serialization.test.ts +0 -256
- package/lib/serialization.ts +0 -176
- package/lib/utils.ts +0 -75
- package/tests/data.mavlink +0 -0
- package/tests/main.ts +0 -59
- package/tsconfig.json +0 -16
package/dist/lib/mavlink.js
CHANGED
@@ -1 +1,704 @@
|
|
1
|
-
"use strict";Object.defineProperty(exports,"__esModule",{value:!0}),exports.sendSigned=exports.send=exports.createMavLinkStream=exports.MavLinkPacketParser=exports.MavLinkPacketSplitter=exports.MavLinkPacket=exports.MavLinkPacketSignature=exports.MavLinkProtocolV2=exports.MavLinkProtocolV1=exports.MavLinkProtocol=exports.MavLinkPacketHeader=void 0;const stream_1=require("stream"),crypto_1=require("crypto"),mavlink_mappings_1=require("mavlink-mappings"),mavlink_mappings_2=require("mavlink-mappings"),utils_1=require("./utils"),logger_1=require("./logger"),serialization_1=require("./serialization");class MavLinkPacketHeader{constructor(){this.timestamp=null,this.magic=0,this.payloadLength=0,this.incompatibilityFlags=0,this.compatibilityFlags=0,this.seq=0,this.sysid=0,this.compid=0,this.msgid=0}}exports.MavLinkPacketHeader=MavLinkPacketHeader;class MavLinkProtocol{constructor(){this.log=logger_1.Logger.getLogger(this)}data(t,s){this.log.trace("Deserializing",s.MSG_NAME,"with payload of size",t.length);const n=new s;return s.FIELDS.forEach(e=>{const i=serialization_1.DESERIALIZERS[e.type];if(!i)throw new Error(`Unknown field type ${e.type}`);n[e.name]=i(t,e.offset,e.length)}),n}}exports.MavLinkProtocol=MavLinkProtocol,MavLinkProtocol.NAME="unknown",MavLinkProtocol.START_BYTE=0,MavLinkProtocol.PAYLOAD_OFFSET=0,MavLinkProtocol.CHECKSUM_LENGTH=2,MavLinkProtocol.SYS_ID=254,MavLinkProtocol.COMP_ID=1;class MavLinkProtocolV1 extends MavLinkProtocol{constructor(t=MavLinkProtocol.SYS_ID,s=MavLinkProtocol.COMP_ID){super(),this.sysid=t,this.compid=s}serialize(t,s){this.log.trace("Serializing message (seq:",s,")");const n=t.constructor,e=Buffer.from(new Uint8Array(MavLinkProtocolV1.PAYLOAD_OFFSET+n.PAYLOAD_LENGTH+MavLinkProtocol.CHECKSUM_LENGTH));e.writeUInt8(MavLinkProtocolV1.START_BYTE,0),e.writeUInt8(n.PAYLOAD_LENGTH,1),e.writeUInt8(s,2),e.writeUInt8(this.sysid,3),e.writeUInt8(this.compid,4),e.writeUInt8(n.MSG_ID,5),n.FIELDS.forEach(r=>{const o=serialization_1.SERIALIZERS[r.type];if(!o)throw new Error(`Unknown field type ${r.type}: serializer not found`);o(t[r.name],e,r.offset+MavLinkProtocolV1.PAYLOAD_OFFSET,r.length)});const i=(0,mavlink_mappings_1.x25crc)(e,1,2,n.MAGIC_NUMBER);return e.writeUInt16LE(i,e.length-2),e}header(t,s=null){this.log.trace("Reading header from buffer (len:",t.length,")");const n=t.readUInt8(0);if(n!==MavLinkProtocolV1.START_BYTE)throw new Error(`Invalid start byte (expected: ${MavLinkProtocolV1.START_BYTE}, got ${n})`);const e=new MavLinkPacketHeader;return e.timestamp=s,e.magic=n,e.payloadLength=t.readUInt8(1),e.seq=t.readUInt8(2),e.sysid=t.readUInt8(3),e.compid=t.readUInt8(4),e.msgid=t.readUInt8(5),e}crc(t){this.log.trace("Reading crc from buffer (len:",t.length,")");const s=t.readUInt8(1);return t.readUInt16LE(MavLinkProtocolV1.PAYLOAD_OFFSET+s)}payload(t){this.log.trace("Reading payload from buffer (len:",t.length,")");const s=t.readUInt8(1),n=t.slice(MavLinkProtocolV1.PAYLOAD_OFFSET,MavLinkProtocolV1.PAYLOAD_OFFSET+s),e=Buffer.from(new Uint8Array(255-n.length));return Buffer.concat([n,e])}}exports.MavLinkProtocolV1=MavLinkProtocolV1,MavLinkProtocolV1.NAME="MAV_V1",MavLinkProtocolV1.START_BYTE=254,MavLinkProtocolV1.PAYLOAD_OFFSET=6;class MavLinkProtocolV2 extends MavLinkProtocol{constructor(t=MavLinkProtocol.SYS_ID,s=MavLinkProtocol.COMP_ID,n=MavLinkProtocolV2.INCOMPATIBILITY_FLAGS,e=MavLinkProtocolV2.COMPATIBILITY_FLAGS){super(),this.sysid=t,this.compid=s,this.incompatibilityFlags=n,this.compatibilityFlags=e}serialize(t,s){this.log.trace("Serializing message (seq:",s,")");const n=t.constructor,e=Buffer.from(new Uint8Array(MavLinkProtocolV2.PAYLOAD_OFFSET+n.PAYLOAD_LENGTH+MavLinkProtocol.CHECKSUM_LENGTH));e.writeUInt8(MavLinkProtocolV2.START_BYTE,0),e.writeUInt8(this.incompatibilityFlags,2),e.writeUInt8(this.compatibilityFlags,3),e.writeUInt8(s,4),e.writeUInt8(this.sysid,5),e.writeUInt8(this.compid,6),e.writeUIntLE(n.MSG_ID,7,3),n.FIELDS.forEach(c=>{const h=serialization_1.SERIALIZERS[c.type];if(!h)throw new Error(`Unknown field type ${c.type}: serializer not found`);h(t[c.name],e,c.offset+MavLinkProtocolV2.PAYLOAD_OFFSET,c.length)});const i=this.calculateTruncatedPayloadLength(e);e.writeUInt8(i,1);const r=e.slice(0,MavLinkProtocolV2.PAYLOAD_OFFSET+i+MavLinkProtocol.CHECKSUM_LENGTH),o=(0,mavlink_mappings_1.x25crc)(r,1,2,n.MAGIC_NUMBER);return r.writeUInt16LE(o,r.length-MavLinkProtocol.CHECKSUM_LENGTH),r}sign(t,s,n,e=Date.now()){this.log.trace("Signing message");const i=Buffer.concat([t,Buffer.from(new Uint8Array(MavLinkPacketSignature.SIGNATURE_LENGTH))]),r=new MavLinkPacketSignature(i);return r.linkId=s,r.timestamp=e,r.signature=r.calculate(n),i}calculateTruncatedPayloadLength(t){let s=t.length;for(let n=t.length-MavLinkProtocol.CHECKSUM_LENGTH-1;n>=MavLinkProtocolV2.PAYLOAD_OFFSET;n--)if(s=n,t[n]!==0){s++;break}return s-MavLinkProtocolV2.PAYLOAD_OFFSET}header(t,s=null){this.log.trace("Reading header from buffer (len:",t.length,")");const n=t.readUInt8(0);if(n!==MavLinkProtocolV2.START_BYTE)throw new Error(`Invalid start byte (expected: ${MavLinkProtocolV2.START_BYTE}, got ${n})`);const e=new MavLinkPacketHeader;return e.timestamp=s,e.magic=n,e.payloadLength=t.readUInt8(1),e.incompatibilityFlags=t.readUInt8(2),e.compatibilityFlags=t.readUInt8(3),e.seq=t.readUInt8(4),e.sysid=t.readUInt8(5),e.compid=t.readUInt8(6),e.msgid=t.readUIntLE(7,3),e}crc(t){this.log.trace("Reading crc from buffer (len:",t.length,")");const s=t.readUInt8(1);return t.readUInt16LE(MavLinkProtocolV2.PAYLOAD_OFFSET+s)}payload(t){this.log.trace("Reading payload from buffer (len:",t.length,")");const s=t.readUInt8(1),n=t.slice(MavLinkProtocolV2.PAYLOAD_OFFSET,MavLinkProtocolV2.PAYLOAD_OFFSET+s),e=Buffer.from(new Uint8Array(255-n.length));return Buffer.concat([n,e])}signature(t,s){return this.log.trace("Reading signature from buffer (len:",t.length,")"),s.incompatibilityFlags&MavLinkProtocolV2.IFLAG_SIGNED?new MavLinkPacketSignature(t):null}}exports.MavLinkProtocolV2=MavLinkProtocolV2,MavLinkProtocolV2.NAME="MAV_V2",MavLinkProtocolV2.START_BYTE=253,MavLinkProtocolV2.PAYLOAD_OFFSET=10,MavLinkProtocolV2.INCOMPATIBILITY_FLAGS=0,MavLinkProtocolV2.COMPATIBILITY_FLAGS=0,MavLinkProtocolV2.IFLAG_SIGNED=1;const KNOWN_PROTOCOLS_BY_STX={[MavLinkProtocolV1.START_BYTE]:MavLinkProtocolV1,[MavLinkProtocolV2.START_BYTE]:MavLinkProtocolV2};class MavLinkPacketSignature{constructor(t){this.buffer=t}static key(t){return(0,crypto_1.createHash)("sha256").update(t).digest()}get offset(){return this.buffer.length-MavLinkPacketSignature.SIGNATURE_LENGTH}get linkId(){return this.buffer.readUInt8(this.offset)}set linkId(t){this.buffer.writeUInt8(this.offset)}get timestamp(){return this.buffer.readUIntLE(this.offset+1,6)}set timestamp(t){this.buffer.writeUIntLE(t,this.offset+1,6)}get signature(){return this.buffer.slice(this.offset+7,this.offset+7+6).toString("hex")}set signature(t){this.buffer.write(t,this.offset+7,"hex")}calculate(t){return(0,crypto_1.createHash)("sha256").update(t).update(this.buffer.slice(0,this.buffer.length-6)).digest("hex").substr(0,12)}matches(t){return this.calculate(t)===this.signature}toString(){return`linkid: ${this.linkId}, timestamp ${this.timestamp}, signature ${this.signature}`}}exports.MavLinkPacketSignature=MavLinkPacketSignature,MavLinkPacketSignature.SIGNATURE_LENGTH=13;class MavLinkPacket{constructor(t,s=new MavLinkPacketHeader,n=Buffer.from(new Uint8Array(255)),e=0,i=new MavLinkProtocolV1,r=null){this.buffer=t,this.header=s,this.payload=n,this.crc=e,this.protocol=i,this.signature=r}debug(){return`Packet (proto: ${this.protocol.constructor.NAME}, sysid: ${this.header.sysid}, compid: ${this.header.compid}, msgid: ${this.header.msgid}, seq: ${this.header.seq}, plen: ${this.header.payloadLength}, magic: ${mavlink_mappings_2.MSG_ID_MAGIC_NUMBER[this.header.msgid]} (${(0,utils_1.hex)(mavlink_mappings_2.MSG_ID_MAGIC_NUMBER[this.header.msgid])}), crc: ${(0,utils_1.hex)(this.crc,4)}`+this.signatureToString(this.signature)+")"}signatureToString(t){return t?`, ${t.toString()}`:""}}exports.MavLinkPacket=MavLinkPacket;var PacketValidationResult;(function(a){a[a.VALID=0]="VALID",a[a.INVALID=1]="INVALID",a[a.UNKNOWN=2]="UNKNOWN"})(PacketValidationResult||(PacketValidationResult={}));class MavLinkPacketSplitter extends stream_1.Transform{constructor(t={},s=()=>{}){super({...t,objectMode:!0}),this.log=logger_1.Logger.getLogger(this),this.buffer=Buffer.from([]),this.onCrcError=null,this.timestamp=null,this._validPackagesCount=0,this._unknownPackagesCount=0,this._invalidPackagesCount=0,this.onCrcError=s}_transform(t,s,n){for(this.buffer=Buffer.concat([this.buffer,t]);this.buffer.byteLength>0;){const e=this.findStartOfPacket(this.buffer);if(e===null)break;e>=8?this.timestamp=this.buffer.readBigUInt64BE(e-8)/1000n:this.timestamp=null,e>0&&(this.buffer=this.buffer.slice(e)),this.log.debug("Found potential packet start at",e);const i=this.getPacketProtocol(this.buffer);if(this.log.debug("Packet protocol is",i.NAME),this.buffer.length<i.PAYLOAD_OFFSET+MavLinkProtocol.CHECKSUM_LENGTH){this.log.debug("Current buffer shorter than the shortest message - skipping");break}const r=this.readPacketLength(this.buffer,i);if(this.log.debug("Expected buffer length:",r,`(${(0,utils_1.hex)(r)})`),this.buffer.length<r){this.log.debug("Current buffer is not fully retrieved yet - skipping");break}else this.log.debug("Current buffer length:",this.buffer.length,`(${(0,utils_1.hex)(this.buffer.length,4)})`);const o=this.buffer.slice(0,r);switch(this.log.debug("Recognized buffer length:",o.length,`(${(0,utils_1.hex)(o.length,2)})`),this.validatePacket(o,i)){case PacketValidationResult.VALID:this.log.debug("Found a valid packet"),this._validPackagesCount++,this.push({buffer:o,timestamp:this.timestamp}),this.buffer=this.buffer.slice(r);break;case PacketValidationResult.INVALID:this.log.debug("Found an invalid packet - skipping"),this._invalidPackagesCount++,this.buffer=this.buffer.slice(1);break;case PacketValidationResult.UNKNOWN:this.log.debug("Found an unknown packet - skipping"),this._unknownPackagesCount++,this.buffer=this.buffer.slice(r);break}}n(null)}findStartOfPacket(t){const s=t.indexOf(MavLinkProtocolV1.START_BYTE),n=t.indexOf(MavLinkProtocolV2.START_BYTE);return s>=0&&n>=0?s<n?s:n:s>=0?s:n>=0?n:null}getPacketProtocol(t){return KNOWN_PROTOCOLS_BY_STX[t.readUInt8(0)]||null}readPacketLength(t,s){const n=t.readUInt8(1);return s.PAYLOAD_OFFSET+n+MavLinkProtocol.CHECKSUM_LENGTH+(this.isV2Signed(t)?MavLinkPacketSignature.SIGNATURE_LENGTH:0)}validatePacket(t,s){const n=new s,e=n.header(t),i=mavlink_mappings_2.MSG_ID_MAGIC_NUMBER[e.msgid];if(i){const r=n.crc(t),o=this.isV2Signed(t)?MavLinkPacketSignature.SIGNATURE_LENGTH+MavLinkProtocol.CHECKSUM_LENGTH:MavLinkProtocol.CHECKSUM_LENGTH,c=(0,mavlink_mappings_1.x25crc)(t,1,o,i);if(r===c)return PacketValidationResult.VALID;{const h=[`CRC error; expected: ${c} (${(0,utils_1.hex)(c,4)}), got ${r} (${(0,utils_1.hex)(r,4)});`,`msgid: ${e.msgid} (${(0,utils_1.hex)(e.msgid)}),`,`seq: ${e.seq} (${(0,utils_1.hex)(e.seq)}),`,`plen: ${e.payloadLength} (${(0,utils_1.hex)(e.payloadLength)}),`,`magic: ${i} (${(0,utils_1.hex)(i)})`];return this.log.warn(h.join(" ")),this.onCrcError(t),PacketValidationResult.INVALID}}else return this.log.debug(`Unknown message with id ${e.msgid} (magic number not found) - skipping`),PacketValidationResult.UNKNOWN}isV2Signed(t){if(t.readUInt8(0)===MavLinkProtocolV2.START_BYTE)return!!(t.readUInt8(2)&MavLinkProtocolV2.IFLAG_SIGNED)}get validPackages(){return this._validPackagesCount}resetValidPackagesCount(){this._validPackagesCount=0}get invalidPackages(){return this._invalidPackagesCount}resetInvalidPackagesCount(){this._invalidPackagesCount=0}get unknownPackagesCount(){return this._unknownPackagesCount}resetUnknownPackagesCount(){this._unknownPackagesCount=0}}exports.MavLinkPacketSplitter=MavLinkPacketSplitter;class MavLinkPacketParser extends stream_1.Transform{constructor(t={}){super({...t,objectMode:!0}),this.log=logger_1.Logger.getLogger(this)}getProtocol(t){const s=t.readUInt8(0);switch(s){case MavLinkProtocolV1.START_BYTE:return new MavLinkProtocolV1;case MavLinkProtocolV2.START_BYTE:return new MavLinkProtocolV2;default:throw new Error(`Unknown protocol '${(0,utils_1.hex)(s)}'`)}}_transform({buffer:t=Buffer.from([]),timestamp:s=null,...n}={},e,i){const r=this.getProtocol(t),o=r.header(t,s),c=r.payload(t),h=r.crc(t),l=r instanceof MavLinkProtocolV2?r.signature(t,o):null,g=new MavLinkPacket(t,o,c,h,r,l);i(null,g)}}exports.MavLinkPacketParser=MavLinkPacketParser;function createMavLinkStream(a,t){return a.pipe(new MavLinkPacketSplitter({},t)).pipe(new MavLinkPacketParser)}exports.createMavLinkStream=createMavLinkStream;let seq=0;async function send(a,t,s=new MavLinkProtocolV1){return new Promise((n,e)=>{const i=s.serialize(t,seq++);seq&=255,a.write(i,r=>{r?e(r):n(i.length)})})}exports.send=send;async function sendSigned(a,t,s,n=1,e=MavLinkProtocol.SYS_ID,i=MavLinkProtocol.COMP_ID,r=Date.now()){return new Promise((o,c)=>{const h=new MavLinkProtocolV2(e,i,MavLinkProtocolV2.IFLAG_SIGNED),l=h.serialize(t,seq++);seq&=255;const g=h.sign(l,n,s,r);a.write(g,u=>{u?c(u):o(g.length)})})}exports.sendSigned=sendSigned;
|
1
|
+
"use strict";
|
2
|
+
Object.defineProperty(exports, "__esModule", { value: true });
|
3
|
+
exports.sendSigned = exports.send = exports.createMavLinkStream = exports.MavLinkPacketParser = exports.MavLinkPacketSplitter = exports.MavLinkPacket = exports.MavLinkPacketSignature = exports.MavLinkProtocolV2 = exports.MavLinkProtocolV1 = exports.MavLinkProtocol = exports.MavLinkPacketHeader = void 0;
|
4
|
+
const stream_1 = require("stream");
|
5
|
+
const crypto_1 = require("crypto");
|
6
|
+
const mavlink_mappings_1 = require("mavlink-mappings");
|
7
|
+
const mavlink_mappings_2 = require("mavlink-mappings");
|
8
|
+
const utils_1 = require("./utils");
|
9
|
+
const logger_1 = require("./logger");
|
10
|
+
const serialization_1 = require("./serialization");
|
11
|
+
/**
|
12
|
+
* Header definition of the MavLink packet
|
13
|
+
*/
|
14
|
+
class MavLinkPacketHeader {
|
15
|
+
timestamp = null;
|
16
|
+
magic = 0;
|
17
|
+
payloadLength = 0;
|
18
|
+
incompatibilityFlags = 0;
|
19
|
+
compatibilityFlags = 0;
|
20
|
+
seq = 0;
|
21
|
+
sysid = 0;
|
22
|
+
compid = 0;
|
23
|
+
msgid = 0;
|
24
|
+
}
|
25
|
+
exports.MavLinkPacketHeader = MavLinkPacketHeader;
|
26
|
+
/**
|
27
|
+
* Base class for protocols
|
28
|
+
*
|
29
|
+
* Implements common functionality like getting the CRC and deserializing
|
30
|
+
* data classes from the given payload buffer
|
31
|
+
*/
|
32
|
+
class MavLinkProtocol {
|
33
|
+
log = logger_1.Logger.getLogger(this);
|
34
|
+
static NAME = 'unknown';
|
35
|
+
static START_BYTE = 0;
|
36
|
+
static PAYLOAD_OFFSET = 0;
|
37
|
+
static CHECKSUM_LENGTH = 2;
|
38
|
+
static SYS_ID = 254;
|
39
|
+
static COMP_ID = 1;
|
40
|
+
/**
|
41
|
+
* Deserialize payload into actual data class
|
42
|
+
*/
|
43
|
+
data(payload, clazz) {
|
44
|
+
this.log.trace('Deserializing', clazz.MSG_NAME, 'with payload of size', payload.length);
|
45
|
+
const instance = new clazz();
|
46
|
+
for (const field of clazz.FIELDS) {
|
47
|
+
const deserialize = serialization_1.DESERIALIZERS[field.type];
|
48
|
+
if (!deserialize) {
|
49
|
+
throw new Error(`Unknown field type ${field.type}`);
|
50
|
+
}
|
51
|
+
// @ts-ignore
|
52
|
+
instance[field.name] = deserialize(payload, field.offset, field.length);
|
53
|
+
}
|
54
|
+
return instance;
|
55
|
+
}
|
56
|
+
}
|
57
|
+
exports.MavLinkProtocol = MavLinkProtocol;
|
58
|
+
/**
|
59
|
+
* MavLink Protocol V1
|
60
|
+
*/
|
61
|
+
class MavLinkProtocolV1 extends MavLinkProtocol {
|
62
|
+
sysid;
|
63
|
+
compid;
|
64
|
+
static NAME = 'MAV_V1';
|
65
|
+
static START_BYTE = 0xFE;
|
66
|
+
static PAYLOAD_OFFSET = 6;
|
67
|
+
constructor(sysid = MavLinkProtocol.SYS_ID, compid = MavLinkProtocol.COMP_ID) {
|
68
|
+
super();
|
69
|
+
this.sysid = sysid;
|
70
|
+
this.compid = compid;
|
71
|
+
}
|
72
|
+
serialize(message, seq) {
|
73
|
+
this.log.trace('Serializing message (seq:', seq, ')');
|
74
|
+
const definition = message.constructor;
|
75
|
+
const buffer = Buffer.from(new Uint8Array(MavLinkProtocolV1.PAYLOAD_OFFSET + definition.PAYLOAD_LENGTH + MavLinkProtocol.CHECKSUM_LENGTH));
|
76
|
+
// serialize header
|
77
|
+
buffer.writeUInt8(MavLinkProtocolV1.START_BYTE, 0);
|
78
|
+
buffer.writeUInt8(definition.PAYLOAD_LENGTH, 1);
|
79
|
+
buffer.writeUInt8(seq, 2);
|
80
|
+
buffer.writeUInt8(this.sysid, 3);
|
81
|
+
buffer.writeUInt8(this.compid, 4);
|
82
|
+
buffer.writeUInt8(definition.MSG_ID, 5);
|
83
|
+
// serialize fields
|
84
|
+
definition.FIELDS.forEach(field => {
|
85
|
+
const serialize = serialization_1.SERIALIZERS[field.type];
|
86
|
+
if (!serialize)
|
87
|
+
throw new Error(`Unknown field type ${field.type}: serializer not found`);
|
88
|
+
// @ts-ignore
|
89
|
+
serialize(message[field.name], buffer, field.offset + MavLinkProtocolV1.PAYLOAD_OFFSET, field.length);
|
90
|
+
});
|
91
|
+
// serialize checksum
|
92
|
+
const crc = (0, mavlink_mappings_1.x25crc)(buffer, 1, 2, definition.MAGIC_NUMBER);
|
93
|
+
buffer.writeUInt16LE(crc, buffer.length - 2);
|
94
|
+
return buffer;
|
95
|
+
}
|
96
|
+
header(buffer, timestamp) {
|
97
|
+
this.log.trace('Reading header from buffer (len:', buffer.length, ')');
|
98
|
+
const startByte = buffer.readUInt8(0);
|
99
|
+
if (startByte !== MavLinkProtocolV1.START_BYTE) {
|
100
|
+
throw new Error(`Invalid start byte (expected: ${MavLinkProtocolV1.START_BYTE}, got ${startByte})`);
|
101
|
+
}
|
102
|
+
const result = new MavLinkPacketHeader();
|
103
|
+
result.timestamp = timestamp || null;
|
104
|
+
result.magic = startByte;
|
105
|
+
result.payloadLength = buffer.readUInt8(1);
|
106
|
+
result.seq = buffer.readUInt8(2);
|
107
|
+
result.sysid = buffer.readUInt8(3);
|
108
|
+
result.compid = buffer.readUInt8(4);
|
109
|
+
result.msgid = buffer.readUInt8(5);
|
110
|
+
return result;
|
111
|
+
}
|
112
|
+
/**
|
113
|
+
* Deserialize packet checksum
|
114
|
+
*/
|
115
|
+
crc(buffer) {
|
116
|
+
this.log.trace('Reading crc from buffer (len:', buffer.length, ')');
|
117
|
+
const plen = buffer.readUInt8(1);
|
118
|
+
return buffer.readUInt16LE(MavLinkProtocolV1.PAYLOAD_OFFSET + plen);
|
119
|
+
}
|
120
|
+
payload(buffer) {
|
121
|
+
this.log.trace('Reading payload from buffer (len:', buffer.length, ')');
|
122
|
+
const plen = buffer.readUInt8(1);
|
123
|
+
const payload = buffer.slice(MavLinkProtocolV1.PAYLOAD_OFFSET, MavLinkProtocolV1.PAYLOAD_OFFSET + plen);
|
124
|
+
const padding = Buffer.from(new Uint8Array(255 - payload.length));
|
125
|
+
return Buffer.concat([payload, padding]);
|
126
|
+
}
|
127
|
+
}
|
128
|
+
exports.MavLinkProtocolV1 = MavLinkProtocolV1;
|
129
|
+
/**
|
130
|
+
* MavLink Protocol V2
|
131
|
+
*/
|
132
|
+
class MavLinkProtocolV2 extends MavLinkProtocol {
|
133
|
+
sysid;
|
134
|
+
compid;
|
135
|
+
incompatibilityFlags;
|
136
|
+
compatibilityFlags;
|
137
|
+
static NAME = 'MAV_V2';
|
138
|
+
static START_BYTE = 0xFD;
|
139
|
+
static PAYLOAD_OFFSET = 10;
|
140
|
+
static INCOMPATIBILITY_FLAGS = 0;
|
141
|
+
static COMPATIBILITY_FLAGS = 0;
|
142
|
+
static IFLAG_SIGNED = 0x01;
|
143
|
+
constructor(sysid = MavLinkProtocol.SYS_ID, compid = MavLinkProtocol.COMP_ID, incompatibilityFlags = MavLinkProtocolV2.INCOMPATIBILITY_FLAGS, compatibilityFlags = MavLinkProtocolV2.COMPATIBILITY_FLAGS) {
|
144
|
+
super();
|
145
|
+
this.sysid = sysid;
|
146
|
+
this.compid = compid;
|
147
|
+
this.incompatibilityFlags = incompatibilityFlags;
|
148
|
+
this.compatibilityFlags = compatibilityFlags;
|
149
|
+
}
|
150
|
+
serialize(message, seq) {
|
151
|
+
this.log.trace('Serializing message (seq:', seq, ')');
|
152
|
+
const definition = message.constructor;
|
153
|
+
const buffer = Buffer.from(new Uint8Array(MavLinkProtocolV2.PAYLOAD_OFFSET + definition.PAYLOAD_LENGTH + MavLinkProtocol.CHECKSUM_LENGTH));
|
154
|
+
buffer.writeUInt8(MavLinkProtocolV2.START_BYTE, 0);
|
155
|
+
buffer.writeUInt8(this.incompatibilityFlags, 2);
|
156
|
+
buffer.writeUInt8(this.compatibilityFlags, 3);
|
157
|
+
buffer.writeUInt8(seq, 4);
|
158
|
+
buffer.writeUInt8(this.sysid, 5);
|
159
|
+
buffer.writeUInt8(this.compid, 6);
|
160
|
+
buffer.writeUIntLE(definition.MSG_ID, 7, 3);
|
161
|
+
definition.FIELDS.forEach(field => {
|
162
|
+
const serialize = serialization_1.SERIALIZERS[field.type];
|
163
|
+
if (!serialize)
|
164
|
+
throw new Error(`Unknown field type ${field.type}: serializer not found`);
|
165
|
+
// @ts-ignore
|
166
|
+
serialize(message[field.name], buffer, field.offset + MavLinkProtocolV2.PAYLOAD_OFFSET, field.length);
|
167
|
+
});
|
168
|
+
// calculate actual truncated payload length
|
169
|
+
const payloadLength = this.calculateTruncatedPayloadLength(buffer);
|
170
|
+
buffer.writeUInt8(payloadLength, 1);
|
171
|
+
// slice out the message buffer
|
172
|
+
const result = buffer.slice(0, MavLinkProtocolV2.PAYLOAD_OFFSET + payloadLength + MavLinkProtocol.CHECKSUM_LENGTH);
|
173
|
+
const crc = (0, mavlink_mappings_1.x25crc)(result, 1, 2, definition.MAGIC_NUMBER);
|
174
|
+
result.writeUInt16LE(crc, result.length - MavLinkProtocol.CHECKSUM_LENGTH);
|
175
|
+
return result;
|
176
|
+
}
|
177
|
+
/**
|
178
|
+
* Create a signed package buffer
|
179
|
+
*
|
180
|
+
* @param buffer buffer with the original, unsigned package
|
181
|
+
* @param linkId id of the link
|
182
|
+
* @param key key to sign the package with
|
183
|
+
* @param timestamp optional timestamp for packet signing (default: Date.now())
|
184
|
+
* @returns signed package
|
185
|
+
*/
|
186
|
+
sign(buffer, linkId, key, timestamp = Date.now()) {
|
187
|
+
this.log.trace('Signing message');
|
188
|
+
const result = Buffer.concat([
|
189
|
+
buffer,
|
190
|
+
Buffer.from(new Uint8Array(MavLinkPacketSignature.SIGNATURE_LENGTH))
|
191
|
+
]);
|
192
|
+
const signer = new MavLinkPacketSignature(result);
|
193
|
+
signer.linkId = linkId;
|
194
|
+
signer.timestamp = timestamp;
|
195
|
+
signer.signature = signer.calculate(key);
|
196
|
+
return result;
|
197
|
+
}
|
198
|
+
calculateTruncatedPayloadLength(buffer) {
|
199
|
+
let result = buffer.length;
|
200
|
+
for (let i = buffer.length - MavLinkProtocol.CHECKSUM_LENGTH - 1; i >= MavLinkProtocolV2.PAYLOAD_OFFSET; i--) {
|
201
|
+
result = i;
|
202
|
+
if (buffer[i] !== 0) {
|
203
|
+
result++;
|
204
|
+
break;
|
205
|
+
}
|
206
|
+
}
|
207
|
+
return result - MavLinkProtocolV2.PAYLOAD_OFFSET;
|
208
|
+
}
|
209
|
+
header(buffer, timestamp) {
|
210
|
+
this.log.trace('Reading header from buffer (len:', buffer.length, ')');
|
211
|
+
const startByte = buffer.readUInt8(0);
|
212
|
+
if (startByte !== MavLinkProtocolV2.START_BYTE) {
|
213
|
+
throw new Error(`Invalid start byte (expected: ${MavLinkProtocolV2.START_BYTE}, got ${startByte})`);
|
214
|
+
}
|
215
|
+
const result = new MavLinkPacketHeader();
|
216
|
+
result.timestamp = timestamp || null;
|
217
|
+
result.magic = startByte;
|
218
|
+
result.payloadLength = buffer.readUInt8(1);
|
219
|
+
result.incompatibilityFlags = buffer.readUInt8(2);
|
220
|
+
result.compatibilityFlags = buffer.readUInt8(3);
|
221
|
+
result.seq = buffer.readUInt8(4);
|
222
|
+
result.sysid = buffer.readUInt8(5);
|
223
|
+
result.compid = buffer.readUInt8(6);
|
224
|
+
result.msgid = buffer.readUIntLE(7, 3);
|
225
|
+
return result;
|
226
|
+
}
|
227
|
+
/**
|
228
|
+
* Deserialize packet checksum
|
229
|
+
*/
|
230
|
+
crc(buffer) {
|
231
|
+
this.log.trace('Reading crc from buffer (len:', buffer.length, ')');
|
232
|
+
const plen = buffer.readUInt8(1);
|
233
|
+
return buffer.readUInt16LE(MavLinkProtocolV2.PAYLOAD_OFFSET + plen);
|
234
|
+
}
|
235
|
+
payload(buffer) {
|
236
|
+
this.log.trace('Reading payload from buffer (len:', buffer.length, ')');
|
237
|
+
const plen = buffer.readUInt8(1);
|
238
|
+
const payload = buffer.slice(MavLinkProtocolV2.PAYLOAD_OFFSET, MavLinkProtocolV2.PAYLOAD_OFFSET + plen);
|
239
|
+
const padding = Buffer.from(new Uint8Array(255 - payload.length));
|
240
|
+
return Buffer.concat([payload, padding]);
|
241
|
+
}
|
242
|
+
signature(buffer, header) {
|
243
|
+
this.log.trace('Reading signature from buffer (len:', buffer.length, ')');
|
244
|
+
if (header.incompatibilityFlags & MavLinkProtocolV2.IFLAG_SIGNED) {
|
245
|
+
return new MavLinkPacketSignature(buffer);
|
246
|
+
}
|
247
|
+
else {
|
248
|
+
return null;
|
249
|
+
}
|
250
|
+
}
|
251
|
+
}
|
252
|
+
exports.MavLinkProtocolV2 = MavLinkProtocolV2;
|
253
|
+
/**
|
254
|
+
* Registry of known protocols by STX
|
255
|
+
*/
|
256
|
+
const KNOWN_PROTOCOLS_BY_STX = {
|
257
|
+
[MavLinkProtocolV1.START_BYTE]: MavLinkProtocolV1,
|
258
|
+
[MavLinkProtocolV2.START_BYTE]: MavLinkProtocolV2,
|
259
|
+
};
|
260
|
+
/**
|
261
|
+
* MavLink packet signature definition
|
262
|
+
*/
|
263
|
+
class MavLinkPacketSignature {
|
264
|
+
buffer;
|
265
|
+
static SIGNATURE_LENGTH = 13;
|
266
|
+
/**
|
267
|
+
* Calculate key based on secret passphrase
|
268
|
+
*
|
269
|
+
* @param passphrase secret to generate the key
|
270
|
+
* @returns key as a buffer
|
271
|
+
*/
|
272
|
+
static key(passphrase) {
|
273
|
+
return (0, crypto_1.createHash)('sha256')
|
274
|
+
.update(passphrase)
|
275
|
+
.digest();
|
276
|
+
}
|
277
|
+
constructor(buffer) {
|
278
|
+
this.buffer = buffer;
|
279
|
+
}
|
280
|
+
get offset() {
|
281
|
+
return this.buffer.length - MavLinkPacketSignature.SIGNATURE_LENGTH;
|
282
|
+
}
|
283
|
+
/**
|
284
|
+
* Get the linkId from signature
|
285
|
+
*/
|
286
|
+
get linkId() {
|
287
|
+
return this.buffer.readUInt8(this.offset);
|
288
|
+
}
|
289
|
+
/**
|
290
|
+
* Set the linkId in signature
|
291
|
+
*/
|
292
|
+
set linkId(value) {
|
293
|
+
this.buffer.writeUInt8(this.offset);
|
294
|
+
}
|
295
|
+
/**
|
296
|
+
* Get the timestamp from signature
|
297
|
+
*/
|
298
|
+
get timestamp() {
|
299
|
+
return this.buffer.readUIntLE(this.offset + 1, 6);
|
300
|
+
}
|
301
|
+
/**
|
302
|
+
* Set the linkId in signature
|
303
|
+
*/
|
304
|
+
set timestamp(value) {
|
305
|
+
this.buffer.writeUIntLE(value, this.offset + 1, 6);
|
306
|
+
}
|
307
|
+
/**
|
308
|
+
* Get the signature from signature
|
309
|
+
*/
|
310
|
+
get signature() {
|
311
|
+
return this.buffer.slice(this.offset + 7, this.offset + 7 + 6).toString('hex');
|
312
|
+
}
|
313
|
+
/**
|
314
|
+
* Set the signature in signature
|
315
|
+
*/
|
316
|
+
set signature(value) {
|
317
|
+
this.buffer.write(value, this.offset + 7, 'hex');
|
318
|
+
}
|
319
|
+
/**
|
320
|
+
* Calculates signature of the packet buffer using the provided secret.
|
321
|
+
* The secret is converted to a hash using the sha256 algorithm which matches
|
322
|
+
* the way Mission Planner creates keys.
|
323
|
+
*
|
324
|
+
* @param key the secret key (Buffer)
|
325
|
+
* @returns calculated signature value
|
326
|
+
*/
|
327
|
+
calculate(key) {
|
328
|
+
const hash = (0, crypto_1.createHash)('sha256')
|
329
|
+
.update(key)
|
330
|
+
.update(this.buffer.slice(0, this.buffer.length - 6))
|
331
|
+
.digest('hex')
|
332
|
+
.substr(0, 12);
|
333
|
+
return hash;
|
334
|
+
}
|
335
|
+
/**
|
336
|
+
* Checks the signature of the packet buffer against a given secret
|
337
|
+
* The secret is converted to a hash using the sha256 algorithm which matches
|
338
|
+
* the way Mission Planner creates keys.
|
339
|
+
*
|
340
|
+
* @param key key
|
341
|
+
* @returns true if the signature matches, false otherwise
|
342
|
+
*/
|
343
|
+
matches(key) {
|
344
|
+
return this.calculate(key) === this.signature;
|
345
|
+
}
|
346
|
+
toString() {
|
347
|
+
return `linkid: ${this.linkId}, timestamp ${this.timestamp}, signature ${this.signature}`;
|
348
|
+
}
|
349
|
+
}
|
350
|
+
exports.MavLinkPacketSignature = MavLinkPacketSignature;
|
351
|
+
/**
|
352
|
+
* MavLink packet definition
|
353
|
+
*/
|
354
|
+
class MavLinkPacket {
|
355
|
+
buffer;
|
356
|
+
header;
|
357
|
+
payload;
|
358
|
+
crc;
|
359
|
+
protocol;
|
360
|
+
signature;
|
361
|
+
constructor(buffer, header = new MavLinkPacketHeader(), payload = Buffer.from(new Uint8Array(255)), crc = 0, protocol = new MavLinkProtocolV1(), signature = null) {
|
362
|
+
this.buffer = buffer;
|
363
|
+
this.header = header;
|
364
|
+
this.payload = payload;
|
365
|
+
this.crc = crc;
|
366
|
+
this.protocol = protocol;
|
367
|
+
this.signature = signature;
|
368
|
+
}
|
369
|
+
/**
|
370
|
+
* Debug information about the packet
|
371
|
+
*
|
372
|
+
* @returns string representing debug information about a packet
|
373
|
+
*/
|
374
|
+
debug() {
|
375
|
+
return 'Packet ('
|
376
|
+
// @ts-ignore
|
377
|
+
+ `proto: ${this.protocol.constructor['NAME']}, `
|
378
|
+
+ `sysid: ${this.header.sysid}, `
|
379
|
+
+ `compid: ${this.header.compid}, `
|
380
|
+
+ `msgid: ${this.header.msgid}, `
|
381
|
+
+ `seq: ${this.header.seq}, `
|
382
|
+
+ `plen: ${this.header.payloadLength}, `
|
383
|
+
// @ts-ignore
|
384
|
+
+ `magic: ${mavlink_mappings_2.MSG_ID_MAGIC_NUMBER[this.header.msgid]} (${(0, utils_1.hex)(mavlink_mappings_2.MSG_ID_MAGIC_NUMBER[this.header.msgid])}), `
|
385
|
+
+ `crc: ${(0, utils_1.hex)(this.crc, 4)}`
|
386
|
+
// @ts-ignore
|
387
|
+
+ this.signatureToString(this.signature)
|
388
|
+
+ ')';
|
389
|
+
}
|
390
|
+
signatureToString(signature) {
|
391
|
+
return signature ? `, ${signature.toString()}` : '';
|
392
|
+
}
|
393
|
+
}
|
394
|
+
exports.MavLinkPacket = MavLinkPacket;
|
395
|
+
/**
|
396
|
+
* This enum describes the different ways validation of a buffer can end
|
397
|
+
*/
|
398
|
+
var PacketValidationResult;
|
399
|
+
(function (PacketValidationResult) {
|
400
|
+
PacketValidationResult[PacketValidationResult["VALID"] = 0] = "VALID";
|
401
|
+
PacketValidationResult[PacketValidationResult["INVALID"] = 1] = "INVALID";
|
402
|
+
PacketValidationResult[PacketValidationResult["UNKNOWN"] = 2] = "UNKNOWN";
|
403
|
+
})(PacketValidationResult || (PacketValidationResult = {}));
|
404
|
+
/**
|
405
|
+
* A transform stream that splits the incomming data stream into chunks containing full MavLink messages
|
406
|
+
*/
|
407
|
+
class MavLinkPacketSplitter extends stream_1.Transform {
|
408
|
+
log = logger_1.Logger.getLogger(this);
|
409
|
+
buffer = Buffer.from([]);
|
410
|
+
onCrcError = null;
|
411
|
+
timestamp = null;
|
412
|
+
_validPackagesCount = 0;
|
413
|
+
_unknownPackagesCount = 0;
|
414
|
+
_invalidPackagesCount = 0;
|
415
|
+
/**
|
416
|
+
* @param opts options to pass on to the Transform constructor
|
417
|
+
* @param verbose print diagnostic information
|
418
|
+
* @param onCrcError callback executed if there is a CRC error (mostly for debugging)
|
419
|
+
*/
|
420
|
+
constructor(opts = {}, onCrcError = () => { }) {
|
421
|
+
super({ ...opts, objectMode: true });
|
422
|
+
this.onCrcError = onCrcError;
|
423
|
+
}
|
424
|
+
_transform(chunk, encoding, callback) {
|
425
|
+
this.buffer = Buffer.concat([this.buffer, chunk]);
|
426
|
+
while (this.buffer.byteLength > 0) {
|
427
|
+
const offset = this.findStartOfPacket(this.buffer);
|
428
|
+
if (offset === null) {
|
429
|
+
// start of the package was not found - need more data
|
430
|
+
break;
|
431
|
+
}
|
432
|
+
// if the current offset is exactly the size of the timestamp field from tlog then read it.
|
433
|
+
if (offset >= 8) {
|
434
|
+
this.timestamp = this.buffer.readBigUInt64BE(offset - 8) / 1000n;
|
435
|
+
}
|
436
|
+
else {
|
437
|
+
this.timestamp = null;
|
438
|
+
}
|
439
|
+
// fast-forward the buffer to the first start byte
|
440
|
+
if (offset > 0) {
|
441
|
+
this.buffer = this.buffer.slice(offset);
|
442
|
+
}
|
443
|
+
this.log.debug('Found potential packet start at', offset);
|
444
|
+
// get protocol this buffer is encoded with
|
445
|
+
const Protocol = this.getPacketProtocol(this.buffer);
|
446
|
+
this.log.debug('Packet protocol is', Protocol.NAME);
|
447
|
+
// check if the buffer contains at least the minumum size of data
|
448
|
+
if (this.buffer.length < Protocol.PAYLOAD_OFFSET + MavLinkProtocol.CHECKSUM_LENGTH) {
|
449
|
+
// current buffer shorter than the shortest message - skipping
|
450
|
+
this.log.debug('Current buffer shorter than the shortest message - skipping');
|
451
|
+
break;
|
452
|
+
}
|
453
|
+
// check if the current buffer contains the entire message
|
454
|
+
const expectedBufferLength = this.readPacketLength(this.buffer, Protocol);
|
455
|
+
this.log.debug('Expected buffer length:', expectedBufferLength, `(${(0, utils_1.hex)(expectedBufferLength)})`);
|
456
|
+
if (this.buffer.length < expectedBufferLength) {
|
457
|
+
// current buffer is not fully retrieved yet - skipping
|
458
|
+
this.log.debug('Current buffer is not fully retrieved yet - skipping');
|
459
|
+
break;
|
460
|
+
}
|
461
|
+
else {
|
462
|
+
this.log.debug('Current buffer length:', this.buffer.length, `(${(0, utils_1.hex)(this.buffer.length, 4)})`);
|
463
|
+
}
|
464
|
+
// retrieve the buffer based on payload size
|
465
|
+
const buffer = this.buffer.slice(0, expectedBufferLength);
|
466
|
+
this.log.debug('Recognized buffer length:', buffer.length, `(${(0, utils_1.hex)(buffer.length, 2)})`);
|
467
|
+
switch (this.validatePacket(buffer, Protocol)) {
|
468
|
+
case PacketValidationResult.VALID:
|
469
|
+
this.log.debug('Found a valid packet');
|
470
|
+
this._validPackagesCount++;
|
471
|
+
this.push({ buffer, timestamp: this.timestamp });
|
472
|
+
// truncate the buffer to remove the current message
|
473
|
+
this.buffer = this.buffer.slice(expectedBufferLength);
|
474
|
+
break;
|
475
|
+
case PacketValidationResult.INVALID:
|
476
|
+
this.log.debug('Found an invalid packet - skipping');
|
477
|
+
this._invalidPackagesCount++;
|
478
|
+
// truncate the buffer to remove the wrongly identified STX
|
479
|
+
this.buffer = this.buffer.slice(1);
|
480
|
+
break;
|
481
|
+
case PacketValidationResult.UNKNOWN:
|
482
|
+
this.log.debug('Found an unknown packet - skipping');
|
483
|
+
this._unknownPackagesCount++;
|
484
|
+
// truncate the buffer to remove the current message
|
485
|
+
this.buffer = this.buffer.slice(expectedBufferLength);
|
486
|
+
break;
|
487
|
+
}
|
488
|
+
}
|
489
|
+
callback(null);
|
490
|
+
}
|
491
|
+
findStartOfPacket(buffer) {
|
492
|
+
const stxv1 = buffer.indexOf(MavLinkProtocolV1.START_BYTE);
|
493
|
+
const stxv2 = buffer.indexOf(MavLinkProtocolV2.START_BYTE);
|
494
|
+
if (stxv1 >= 0 && stxv2 >= 0) {
|
495
|
+
// in the current buffer both STX v1 and v2 are found - get the first one
|
496
|
+
if (stxv1 < stxv2) {
|
497
|
+
return stxv1;
|
498
|
+
}
|
499
|
+
else {
|
500
|
+
return stxv2;
|
501
|
+
}
|
502
|
+
}
|
503
|
+
else if (stxv1 >= 0) {
|
504
|
+
// in the current buffer STX v1 is found
|
505
|
+
return stxv1;
|
506
|
+
}
|
507
|
+
else if (stxv2 >= 0) {
|
508
|
+
// in the current buffer STX v2 is found
|
509
|
+
return stxv2;
|
510
|
+
}
|
511
|
+
else {
|
512
|
+
// no STX found
|
513
|
+
return null;
|
514
|
+
}
|
515
|
+
}
|
516
|
+
getPacketProtocol(buffer) {
|
517
|
+
return KNOWN_PROTOCOLS_BY_STX[buffer.readUInt8(0)] || null;
|
518
|
+
}
|
519
|
+
readPacketLength(buffer, Protocol) {
|
520
|
+
// check if the current buffer contains the entire message
|
521
|
+
const payloadLength = buffer.readUInt8(1);
|
522
|
+
return Protocol.PAYLOAD_OFFSET
|
523
|
+
+ payloadLength
|
524
|
+
+ MavLinkProtocol.CHECKSUM_LENGTH
|
525
|
+
+ (this.isV2Signed(buffer) ? MavLinkPacketSignature.SIGNATURE_LENGTH : 0);
|
526
|
+
}
|
527
|
+
validatePacket(buffer, Protocol) {
|
528
|
+
const protocol = new Protocol();
|
529
|
+
const header = protocol.header(buffer);
|
530
|
+
// @ts-ignore
|
531
|
+
const magic = mavlink_mappings_2.MSG_ID_MAGIC_NUMBER[header.msgid];
|
532
|
+
if (magic) {
|
533
|
+
const crc = protocol.crc(buffer);
|
534
|
+
const trim = this.isV2Signed(buffer)
|
535
|
+
? MavLinkPacketSignature.SIGNATURE_LENGTH + MavLinkProtocol.CHECKSUM_LENGTH
|
536
|
+
: MavLinkProtocol.CHECKSUM_LENGTH;
|
537
|
+
const crc2 = (0, mavlink_mappings_1.x25crc)(buffer, 1, trim, magic);
|
538
|
+
if (crc === crc2) {
|
539
|
+
// this is a proper message that is known and has been validated for corrupted data
|
540
|
+
return PacketValidationResult.VALID;
|
541
|
+
}
|
542
|
+
else {
|
543
|
+
// CRC mismatch
|
544
|
+
const message = [
|
545
|
+
`CRC error; expected: ${crc2} (${(0, utils_1.hex)(crc2, 4)}), got ${crc} (${(0, utils_1.hex)(crc, 4)});`,
|
546
|
+
`msgid: ${header.msgid} (${(0, utils_1.hex)(header.msgid)}),`,
|
547
|
+
`seq: ${header.seq} (${(0, utils_1.hex)(header.seq)}),`,
|
548
|
+
`plen: ${header.payloadLength} (${(0, utils_1.hex)(header.payloadLength)}),`,
|
549
|
+
`magic: ${magic} (${(0, utils_1.hex)(magic)})`,
|
550
|
+
];
|
551
|
+
this.log.warn(message.join(' '));
|
552
|
+
if (this.onCrcError)
|
553
|
+
this.onCrcError(buffer);
|
554
|
+
return PacketValidationResult.INVALID;
|
555
|
+
}
|
556
|
+
}
|
557
|
+
else {
|
558
|
+
// unknown message (as in not generated from the XML sources)
|
559
|
+
this.log.debug(`Unknown message with id ${header.msgid} (magic number not found) - skipping`);
|
560
|
+
return PacketValidationResult.UNKNOWN;
|
561
|
+
}
|
562
|
+
}
|
563
|
+
/**
|
564
|
+
* Checks if the buffer contains the entire message with signature
|
565
|
+
*
|
566
|
+
* @param buffer buffer with the message
|
567
|
+
*/
|
568
|
+
isV2Signed(buffer) {
|
569
|
+
const protocol = buffer.readUInt8(0);
|
570
|
+
if (protocol === MavLinkProtocolV2.START_BYTE) {
|
571
|
+
const flags = buffer.readUInt8(2);
|
572
|
+
return !!(flags & MavLinkProtocolV2.IFLAG_SIGNED);
|
573
|
+
}
|
574
|
+
}
|
575
|
+
/**
|
576
|
+
* Number of invalid packages
|
577
|
+
*/
|
578
|
+
get validPackages() {
|
579
|
+
return this._validPackagesCount;
|
580
|
+
}
|
581
|
+
/**
|
582
|
+
* Reset the number of valid packages
|
583
|
+
*/
|
584
|
+
resetValidPackagesCount() {
|
585
|
+
this._validPackagesCount = 0;
|
586
|
+
}
|
587
|
+
/**
|
588
|
+
* Number of invalid packages
|
589
|
+
*/
|
590
|
+
get invalidPackages() {
|
591
|
+
return this._invalidPackagesCount;
|
592
|
+
}
|
593
|
+
/**
|
594
|
+
* Reset the number of invalid packages
|
595
|
+
*/
|
596
|
+
resetInvalidPackagesCount() {
|
597
|
+
this._invalidPackagesCount = 0;
|
598
|
+
}
|
599
|
+
/**
|
600
|
+
* Number of invalid packages
|
601
|
+
*/
|
602
|
+
get unknownPackagesCount() {
|
603
|
+
return this._unknownPackagesCount;
|
604
|
+
}
|
605
|
+
/**
|
606
|
+
* Reset the number of invalid packages
|
607
|
+
*/
|
608
|
+
resetUnknownPackagesCount() {
|
609
|
+
this._unknownPackagesCount = 0;
|
610
|
+
}
|
611
|
+
}
|
612
|
+
exports.MavLinkPacketSplitter = MavLinkPacketSplitter;
|
613
|
+
/**
|
614
|
+
* A transform stream that takes a buffer with data and converts it to MavLinkPacket object
|
615
|
+
*/
|
616
|
+
class MavLinkPacketParser extends stream_1.Transform {
|
617
|
+
log = logger_1.Logger.getLogger(this);
|
618
|
+
constructor(opts = {}) {
|
619
|
+
super({ ...opts, objectMode: true });
|
620
|
+
}
|
621
|
+
getProtocol(buffer) {
|
622
|
+
const startByte = buffer.readUInt8(0);
|
623
|
+
switch (startByte) {
|
624
|
+
case MavLinkProtocolV1.START_BYTE:
|
625
|
+
return new MavLinkProtocolV1();
|
626
|
+
case MavLinkProtocolV2.START_BYTE:
|
627
|
+
return new MavLinkProtocolV2();
|
628
|
+
default:
|
629
|
+
throw new Error(`Unknown protocol '${(0, utils_1.hex)(startByte)}'`);
|
630
|
+
}
|
631
|
+
}
|
632
|
+
_transform({ buffer = Buffer.from([]), timestamp = null, ...rest } = {}, encoding, callback) {
|
633
|
+
const protocol = this.getProtocol(buffer);
|
634
|
+
const header = protocol.header(buffer, timestamp || undefined);
|
635
|
+
const payload = protocol.payload(buffer);
|
636
|
+
const crc = protocol.crc(buffer);
|
637
|
+
const signature = protocol instanceof MavLinkProtocolV2
|
638
|
+
? protocol.signature(buffer, header)
|
639
|
+
: null;
|
640
|
+
const packet = new MavLinkPacket(buffer, header, payload, crc, protocol, signature);
|
641
|
+
callback(null, packet);
|
642
|
+
}
|
643
|
+
}
|
644
|
+
exports.MavLinkPacketParser = MavLinkPacketParser;
|
645
|
+
/**
|
646
|
+
* Creates a MavLink packet stream reader that is reading packets from the given input
|
647
|
+
*
|
648
|
+
* @param input input stream to read from
|
649
|
+
*/
|
650
|
+
function createMavLinkStream(input, onCrcError) {
|
651
|
+
return input
|
652
|
+
.pipe(new MavLinkPacketSplitter({}, onCrcError))
|
653
|
+
.pipe(new MavLinkPacketParser());
|
654
|
+
}
|
655
|
+
exports.createMavLinkStream = createMavLinkStream;
|
656
|
+
let seq = 0;
|
657
|
+
/**
|
658
|
+
* Send a packet to the stream
|
659
|
+
*
|
660
|
+
* @param stream Stream to send the data to
|
661
|
+
* @param msg message to serialize and send
|
662
|
+
* @param protocol protocol to use (default: MavLinkProtocolV1)
|
663
|
+
* @returns number of bytes sent
|
664
|
+
*/
|
665
|
+
async function send(stream, msg, protocol = new MavLinkProtocolV1()) {
|
666
|
+
return new Promise((resolve, reject) => {
|
667
|
+
const buffer = protocol.serialize(msg, seq++);
|
668
|
+
seq &= 255;
|
669
|
+
stream.write(buffer, err => {
|
670
|
+
if (err)
|
671
|
+
reject(err);
|
672
|
+
else
|
673
|
+
resolve(buffer.length);
|
674
|
+
});
|
675
|
+
});
|
676
|
+
}
|
677
|
+
exports.send = send;
|
678
|
+
/**
|
679
|
+
* Send a signed packet to the stream. Signed packets are always V2 protocol
|
680
|
+
*
|
681
|
+
* @param stream Stream to send the data to
|
682
|
+
* @param msg message to serialize and send
|
683
|
+
* @param key key to sign the message with
|
684
|
+
* @param linkId link id for the signature
|
685
|
+
* @param sysid system id
|
686
|
+
* @param compid component id
|
687
|
+
* @param timestamp optional timestamp for packet signing (default: Date.now())
|
688
|
+
* @returns number of bytes sent
|
689
|
+
*/
|
690
|
+
async function sendSigned(stream, msg, key, linkId = 1, sysid = MavLinkProtocol.SYS_ID, compid = MavLinkProtocol.COMP_ID, timestamp = Date.now()) {
|
691
|
+
return new Promise((resolve, reject) => {
|
692
|
+
const protocol = new MavLinkProtocolV2(sysid, compid, MavLinkProtocolV2.IFLAG_SIGNED);
|
693
|
+
const b1 = protocol.serialize(msg, seq++);
|
694
|
+
seq &= 255;
|
695
|
+
const b2 = protocol.sign(b1, linkId, key, timestamp);
|
696
|
+
stream.write(b2, err => {
|
697
|
+
if (err)
|
698
|
+
reject(err);
|
699
|
+
else
|
700
|
+
resolve(b2.length);
|
701
|
+
});
|
702
|
+
});
|
703
|
+
}
|
704
|
+
exports.sendSigned = sendSigned;
|