node-mavlink 1.1.1 → 1.3.0

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