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.
- 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 +16 -9
- 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/examples/parse-tlog-file.ts +43 -0
- package/examples/send-receive-file.ts +6 -2
- package/examples/vtol.tlog +0 -0
- package/package.json +11 -9
- package/sanity-check.cjs +8 -0
- package/sanity-check.mjs +8 -0
- package/.vscode/launch.json +0 -19
- package/.vscode/settings.json +0 -3
- package/coverage/coverage-final.json +0 -1
- package/coverage/lcov-report/base.css +0 -224
- package/coverage/lcov-report/block-navigation.js +0 -87
- package/coverage/lcov-report/favicon.png +0 -0
- package/coverage/lcov-report/index.html +0 -101
- package/coverage/lcov-report/prettify.css +0 -1
- package/coverage/lcov-report/prettify.js +0 -2
- package/coverage/lcov-report/serialization.ts.html +0 -613
- package/coverage/lcov-report/sort-arrow-sprite.png +0 -0
- package/coverage/lcov-report/sorter.js +0 -196
- package/coverage/lcov.info +0 -0
- 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 -796
- 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,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
|
-
}
|