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