├── .gitignore ├── .npmignore ├── .npmrc ├── .nvmrc ├── .vscode ├── launch.json └── settings.json ├── LICENSE ├── README.md ├── examples ├── GH-5.bin ├── mavlink-v2-3412-packets.bin ├── parse-tlog-file.ts ├── pool-param-udp.ts ├── send-receive-file.ts ├── send-receive-serial.ts ├── send-receive-signed-serial.ts ├── send-receive-signed-udp.ts ├── send-receive-udp.ts └── vtol.tlog ├── index.ts ├── jest.config.js ├── lib ├── logger.ts ├── magic-numbers.ts ├── mavesp.ts ├── mavlink.test.ts ├── mavlink.ts ├── serialization.test.ts ├── serialization.ts └── utils.ts ├── package-lock.json ├── package.json ├── sanity-check-minimal.cjs ├── sanity-check-minimal.mjs ├── sanity-check.cjs ├── sanity-check.mjs ├── tests ├── data.mavlink └── main.js └── tsconfig.json /.gitignore: -------------------------------------------------------------------------------- 1 | node_modules 2 | *.xml 3 | dist 4 | coverage 5 | -------------------------------------------------------------------------------- /.npmignore: -------------------------------------------------------------------------------- 1 | node_modules 2 | .vscode 3 | *.xml 4 | *.test.ts 5 | code-generator.ts 6 | lib 7 | !dist/lib 8 | jest.config.js 9 | *.tgz 10 | index.ts 11 | sanity-check.js 12 | tests 13 | tsconfig.json 14 | -------------------------------------------------------------------------------- /.npmrc: -------------------------------------------------------------------------------- 1 | tag-version-prefix="" 2 | message="Release %s" 3 | -------------------------------------------------------------------------------- /.nvmrc: -------------------------------------------------------------------------------- 1 | 14 2 | -------------------------------------------------------------------------------- /.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | "version": "0.2.0", 3 | "configurations": [ 4 | { 5 | "type": "node", 6 | "request": "launch", 7 | "name": "Main tests", 8 | "runtimeArgs": [ 9 | "-r", 10 | "ts-node/register" 11 | ], 12 | "args": [ 13 | "${workspaceFolder}/tests/main.ts", 14 | "e2e", 15 | "--input=tests/data.mavlink" 16 | ] 17 | }, 18 | { 19 | "type": "node", 20 | "request": "launch", 21 | "name": "Sanity tests", 22 | "args": [ 23 | "${workspaceFolder}/sanity-check.mjs", 24 | ] 25 | } 26 | ], 27 | } -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "jest.jestCommandLine": "npx jest", 3 | "cSpell.words": [ 4 | "Ardu", 5 | "Ardupilot", 6 | "ardupilotmega", 7 | "asluav", 8 | "autotest", 9 | "baudrate", 10 | "baudrates", 11 | "clazz", 12 | "codepaths", 13 | "compid", 14 | "DESERIALIZERS", 15 | "emirkartal", 16 | "icarous", 17 | "IFLAG", 18 | "linkid", 19 | "MAVESP", 20 | "mavlink", 21 | "msgid", 22 | "plen", 23 | "pymavlink", 24 | "Statustext", 25 | "stxv", 26 | "sysid", 27 | "tlog", 28 | "ualberta", 29 | "uavionix", 30 | "udpin" 31 | ] 32 | } -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | If you're looking for the officially supported bindings for JavaScript see the [pymavlink](https://github.com/ArduPilot/pymavlink/tree/master/generator/javascript) project. 2 | 3 | # Node.js MavLink library 4 | 5 | This package is the implementation of serialization and parsing of MavLink messages for v1 and v2 protocols. 6 | 7 | It consists partially of code generated from the XML documents in the original MavLink repository and a few pieces that define the parser and serializer. 8 | 9 | ## Getting started 10 | 11 | It is extremely easy to get started using this library. First you need to install the required packages. For demonstration purposes we'll be using the `serialport` package to read the data from serial port. 12 | 13 | ### Reading messages 14 | 15 | ``` 16 | $ npm install --save node-mavlink serialport 17 | ``` 18 | 19 | Once you've done it you can start using it. First you'll need a serial port that can parse messages one by one. Please note that since we're using ECMAScript modules the file name should end with `.mjs` extension (e.g. `test.mjs`) 20 | 21 | ```javascript 22 | import { SerialPort } from 'serialport' 23 | import { MavLinkPacketSplitter, MavLinkPacketParser } from 'node-mavlink' 24 | 25 | // substitute /dev/ttyACM0 with your serial port! 26 | const port = new SerialPort({ path: '/dev/ttyACM0', baudRate: 115200 }) 27 | 28 | // constructing a reader that will emit each packet separately 29 | const reader = port 30 | .pipe(new MavLinkPacketSplitter()) 31 | .pipe(new MavLinkPacketParser()) 32 | 33 | reader.on('data', packet => { 34 | console.log(packet) 35 | }) 36 | ``` 37 | 38 | That's it! That is all it takes to read the raw data. But it doesn't end there - in fact this is only the beginning of what this library can do for you. 39 | 40 | Each message consists of multiple fields that contain specific data. Parsing the data is also very easy. 41 | 42 | ```javascript 43 | import { 44 | MavLinkPacketRegistry, 45 | minimal, common, ardupilotmega 46 | } from 'node-mavlink' 47 | 48 | // create a registry of mappings between a message id and a data class 49 | const REGISTRY: MavLinkPacketRegistry = { 50 | ...minimal.REGISTRY, 51 | ...common.REGISTRY, 52 | ...ardupilotmega.REGISTRY, 53 | } 54 | 55 | reader.on('data', packet => { 56 | const clazz = REGISTRY[packet.header.msgid] 57 | if (clazz) { 58 | const data = packet.protocol.data(packet.payload, clazz) 59 | console.log('Received packet:', data) 60 | } 61 | }) 62 | ``` 63 | 64 | ### Sending messages 65 | 66 | Sending messages is also very easy. One example that is very useful is to send the `REQUEST_PROTOCOL_VERSION` to switch to protocol version 2. 67 | 68 | ```javascript 69 | import { MavLinkProtocolV2, send } from 'node-mavlink' 70 | 71 | // Create an instance of of the `RequestProtocolVersionCommand` 72 | // class that will be the vessel for containing the command data. 73 | // Underneath the cover it uses CommandLong to convert the data. 74 | // 75 | // By convention the intermediate fields that are then serialized 76 | // are named with `_` (underscore) prefix and should not be used 77 | // directly. That doesn't mean you can't use them, but if there 78 | // is an equivalent Command class it is just a lot easier and every 79 | // parameter not only has a more descriptive names but also in-line 80 | // documentation. 81 | const command = new common.RequestProtocolVersionCommand() 82 | command.confirmation = 1 83 | 84 | port.on('open', async () => { 85 | // the port is open - we're ready to send data 86 | await send(port, command, new MavLinkProtocolV2()) 87 | }) 88 | ``` 89 | 90 | ## Interacting with other communication mediums 91 | 92 | The splitter and parser work with generic streams. Of course the obvious choice for many use cases will be a serial port but the support doesn't end there. 93 | 94 | There are options for streams working over network (TCP or UDP), GSM network - pretty much anything that sends and receives data over Node.js `Stream`s. 95 | 96 | Here's an example for connecting to telemetry via TCP (for example using [esp-link](https://github.com/jeelabs/esp-link) and a cheap ESP8266 module) 97 | 98 | ```javascript 99 | import { connect } from 'net' 100 | 101 | // substitute 192.168.4.1 with the IP address of your module 102 | const port = connect({ host: '192.168.4.1', port: 2323 }) 103 | 104 | port.on('connect', () => { 105 | console.log('Connected!') 106 | // here you can start sending commands 107 | }) 108 | ``` 109 | 110 | The rest is exactly the same. The TCP connection also is a stream so piping the data through the `MavLinkPacketSplitter` and `MavLinkPacketParser` works as expected. 111 | 112 | ### A short note to my future self and others about baudrates 113 | 114 | The default serial port speed for telemetry in Ardupilot is 57600 bauds. This means that in the user interface of the esp-link (accessible via a web page under the same IP address) you need to make sure the speed is properly set on the _µC Console_ tab. Just select the proper baudrate from the dropdown at the top of the page and you'll be all set. No reboot of the module required! 115 | 116 | ### Using MAVESP8266 in UDP mode 117 | 118 | The _official_ firmware for setting up a UDP telemetry using ESP8266 is [MAVESP8266](https://ardupilot.org/copter/docs/common-esp8266-telemetry.html). This firmware exposes messages over UDP rather than TCP but has other advantages (see the documentation). 119 | 120 | To setup a stream that reads from a UDP socket isn't as easy as with TCP sockets (which are in a sense streams on their own) but is not hard at all because the library exposes the `MavEsp8266` class that encapsulates all of the hard work for you: 121 | 122 | ```javascript 123 | import { MavEsp8266, common } from 'node-mavlink' 124 | 125 | async function main() { 126 | const port = new MavEsp8266() 127 | 128 | // start the communication 129 | await port.start() 130 | 131 | // log incoming packets 132 | port.on('data', packet => { 133 | console.log(packet.debug()) 134 | }) 135 | 136 | // You're now ready to send messages to the controller using the socket 137 | // let's request the list of parameters 138 | const message = new common.ParamRequestList() 139 | message.targetSystem = 1 140 | message.targetComponent = 1 141 | 142 | // The `send` method is another utility method, very handy to have it provided 143 | // by the library. It takes care of the sequence number and data serialization. 144 | await port.send(message) 145 | } 146 | 147 | main() 148 | ``` 149 | 150 | That's it! Easy as a lion :) 151 | 152 | ## Signed packages 153 | 154 | MavLink v2 introduces package signing. The way it currently works with Mission planner is you give it a pass phrase, Mission Planner encodes it using sha256 hashing algorithm and uses it as part of the signature calculation. Therefore if someone does not know the secret passphrase they won't be able to create packets that would seem to be coming from a source. It's a kind of security thing. 155 | 156 | ### Reading signature 157 | 158 | The `node-mavlink` library introduced signature parsing in version 0.0.1-beta.10. The way to verify if a package can be trusted is as follows: 159 | 160 | ```javascript 161 | import { MavLinkPacketSignature } from 'node-mavlink' 162 | 163 | // calculate secret key (change 'qwerty' to your secret phrase) 164 | const key = MavLinkPacketSignature.key('qwerty') 165 | 166 | // log incoming messages 167 | port.on('data', packet => { 168 | console.log(packet.debug()) 169 | if (packet.signature) { 170 | if (packet.signature.matches(key)) { 171 | // signature valid 172 | } else { 173 | // signature not valid - possible fraud package detected 174 | } 175 | } else { 176 | // packet is not signed 177 | } 178 | }) 179 | ``` 180 | 181 | What you do with that information is up to you. You can continue to process that package or you can drop it. The library imposes no restriction on packets with invalid signatures. 182 | 183 | ### Sending signed packages 184 | 185 | First we need to learn how to create a secure key. As mentioned before the key in Mission Planner is created by calculating an SHA256 checksum over a secret phrase that you can specify and then taking the first 6 bytes of it. 186 | 187 | To do the same using this library: 188 | 189 | ```javascript 190 | import { MavLinkPacketSignature } from 'node-mavlink' 191 | 192 | const key = MavLinkPacketSignature.key('your very secret passphrase') 193 | ``` 194 | 195 | Now that we have the key ready we can send signed packages. Let's use the `ParamRequestList` as an example: 196 | 197 | ```javascript 198 | import { common, sendSigned } from 'node-mavlink' 199 | 200 | async function requestParameterList() { 201 | const message = new common.ParamRequestList() 202 | message.targetSystem = 1 203 | message.targetComponent = 1 204 | 205 | await sendSigned(port, message, key) 206 | } 207 | ``` 208 | 209 | If you're using the `MavEsp8266` class for communicating over UDP it also exposes the `sendSigned()` method with the same signature. 210 | 211 | ## Utility functions 212 | 213 | The library exposes a few utility functions that make the life easier when writing application code 214 | 215 | #### `async waitFor(cb: Function, timeout: number, interval: number)` 216 | 217 | This function calls the `cb` callback periodically at the `interval` (default: 100ms) and if it returns a `truthy` value it will stop polling. If, however, the value is `falsy` for a longer period of time than the `timeout` (default: 10000ms) then it will throw a `Timeout` error. 218 | 219 | #### `async send(stream: Writable, msg: MavLinkData, protocol: MavLinkProtocol)` 220 | 221 | This function serializes the `msg` message using the provided `protocol` (default: `MavLinkProtocolV1`) and sends it to the `stream`. If the process is successful the method returns with the length of written data denoting that no error occurred. However, if the process was not successful it will error out with the underlying error object returned on by the stream. 222 | 223 | #### `async sendSigned(stream: Writable, msg: MavLinkData, key: Buffer, linkId: uint8_t, sysid: uint8_t, compid: uint8_t)` 224 | 225 | This is a similar function to `send` but does so using MavLink v2 protocol and signs the message. 226 | 227 | The default values for some parameters are as follows: 228 | - `linkId` = 1 229 | - `sysid` = 254 230 | - `compid` = 1 231 | 232 | #### `async sleep(ms: number)` 233 | 234 | This is a very handy utility function that asynchronously pauses for a given time (ms). 235 | 236 | ## Running sim_vehicle.py 237 | 238 | The easiest way to start playing around with this package is to use `sim_vehicle.py`. You can use the default parameters for the MavEsp8266 if you'll make the simulator compatible with it: 239 | 240 | ``` 241 | $ Tools/autotest/sim_vehicle.py -v ArduCopter -f quad --console --map --out udpin:127.0.0.1:14555 242 | ``` 243 | 244 | That last parameter (`--out udpin:127.0.0.1:14555`) opens up for incoming messages in port 14555, which is the default send port for MavEsp8266 and its default firmware. 245 | 246 | ## Registering custom messages 247 | 248 | There are times when you want to have custom messages, for example when you're building a rocket and there is no target you can use out of the box. There are actually two scenarios: 249 | 250 | 1. You have a few custom messages, but generally you're happy with the original set of messages 251 | 2. You don't care about the original messages, maybe you do about the heartbeat, but nothing else 252 | 253 | ### Registering a single command 254 | 255 | There are 3 steps to register a custom command: 256 | 257 | a) create a class that defines your custom command 258 | b) add it to your `REGISTRY` 259 | c) register your custom command's magic number 260 | 261 | The first two steps are pretty self explanatory and there is a plethora of examples in the mavlink-mappings project - use those to learn how to create your own message definitions. 262 | 263 | The last step is quite easy: 264 | 265 | ```javascript 266 | import { registerCustomMessageMagicNumber } from 'node-mavlink' 267 | 268 | registerCustomMessageMagicNumber('999999', 42) 269 | ``` 270 | 271 | From now on the splitter will know how to properly calculate CRC for your packages and you're all good. 272 | 273 | ### Replacing magic numbers registry all together 274 | 275 | Well, if all you care about is the ping, why parse anything else, right? And if on top of the ping command you've got a number of custom messages - all the better to not parse even the messages! 276 | 277 | ```javascript 278 | import { MavLinkSplitter, MavLinkParser } from 'node-mavlink' 279 | 280 | const MY_MAGIC_NUMBERS = { 281 | '0': 50, // Heartbeat 282 | // ...other magic number definitions go here 283 | } 284 | 285 | const source = ... // obtain source stream 286 | const reader = source 287 | .pipe(new MavLinkPacketSplitter({}, { magicNumbers: MY_MAGIC_NUMBERS })) 288 | .pipe(new MavLinkPacketParser()) 289 | ``` 290 | 291 | ## Closing thoughts 292 | 293 | The original generated sources lack one very important aspect of a reusable library: documentation. Also, most of the time the names are more C-like than JavaScript-like. 294 | 295 | When generating sources for data classes a number of things happen: 296 | 297 | - `enum` values are trimmed from common prefix; duplicating enum name in its value when its value cannot be spelled without the enum name is pointless and leads to unnecessarily verbose code 298 | - enum values, whenever available, contain JSDoc describing their purpose 299 | - data classes are properly named (PascalCase) 300 | - data classes fields are properly named (camelCase) 301 | - both data classes and their fields whenever available also contain JSDoc 302 | - if a particular data class is deprecated that information is also available in the JSDoc 303 | - some class names are modified compared to their original values (e.g. `Statustext` => `StatusText`) 304 | 305 | This leads to generated code that contains not only raw types but also documentation where it is mostly useful: right at your fingertips. 306 | 307 | I hope you'll enjoy using this library! If you have any comments, find a bug or just generally want to share your thoughts you can reach me via email: padcom@gmail.com 308 | 309 | Peace! 310 | -------------------------------------------------------------------------------- /examples/GH-5.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ArduPilot/node-mavlink/52735f2f4d3945a983dd370ccc8e209c18ea0397/examples/GH-5.bin -------------------------------------------------------------------------------- /examples/mavlink-v2-3412-packets.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ArduPilot/node-mavlink/52735f2f4d3945a983dd370ccc8e209c18ea0397/examples/mavlink-v2-3412-packets.bin -------------------------------------------------------------------------------- /examples/parse-tlog-file.ts: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env -S npx ts-node 2 | 3 | import { createReadStream } from 'fs' 4 | import { Readable } from 'stream' 5 | import { MavLinkTLogPacketSplitter, MavLinkPacketParser } from '..' 6 | import { 7 | minimal, common, ardupilotmega, uavionix, icarous, 8 | asluav, development, ualberta, 9 | } from '..' 10 | 11 | const splitter = new MavLinkTLogPacketSplitter() 12 | const parser = new MavLinkPacketParser() 13 | // const file = createReadStream(__dirname + '/vtol.tlog') 14 | 15 | // Example "dirty" stream containing a date that has the 0xFD protocol magic number in it 16 | // and starts with 1 byte of garbage. Courtesy of emirkartal0 - thank you! 17 | 18 | // @ts-ignore 19 | const data = Buffer.from("b3 20 05 f1 fd 7a 1a c9 48 fd 10 20 20 eb 01 01 1e 20 20 20 20 20 20 95 7e 10 3d 0a 92 90 bb 88 f1 09 be d2 49 20 05 f1 fd 7a 1a f8 28 fd 19 20 20 ec 01 01 21 20 20 20 20 20 20 5c 96 f3 17 d2 29 dd 13 36 d8 0f 20 ee ff ff ff 20 20 20 20 03 6e f0 20 05 f1 fd 7a 1b f2 28 fd 1e 20 20 ed 01 01 18 20 20 20 20 20 20 20 20 20 20 20 20 20 20 20 20 20 20 20 20 20 20 58 20 9a 20 20 20 d3 40 05 0b 66 39".replaceAll(' ',''),"hex") 20 | const file = Readable.from(data) 21 | 22 | const reader = file.pipe(splitter).pipe(parser) 23 | 24 | // create a registry of mappings between a message id and a data class 25 | const REGISTRY = { 26 | ...minimal.REGISTRY, 27 | ...common.REGISTRY, 28 | ...ardupilotmega.REGISTRY, 29 | ...uavionix.REGISTRY, 30 | ...icarous.REGISTRY, 31 | ...asluav.REGISTRY, 32 | ...development.REGISTRY, 33 | ...ualberta.REGISTRY, 34 | } 35 | 36 | reader.on('data', packet => { 37 | const clazz = REGISTRY[packet.header.msgid] 38 | if (clazz) { 39 | const data = packet.protocol.data(packet.payload, clazz) 40 | if (packet.header.timestamp) { 41 | console.log(new Date(Number(packet.header.timestamp)).toISOString(), data) 42 | } else { 43 | console.log(data) 44 | } 45 | } 46 | }) 47 | 48 | file.on('close', () => { 49 | console.log('\n\nNumber of invalid packages:', splitter.invalidPackages) 50 | console.log('Number of unknown packages:', splitter.unknownPackagesCount) 51 | console.log('\nTotal number of consumed packets:', splitter.validPackages) 52 | }) 53 | -------------------------------------------------------------------------------- /examples/pool-param-udp.ts: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env -S npx ts-node 2 | 3 | import { MavEsp8266, common, sleep } from '..' 4 | import { MavLinkPacket } from '..' 5 | 6 | async function main() { 7 | const port = new MavEsp8266() 8 | 9 | // start the communication 10 | // After this line we have received at least one heartbeat message so we 11 | // know what is the remote IP address to send the messages to 12 | const { ip, sendPort, receivePort } = await port.start() 13 | console.log(`Connected to: ${ip}, send port: ${sendPort}, receive port ${receivePort}`) 14 | 15 | // log incoming messages 16 | port.on('data', (packet: MavLinkPacket) => { 17 | if (packet.header.msgid === common.ParamValue.MSG_ID) { 18 | const value = packet.protocol.data(packet.payload, common.ParamValue) 19 | if (value.paramId.startsWith('ATC_RAT_')) { 20 | console.log(value) 21 | } 22 | } 23 | }) 24 | 25 | // You're now ready to send messages to the controller using the socket 26 | // let's request the list of parameters 27 | 28 | await sleep(1000) 29 | 30 | setInterval(() => { 31 | const message = new common.ParamRequestRead() 32 | message.paramIndex = 65535 33 | message.paramId = 'ATC_RAT_RLL_P' 34 | message.targetSystem = 1 35 | message.targetComponent = 1 36 | port.send(message) 37 | }, 200) 38 | 39 | await sleep(100) 40 | 41 | setInterval(() => { 42 | const message = new common.ParamRequestRead() 43 | message.paramIndex = 65535 44 | message.paramId = 'ATC_RAT_PIT_P' 45 | message.targetSystem = 1 46 | message.targetComponent = 1 47 | port.send(message) 48 | }, 200) 49 | } 50 | 51 | main() 52 | -------------------------------------------------------------------------------- /examples/send-receive-file.ts: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env -S npx ts-node 2 | 3 | import { createReadStream } from 'fs' 4 | import { MavLinkPacketSplitter, MavLinkPacketParser } from '..' 5 | import { 6 | minimal, common, ardupilotmega, uavionix, icarous, 7 | asluav, development, ualberta, 8 | } from '..' 9 | 10 | // 11 | // Example how to register your own magic numbers: 12 | // 13 | // const splitter = new MavLinkPacketSplitter({}, { magicNumbers: { '0': 50 } }) 14 | // 15 | const splitter = new MavLinkPacketSplitter() 16 | const parser = new MavLinkPacketParser() 17 | const file = createReadStream(__dirname + '/GH-5.bin') 18 | const reader = file.pipe(splitter).pipe(parser) 19 | 20 | // create a registry of mappings between a message id and a data class 21 | const REGISTRY = { 22 | ...minimal.REGISTRY, 23 | ...common.REGISTRY, 24 | ...ardupilotmega.REGISTRY, 25 | ...uavionix.REGISTRY, 26 | ...icarous.REGISTRY, 27 | ...asluav.REGISTRY, 28 | ...development.REGISTRY, 29 | ...ualberta.REGISTRY, 30 | } 31 | 32 | reader.on('data', packet => { 33 | console.log(packet.debug()) 34 | 35 | const clazz = REGISTRY[packet.header.msgid] 36 | if (clazz) { 37 | const data = packet.protocol.data(packet.payload, clazz) 38 | if (packet.header.timestamp) { 39 | console.log(new Date(Number(packet.header.timestamp)).toISOString(), data) 40 | } else { 41 | console.log(data) 42 | } 43 | } 44 | }) 45 | 46 | file.on('close', () => { 47 | console.log('\n\nNumber of invalid packages:', splitter.invalidPackages) 48 | console.log('Number of unknown packages:', splitter.unknownPackagesCount) 49 | console.log('\nTotal number of consumed packets:', splitter.validPackages) 50 | }) 51 | -------------------------------------------------------------------------------- /examples/send-receive-serial.ts: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env -S npx ts-node 2 | 3 | import { SerialPort } from 'serialport' 4 | import { MavLinkPacketRegistry, MavLinkPacketSplitter, MavLinkPacketParser, MavLinkPacket } from '..' 5 | import { minimal, common, ardupilotmega, waitFor, send } from '..' 6 | 7 | const REGISTRY: MavLinkPacketRegistry = { 8 | ...minimal.REGISTRY, 9 | ...common.REGISTRY, 10 | ...ardupilotmega.REGISTRY, 11 | } 12 | 13 | async function main() { 14 | // Create an output stream to write data to the controller 15 | const port = new SerialPort({ path: '/dev/ttyACM0', baudRate: 115200 }) 16 | 17 | // Create the reader as usual by piping the source stream through the splitter 18 | // and packet parser 19 | const reader = port 20 | .pipe(new MavLinkPacketSplitter()) 21 | .pipe(new MavLinkPacketParser()) 22 | 23 | // A flag that determines if the remote system is alive 24 | let online = false 25 | 26 | // React to packet being retrieved. 27 | // This is the place where all your application-level logic will exist 28 | reader.on('data', (packet: MavLinkPacket) => { 29 | // Output generic debug information about this packet 30 | console.log(packet.debug()) 31 | 32 | online = true 33 | const clazz = REGISTRY[packet.header.msgid] 34 | if (clazz) { 35 | const data = packet.protocol.data(packet.payload, clazz) 36 | console.log('>', data) 37 | } else { 38 | console.log('!', packet.debug()) 39 | } 40 | }) 41 | 42 | // Wait for the remote system to be available 43 | await waitFor(() => online) 44 | 45 | // You're now ready to send messages to the controller using the socket 46 | // let's request the list of parameters 47 | const message = new common.ParamRequestList() 48 | message.targetSystem = 1 49 | message.targetComponent = 1 50 | 51 | // The default protocol (last parameter, absent here) is v1 which is 52 | // good enough for testing. You can instantiate any other protocol and pass it 53 | // on to the `send` method. 54 | // The send method is another utility method, very handy to have it provided 55 | // by the library. It takes care of the sequence number and data serialization. 56 | await send(port, message) 57 | } 58 | 59 | main() 60 | -------------------------------------------------------------------------------- /examples/send-receive-signed-serial.ts: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env -S npx ts-node 2 | 3 | import { SerialPort } from 'serialport' 4 | import { MavLinkPacketSplitter, MavLinkPacketParser } from '..' 5 | import { MavLinkPacket, MavLinkPacketSignature, MavLinkPacketRegistry } from '..' 6 | import { minimal, common, ardupilotmega, waitFor, sendSigned } from '..' 7 | 8 | const REGISTRY: MavLinkPacketRegistry = { 9 | ...minimal.REGISTRY, 10 | ...common.REGISTRY, 11 | ...ardupilotmega.REGISTRY, 12 | } 13 | 14 | // Use your own secret passphrase in place of 'qwerty' 15 | const key = MavLinkPacketSignature.key('qwerty') 16 | 17 | async function main() { 18 | // Create an output stream to write data to the controller 19 | const port = new SerialPort({ path: '/dev/ttyACM0', baudRate: 115200 }) 20 | 21 | // Create the reader as usual by piping the source stream through the splitter 22 | // and packet parser 23 | const reader = port 24 | .pipe(new MavLinkPacketSplitter()) 25 | .pipe(new MavLinkPacketParser()) 26 | 27 | // A flag that determines if the remote system is alive 28 | let online = false 29 | 30 | // React to packet being retrieved. 31 | // This is the place where all your application-level logic will exist 32 | reader.on('data', (packet: MavLinkPacket) => { 33 | online = true 34 | if (packet.signature) { 35 | if (packet.signature.matches(key)) { 36 | console.log('Signature check OK') 37 | } else { 38 | console.log('Signature check FAILED - possible fraud package detected') 39 | } 40 | } else { 41 | console.log('Packet not signed') 42 | } 43 | const clazz = REGISTRY[packet.header.msgid] 44 | if (clazz) { 45 | const data = packet.protocol.data(packet.payload, clazz) 46 | console.log('>', data) 47 | } else { 48 | console.log('!', packet.debug()) 49 | } 50 | }) 51 | 52 | // Wait for the remote system to be available 53 | await waitFor(() => online) 54 | 55 | // You're now ready to send messages to the controller using the socket 56 | // let's request the list of parameters 57 | const message = new common.ParamRequestList() 58 | message.targetSystem = 1 59 | message.targetComponent = 1 60 | 61 | // The `sendSigned` method is another utility method, very handy to have it 62 | // provided by the library. It takes care of the sequence number and data 63 | // serialization. 64 | await sendSigned(port, message, key) 65 | } 66 | 67 | main() 68 | -------------------------------------------------------------------------------- /examples/send-receive-signed-udp.ts: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env -S npx ts-node 2 | 3 | import { MavEsp8266, minimal, common, ardupilotmega } from '..' 4 | import { MavLinkPacketSignature, MavLinkPacket, MavLinkPacketRegistry } from '..' 5 | 6 | const REGISTRY: MavLinkPacketRegistry = { 7 | ...minimal.REGISTRY, 8 | ...common.REGISTRY, 9 | ...ardupilotmega.REGISTRY, 10 | } 11 | 12 | // Use your own secret passphrase in place of 'qwerty' 13 | const key = MavLinkPacketSignature.key('qwerty') 14 | 15 | async function main() { 16 | const port = new MavEsp8266() 17 | 18 | // start the communication 19 | const { ip, sendPort, receivePort } = await port.start() 20 | console.log(`Connected to: ${ip}, send port: ${sendPort}, receive port ${receivePort}`) 21 | 22 | // log incoming messages 23 | port.on('data', (packet: MavLinkPacket) => { 24 | if (packet.signature) { 25 | if (packet.signature.matches(key)) { 26 | console.log('Signature check OK') 27 | } else { 28 | console.log('Signature check FAILED - possible fraud package detected') 29 | } 30 | } else { 31 | console.log('Packet not signed') 32 | } 33 | const clazz = REGISTRY[packet.header.msgid] 34 | if (clazz) { 35 | const data = packet.protocol.data(packet.payload, clazz) 36 | console.log('>', data) 37 | } else { 38 | console.log('!', packet.debug()) 39 | } 40 | }) 41 | 42 | // You're now ready to send messages to the controller using the socket 43 | // let's request the list of parameters 44 | const message = new common.ParamRequestList() 45 | message.targetSystem = 1 46 | message.targetComponent = 1 47 | 48 | // The send method is another utility method, very handy to have it provided 49 | // by the library. It takes care of the sequence number and data serialization. 50 | await port.sendSigned(message, key) 51 | } 52 | 53 | main() 54 | -------------------------------------------------------------------------------- /examples/send-receive-udp.ts: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env -S npx ts-node 2 | 3 | import { MavEsp8266, minimal, common, ardupilotmega } from '..' 4 | import { MavLinkPacket, MavLinkPacketRegistry } from '..' 5 | 6 | const REGISTRY: MavLinkPacketRegistry = { 7 | ...minimal.REGISTRY, 8 | ...common.REGISTRY, 9 | ...ardupilotmega.REGISTRY, 10 | } 11 | 12 | // This is how you could build a registry of all commands from different packages 13 | const COMMANDS: common.MavLinkCommandRegistry = { 14 | ...common.COMMANDS, 15 | ...ardupilotmega.COMMANDS, 16 | } 17 | 18 | async function main() { 19 | const port = new MavEsp8266() 20 | 21 | // start the communication 22 | // After this line we have received at least one heartbeat message so we 23 | // know what is the remote IP address to send the messages to 24 | const { ip, sendPort, receivePort } = await port.start() 25 | console.log(`Connected to: ${ip}, send port: ${sendPort}, receive port ${receivePort}`) 26 | 27 | // log incoming messages 28 | port.on('data', (packet: MavLinkPacket) => { 29 | const clazz = REGISTRY[packet.header.msgid] 30 | if (clazz) { 31 | if (packet.header.msgid === common.CommandAck.MSG_ID) { 32 | const data = packet.protocol.data(packet.payload, clazz) 33 | console.log('>', data) 34 | } 35 | } else { 36 | console.log('!', packet.debug()) 37 | } 38 | }) 39 | 40 | // Create an instance of of the `RequestProtocolVersionCommand` 41 | // class that will be the vessel for containing the command data. 42 | // Underneath the cover it uses CommandLong to convert the data. 43 | // 44 | // By convention the intermediate fields that are then serialized 45 | // are named with `_` (underscore) prefix and should not be used 46 | // directly. That doesn't mean you can't use them, but if there 47 | // is an equivalent Command class it is just a lot easier and every 48 | // parameter not only has a more descriptive names but also in-line 49 | // documentation. 50 | const command = new common.RequestProtocolVersionCommand() 51 | command.confirmation = 1 52 | 53 | await port.send(command) 54 | 55 | // Give the system time to process any incoming acknowledges 56 | const sleep = (ms: number) => new Promise(resolve => setTimeout(resolve, ms)) 57 | await sleep(1000) 58 | 59 | // Close communication 60 | port.close() 61 | } 62 | 63 | main() 64 | -------------------------------------------------------------------------------- /examples/vtol.tlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ArduPilot/node-mavlink/52735f2f4d3945a983dd370ccc8e209c18ea0397/examples/vtol.tlog -------------------------------------------------------------------------------- /index.ts: -------------------------------------------------------------------------------- 1 | export * from 'mavlink-mappings' 2 | export * from './lib/magic-numbers' 3 | export * from './lib/utils' 4 | export * from './lib/logger' 5 | export * from './lib/mavlink' 6 | export * from './lib/mavesp' 7 | -------------------------------------------------------------------------------- /jest.config.js: -------------------------------------------------------------------------------- 1 | /** @type {import('ts-jest/dist/types').InitialOptionsTsJest} */ 2 | module.exports = { 3 | preset: 'ts-jest', 4 | testEnvironment: 'node', 5 | } 6 | -------------------------------------------------------------------------------- /lib/logger.ts: -------------------------------------------------------------------------------- 1 | import { EventEmitter } from 'events' 2 | 3 | /** 4 | * Level of the log entry 5 | */ 6 | export enum LogLevel { 7 | trace = 5, 8 | debug = 4, 9 | info = 3, 10 | warn = 2, 11 | error = 1, 12 | fatal = 0, 13 | } 14 | 15 | type LoggerRegistry = { [x: string]: Logger } 16 | type LoggerEvents = 'log' 17 | type LoggerEventHandler = (context: string, level: LogLevel, message: any[]) => void 18 | 19 | /** 20 | * Simplified interface for logging facilities 21 | */ 22 | export class Logger { 23 | private static readonly events: EventEmitter = new EventEmitter() 24 | private static registry: LoggerRegistry = {} 25 | 26 | /** 27 | * Gets a logger by name 28 | * 29 | * @param context logger context 30 | */ 31 | static getLogger(context: any) { 32 | let name = '' 33 | if (typeof context === 'function') name = context.name 34 | else if (typeof context === 'object') name = (context).constructor.name 35 | else if (typeof context === 'string') name = context 36 | else throw new Error(`Do not know how to get logger for ${context} (${typeof context})`) 37 | 38 | if (!Logger.registry[name]) Logger.registry[name] = new Logger(name) 39 | 40 | return Logger.registry[name] 41 | } 42 | 43 | /** 44 | * Binds an event handler 45 | * 46 | * @param event event to react to 47 | * @param handler event handler 48 | */ 49 | static on(event: LoggerEvents, handler: (context: any, level: LogLevel, message: string) => void) { 50 | this.events.on(event, handler) 51 | } 52 | 53 | /** 54 | * Removes an event handler 55 | * 56 | * @param event event to react to 57 | * @param handler event handler 58 | */ 59 | static off(event: LoggerEvents, handler: LoggerEventHandler) { 60 | this.events.off(event, handler) 61 | } 62 | 63 | private context: string 64 | 65 | /** 66 | * Constructs a new logger instance 67 | * 68 | * @param context logger context 69 | */ 70 | constructor(context: string) { 71 | this.context = context 72 | Logger.events.emit('logger-created', Logger.registry[context]) 73 | } 74 | 75 | /** 76 | * Sends a log message if the trace level is enabled for this logger 77 | * 78 | * @param args parameters for the log entry 79 | */ 80 | trace(...args: any) { 81 | Logger.events.emit('log', { context: this.context, level: LogLevel.trace, message: args }) 82 | } 83 | 84 | /** 85 | * Sends a log message if the debug level is enabled for this logger 86 | * 87 | * @param args parameters for the log entry 88 | */ 89 | debug(...args: any) { 90 | Logger.events.emit('log', { context: this.context, level: LogLevel.debug, message: args }) 91 | } 92 | 93 | /** 94 | * Sends a log message if the info level is enabled for this logger 95 | * 96 | * @param args parameters for the log entry 97 | */ 98 | info(...args: any) { 99 | Logger.events.emit('log', { context: this.context, level: LogLevel.info, message: args }) 100 | } 101 | 102 | /** 103 | * Sends a log message if the warn level is enabled for this logger 104 | * 105 | * @param args parameters for the log entry 106 | */ 107 | warn(...args: any) { 108 | Logger.events.emit('log', { context: this.context, level: LogLevel.warn, message: args }) 109 | } 110 | 111 | /** 112 | * Sends a log message if the error level is enabled for this logger 113 | * 114 | * @param args parameters for the log entry 115 | */ 116 | error(...args: any) { 117 | Logger.events.emit('log', { context: this.context, level: LogLevel.error, message: args }) 118 | } 119 | 120 | /** 121 | * Sends a log message if the fatal level is enabled for this logger 122 | * 123 | * @param args parameters for the log entry 124 | */ 125 | fatal(...args: any) { 126 | Logger.events.emit('log', { context: this.context, level: LogLevel.fatal, message: args }) 127 | } 128 | } 129 | -------------------------------------------------------------------------------- /lib/magic-numbers.ts: -------------------------------------------------------------------------------- 1 | import { MSG_ID_MAGIC_NUMBER } from 'mavlink-mappings' 2 | 3 | export function registerCustomMessageMagicNumber(msgid: string, magic: number) { 4 | const numbers = MSG_ID_MAGIC_NUMBER as Record 5 | if (numbers[msgid] !== undefined) { 6 | throw new Error(`Magic number for message "${msgid}" already registered`) 7 | } else { 8 | numbers[msgid] = magic 9 | } 10 | } 11 | 12 | export function overrideMessageMagicNumber(msgid: string, magic: number) { 13 | const numbers = MSG_ID_MAGIC_NUMBER as Record 14 | if (numbers[msgid] === undefined) { 15 | throw new Error(`Magic number for message "${msgid}" not registered`) 16 | } else { 17 | numbers[msgid] = magic 18 | } 19 | } 20 | -------------------------------------------------------------------------------- /lib/mavesp.ts: -------------------------------------------------------------------------------- 1 | import { EventEmitter } from 'events' 2 | 3 | import { Socket, createSocket, RemoteInfo } from 'dgram' 4 | import { Writable, PassThrough } from 'stream' 5 | import { MavLinkPacketSplitter, MavLinkPacketParser, MavLinkPacketSignature } from './mavlink' 6 | import { MavLinkProtocol, MavLinkProtocolV2 } from './mavlink' 7 | import { waitFor } from './utils' 8 | import { uint8_t, MavLinkData } from 'mavlink-mappings' 9 | 10 | export interface ConnectionInfo { 11 | ip: string 12 | sendPort: number 13 | receivePort: number 14 | } 15 | 16 | /** 17 | * Encapsulation of communication with MavEsp8266 18 | */ 19 | export class MavEsp8266 extends EventEmitter { 20 | private input: Writable 21 | private socket?: Socket 22 | private ip: string = '' 23 | private sendPort: number = 14555 24 | private seq: number = 0 25 | 26 | /** 27 | * @param splitter packet splitter instance 28 | * @param parser packet parser instance 29 | */ 30 | constructor({ 31 | splitter = new MavLinkPacketSplitter(), 32 | parser = new MavLinkPacketParser(), 33 | } = {}) { 34 | super() 35 | 36 | this.input = new PassThrough() 37 | 38 | this.processIncomingUDPData = this.processIncomingUDPData.bind(this) 39 | this.processIncomingPacket = this.processIncomingPacket.bind(this) 40 | 41 | // Create the reader as usual by piping the source stream through the splitter 42 | // and packet parser 43 | const reader = this.input 44 | .pipe(splitter) 45 | .pipe(parser) 46 | 47 | reader.on('data', this.processIncomingPacket) 48 | } 49 | 50 | /** 51 | * Start communication with the controller via MAVESP8266 52 | * 53 | * @param receivePort port to receive messages on (default: 14550) 54 | * @param sendPort port to send messages to (default: 14555) 55 | * @param ip IP address to send to in case there is no broadcast (default: empty string) 56 | */ 57 | async start(receivePort: number = 14550, sendPort: number = 14555, ip: string = ''): Promise { 58 | this.sendPort = sendPort 59 | this.ip = ip; 60 | 61 | // Create a UDP socket 62 | this.socket = createSocket({ type: 'udp4', reuseAddr: true }) 63 | this.socket.on('message', this.processIncomingUDPData) 64 | 65 | // Start listening on the socket 66 | return new Promise((resolve, reject) => { 67 | this.socket?.bind(receivePort, () => { 68 | // Wait for the first package to be returned to read the ip address 69 | // of the controller 70 | waitFor(() => this.ip !== '') 71 | .then(() => { resolve({ ip: this.ip, sendPort, receivePort }) }) 72 | .catch(e => { reject(e) }) 73 | }) 74 | }) 75 | } 76 | 77 | /** 78 | * Closes the client stopping any message handlers 79 | */ 80 | async close(): Promise { 81 | if (!this.socket) throw new Error('Not connected') 82 | 83 | // Unregister event handlers 84 | this.socket.off('message', this.processIncomingUDPData) 85 | 86 | // Close the socket 87 | return new Promise(resolve => { 88 | this.socket?.close(resolve) 89 | }) 90 | } 91 | 92 | /** 93 | * Send a packet 94 | * 95 | * @param msg message to send 96 | * @param sysid system id 97 | * @param compid component id 98 | */ 99 | async send(msg: MavLinkData, sysid: uint8_t = MavLinkProtocol.SYS_ID, compid: uint8_t = MavLinkProtocol.COMP_ID): Promise { 100 | const protocol = new MavLinkProtocolV2(sysid, compid) 101 | const buffer = protocol.serialize(msg, this.seq++) 102 | this.seq &= 255 103 | return this.sendBuffer(buffer) 104 | } 105 | 106 | /** 107 | * Send a signed packet 108 | * 109 | * @param msg message to send 110 | * @param sysid system id 111 | * @param compid component id 112 | * @param linkId link id for the signature 113 | */ 114 | async sendSigned(msg: MavLinkData, key: Buffer, linkId: uint8_t = 1, sysid: uint8_t = MavLinkProtocol.SYS_ID, compid: uint8_t = MavLinkProtocol.COMP_ID): Promise { 115 | const protocol = new MavLinkProtocolV2(sysid, compid, MavLinkProtocolV2.IFLAG_SIGNED) 116 | const b1 = protocol.serialize(msg, this.seq++) 117 | this.seq &= 255 118 | const b2 = protocol.sign(b1, linkId, key) 119 | return this.sendBuffer(b2) 120 | } 121 | 122 | /** 123 | * Send raw data over the socket. Useful for custom implementation of data sending 124 | * 125 | * @param buffer buffer to send 126 | */ 127 | async sendBuffer(buffer: Buffer): Promise { 128 | return new Promise((resolve, reject) => { 129 | this.socket?.send(buffer, this.sendPort, this.ip, (err, bytes) => { 130 | if (err) reject(err) 131 | else resolve(bytes) 132 | }) 133 | }) 134 | } 135 | 136 | private processIncomingUDPData(buffer: Buffer, metadata: RemoteInfo) { 137 | // store the remote ip address 138 | if (this.ip === '') this.ip = metadata.address 139 | // pass on the data to the input stream 140 | this.input.write(buffer) 141 | } 142 | 143 | private processIncomingPacket(packet: any) { 144 | // let the user know we received the packet 145 | this.emit('data', packet) 146 | } 147 | } 148 | -------------------------------------------------------------------------------- /lib/mavlink.test.ts: -------------------------------------------------------------------------------- 1 | import { MavLinkData, uint16_t, uint64_t, uint8_t } from 'mavlink-mappings' 2 | import { MavLinkProtocolV2 } from './mavlink' 3 | import { MavLinkPacketField } from 'mavlink-mappings/dist/lib/mavlink' 4 | 5 | class SingleUint64FieldMessage extends MavLinkData { 6 | static MSG_ID = 0 7 | static MSG_NAME = 'TEST_MESSAGE' 8 | static PAYLOAD_LENGTH = 8 9 | static MAGIC_NUMBER = 0 10 | 11 | static FIELDS: MavLinkPacketField[] = [ 12 | new MavLinkPacketField('uint64field', 'uint64field', 0, false, 8, 'uint64_t', ''), 13 | ] 14 | 15 | constructor() { 16 | super() 17 | this.uint64field = BigInt(0) 18 | } 19 | 20 | uint64field: uint64_t 21 | } 22 | 23 | class TwoFieldMessage extends MavLinkData { 24 | static MSG_ID = 0 25 | static MSG_NAME = 'TEST_MESSAGE' 26 | static PAYLOAD_LENGTH = 9 27 | static MAGIC_NUMBER = 0 28 | 29 | static FIELDS: MavLinkPacketField[] = [ 30 | new MavLinkPacketField('uint8Field', 'uint8Field', 0, false, 1, 'uint8_t', ''), 31 | new MavLinkPacketField('uint64field', 'uint64field', 1, false, 8, 'uint64_t', ''), 32 | ] 33 | 34 | constructor() { 35 | super() 36 | this.uint8Field = 0 37 | this.uint64field = BigInt(0) 38 | } 39 | 40 | uint8Field: uint8_t 41 | uint64field: uint64_t 42 | } 43 | 44 | class ArrayFieldMessage extends MavLinkData { 45 | static MSG_ID = 0 46 | static MSG_NAME = 'TEST_MESSAGE' 47 | static PAYLOAD_LENGTH = 9 48 | static MAGIC_NUMBER = 0 49 | 50 | static FIELDS: MavLinkPacketField[] = [ 51 | new MavLinkPacketField('arrayField', 'arrayField', 0, false, 2, 'uint16_t[]', '', 2), 52 | ] 53 | 54 | constructor() { 55 | super() 56 | this.arrayField = [] 57 | } 58 | 59 | arrayField: uint16_t[] 60 | } 61 | 62 | describe('test parser MavLinkProtocolV2', () => { 63 | it('parses untrimmed buffer', () => { 64 | const mavlink = new MavLinkProtocolV2() 65 | const msg = mavlink.data(Buffer.from([ 0xde, 0xad, 0xbe, 0xef, 0x00, 0x00, 0x00, 0x00 ]), SingleUint64FieldMessage) 66 | expect(msg.uint64field).toBe(Buffer.from([ 0xde, 0xad, 0xbe, 0xef, 0x00, 0x00, 0x00, 0x00 ]).readBigUInt64LE()) 67 | }) 68 | it('parses trimmed buffer', () => { 69 | const mavlink = new MavLinkProtocolV2() 70 | const msg = mavlink.data(Buffer.from([ 0xde, 0xad, 0xbe, 0xef ]), SingleUint64FieldMessage) 71 | expect(msg.uint64field).toBe(Buffer.from([ 0xde, 0xad, 0xbe, 0xef, 0x00, 0x00, 0x00, 0x00 ]).readBigUInt64LE()) 72 | }) 73 | it('parses untrimmed two field buffer', () => { 74 | const mavlink = new MavLinkProtocolV2() 75 | const msg = mavlink.data(Buffer.from([ 0x09, 0xde, 0xad, 0xbe, 0xef, 0x00, 0x00, 0x01, 0x00 ]), TwoFieldMessage) 76 | expect(msg.uint8Field).toBe(Buffer.from([ 0x09 ]).readUInt8()) 77 | expect(msg.uint64field).toBe(Buffer.from([ 0xde, 0xad, 0xbe, 0xef, 0x00, 0x00, 0x01, 0x00 ]).readBigUInt64LE()) 78 | }) 79 | it('parses trimmed two field buffer', () => { 80 | const mavlink = new MavLinkProtocolV2() 81 | const msg = mavlink.data(Buffer.from([ 0x09, 0xde, 0xad, 0xbe, 0xef ]), TwoFieldMessage) 82 | expect(msg.uint8Field).toBe(Buffer.from([ 0x09 ]).readUInt8()) 83 | expect(msg.uint64field).toBe(Buffer.from([ 0xde, 0xad, 0xbe, 0xef, 0x00, 0x00, 0x00, 0x00 ]).readBigUInt64LE()) 84 | }) 85 | it('parses untrimmed array field', () => { 86 | const mavlink = new MavLinkProtocolV2() 87 | const msg = mavlink.data(Buffer.from([ 0xde, 0xad, 0xbe, 0xef ]), ArrayFieldMessage) 88 | expect(msg.arrayField).toStrictEqual([ 89 | Buffer.from([ 0xde, 0xad ]).readUInt16LE(), 90 | Buffer.from([ 0xbe, 0xef ]).readUInt16LE(), 91 | ]) 92 | }) 93 | it('parses trimmed array field', () => { 94 | const mavlink = new MavLinkProtocolV2() 95 | const msg = mavlink.data(Buffer.from([ 0xde, 0xad, 0xbe ]), ArrayFieldMessage) 96 | expect(msg.arrayField).toStrictEqual([ 97 | Buffer.from([ 0xde, 0xad ]).readUInt16LE(), 98 | Buffer.from([ 0xbe, 0x00 ]).readUInt16LE(), 99 | ]) 100 | }) 101 | }) 102 | -------------------------------------------------------------------------------- /lib/mavlink.ts: -------------------------------------------------------------------------------- 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 = 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 | get name(): string { 44 | return (this.constructor as unknown as { 'NAME': string }).NAME 45 | } 46 | 47 | /** 48 | * Serialize a message to a buffer 49 | */ 50 | abstract serialize(message: MavLinkData, seq: uint8_t): Buffer 51 | 52 | /** 53 | * Deserialize packet header 54 | */ 55 | abstract header(buffer: Buffer, timestamp?: bigint): MavLinkPacketHeader 56 | 57 | /** 58 | * Deserialize packet checksum 59 | */ 60 | abstract crc(buffer: Buffer): uint16_t 61 | 62 | /** 63 | * Extract payload buffer 64 | * 65 | * The returned payload buffer needs to be long enough to read all 66 | * the fields, including extensions that are sometimes not being sent 67 | * from the transmitting system. 68 | */ 69 | abstract payload(buffer: Buffer): Buffer 70 | 71 | /** 72 | * Deserialize payload into actual data class 73 | */ 74 | data(payload: Buffer, clazz: MavLinkDataConstructor): T { 75 | this.log.trace('Deserializing', clazz.MSG_NAME, 'with payload of size', payload.length) 76 | 77 | const instance = new clazz() 78 | let payloadLength = payload.length 79 | for (const field of clazz.FIELDS) { 80 | const fieldLength = field.length === 0 ? field.size : field.length * field.size 81 | const deserialize = DESERIALIZERS[field.type] 82 | if (!deserialize) { 83 | throw new Error(`Unknown field type ${field.type}`) 84 | } 85 | 86 | // Pad the payload if it is trimmed 87 | // https://mavlink.io/en/guide/serialization.html 88 | // MAVLink 2 implementations must truncate any empty (zero-filled) 89 | // bytes at the end of the serialized payload before it is sent. 90 | if (fieldLength > payloadLength) { 91 | const diff = fieldLength - payloadLength 92 | const newPayloadLength = payload.length + diff 93 | const newBuffer = Buffer.alloc(newPayloadLength) 94 | payload.copy(newBuffer, 0, 0, payload.length) 95 | payload = newBuffer 96 | } 97 | 98 | // @ts-ignore 99 | instance[field.name] = deserialize(payload, field.offset, field.length) 100 | payloadLength -= fieldLength 101 | } 102 | 103 | return instance 104 | } 105 | } 106 | 107 | /** 108 | * Interface describing static fields of a protocol classes 109 | */ 110 | interface MavLinkProtocolConstructor { 111 | NAME: string 112 | START_BYTE: number 113 | PAYLOAD_OFFSET: number 114 | 115 | SYS_ID: uint8_t 116 | COMP_ID: uint8_t 117 | 118 | new(): MavLinkProtocol 119 | } 120 | 121 | /** 122 | * MavLink Protocol V1 123 | */ 124 | export class MavLinkProtocolV1 extends MavLinkProtocol { 125 | static NAME = 'MAV_V1' 126 | static START_BYTE = 0xFE 127 | static PAYLOAD_OFFSET = 6 128 | 129 | constructor( 130 | public sysid: uint8_t = MavLinkProtocol.SYS_ID, 131 | public compid: uint8_t = MavLinkProtocol.COMP_ID, 132 | ) { 133 | super() 134 | } 135 | 136 | serialize(message: MavLinkData, seq: number): Buffer { 137 | this.log.trace('Serializing message (seq:', seq, ')') 138 | 139 | const definition: MavLinkDataConstructor = message.constructor 140 | const buffer = Buffer.from(new Uint8Array(MavLinkProtocolV1.PAYLOAD_OFFSET + definition.PAYLOAD_LENGTH + MavLinkProtocol.CHECKSUM_LENGTH)) 141 | 142 | // serialize header 143 | buffer.writeUInt8(MavLinkProtocolV1.START_BYTE, 0) 144 | buffer.writeUInt8(definition.PAYLOAD_LENGTH, 1) 145 | buffer.writeUInt8(seq, 2) 146 | buffer.writeUInt8(this.sysid, 3) 147 | buffer.writeUInt8(this.compid, 4) 148 | buffer.writeUInt8(definition.MSG_ID, 5) 149 | 150 | // serialize fields 151 | definition.FIELDS.forEach(field => { 152 | const serialize = SERIALIZERS[field.type] 153 | if (!serialize) throw new Error(`Unknown field type ${field.type}: serializer not found`) 154 | // @ts-ignore 155 | serialize(message[field.name], buffer, field.offset + MavLinkProtocolV1.PAYLOAD_OFFSET, field.length) 156 | }) 157 | 158 | // serialize checksum 159 | const crc = x25crc(buffer, 1, 2, definition.MAGIC_NUMBER) 160 | buffer.writeUInt16LE(crc, buffer.length - 2) 161 | 162 | return buffer 163 | } 164 | 165 | header(buffer: Buffer, timestamp?: bigint): MavLinkPacketHeader { 166 | this.log.trace('Reading header from buffer (len:', buffer.length, ')') 167 | 168 | const startByte = buffer.readUInt8(0) 169 | if (startByte !== MavLinkProtocolV1.START_BYTE) { 170 | throw new Error(`Invalid start byte (expected: ${MavLinkProtocolV1.START_BYTE}, got ${startByte})`) 171 | } 172 | 173 | const result = new MavLinkPacketHeader() 174 | result.timestamp = timestamp || null 175 | result.payloadLength = buffer.readUInt8(1) 176 | result.seq = buffer.readUInt8(2) 177 | result.sysid = buffer.readUInt8(3) 178 | result.compid = buffer.readUInt8(4) 179 | result.msgid = buffer.readUInt8(5) 180 | 181 | return result 182 | } 183 | 184 | /** 185 | * Deserialize packet checksum 186 | */ 187 | crc(buffer: Buffer): uint16_t { 188 | this.log.trace('Reading crc from buffer (len:', buffer.length, ')') 189 | 190 | const plen = buffer.readUInt8(1) 191 | return buffer.readUInt16LE(MavLinkProtocolV1.PAYLOAD_OFFSET + plen) 192 | } 193 | 194 | payload(buffer: Buffer): Buffer { 195 | this.log.trace('Reading payload from buffer (len:', buffer.length, ')') 196 | 197 | const plen = buffer.readUInt8(1) 198 | const payload = buffer.slice(MavLinkProtocolV1.PAYLOAD_OFFSET, MavLinkProtocolV1.PAYLOAD_OFFSET + plen) 199 | const padding = Buffer.from(new Uint8Array(255 - payload.length)) 200 | return Buffer.concat([payload, padding]) 201 | } 202 | } 203 | 204 | /** 205 | * MavLink Protocol V2 206 | */ 207 | export class MavLinkProtocolV2 extends MavLinkProtocol { 208 | static NAME = 'MAV_V2' 209 | static START_BYTE = 0xFD 210 | static PAYLOAD_OFFSET = 10 211 | 212 | static INCOMPATIBILITY_FLAGS: uint8_t = 0 213 | static COMPATIBILITY_FLAGS: uint8_t = 0 214 | 215 | static readonly IFLAG_SIGNED = 0x01 216 | 217 | static readonly SIGNATURE_START_TIME = Date.UTC(2015, 0, 1); 218 | 219 | constructor( 220 | public sysid: uint8_t = MavLinkProtocol.SYS_ID, 221 | public compid: uint8_t = MavLinkProtocol.COMP_ID, 222 | public incompatibilityFlags: uint8_t = MavLinkProtocolV2.INCOMPATIBILITY_FLAGS, 223 | public compatibilityFlags: uint8_t = MavLinkProtocolV2.COMPATIBILITY_FLAGS, 224 | ) { 225 | super() 226 | } 227 | 228 | serialize(message: MavLinkData, seq: number): Buffer { 229 | this.log.trace('Serializing message (seq:', seq, ')') 230 | 231 | const definition: MavLinkDataConstructor = message.constructor 232 | const buffer = Buffer.from(new Uint8Array(MavLinkProtocolV2.PAYLOAD_OFFSET + definition.PAYLOAD_LENGTH + MavLinkProtocol.CHECKSUM_LENGTH)) 233 | 234 | buffer.writeUInt8(MavLinkProtocolV2.START_BYTE, 0) 235 | buffer.writeUInt8(this.incompatibilityFlags, 2) 236 | buffer.writeUInt8(this.compatibilityFlags, 3) 237 | buffer.writeUInt8(seq, 4) 238 | buffer.writeUInt8(this.sysid, 5) 239 | buffer.writeUInt8(this.compid, 6) 240 | buffer.writeUIntLE(definition.MSG_ID, 7, 3) 241 | 242 | definition.FIELDS.forEach(field => { 243 | const serialize = SERIALIZERS[field.type] 244 | if (!serialize) throw new Error(`Unknown field type ${field.type}: serializer not found`) 245 | // @ts-ignore 246 | serialize(message[field.name], buffer, field.offset + MavLinkProtocolV2.PAYLOAD_OFFSET, field.length) 247 | }) 248 | 249 | // calculate actual truncated payload length 250 | const payloadLength = this.calculateTruncatedPayloadLength(buffer) 251 | buffer.writeUInt8(payloadLength, 1) 252 | 253 | // slice out the message buffer 254 | const result = buffer.slice(0, MavLinkProtocolV2.PAYLOAD_OFFSET + payloadLength + MavLinkProtocol.CHECKSUM_LENGTH) 255 | 256 | const crc = x25crc(result, 1, 2, definition.MAGIC_NUMBER) 257 | result.writeUInt16LE(crc, result.length - MavLinkProtocol.CHECKSUM_LENGTH) 258 | 259 | return result 260 | } 261 | 262 | /** 263 | * Create a signed package buffer 264 | * 265 | * @param buffer buffer with the original, unsigned package 266 | * @param linkId id of the link 267 | * @param key key to sign the package with 268 | * @param timestamp optional timestamp for packet signing (default: Date.now()) 269 | * @returns signed package 270 | */ 271 | sign(buffer: Buffer, linkId: number, key: Buffer, timestamp = Date.now()) { 272 | this.log.trace('Signing message') 273 | 274 | const result = Buffer.concat([ 275 | buffer, 276 | Buffer.from(new Uint8Array(MavLinkPacketSignature.SIGNATURE_LENGTH)) 277 | ]) 278 | 279 | const signer = new MavLinkPacketSignature(result) 280 | signer.linkId = linkId 281 | signer.timestamp = (timestamp - MavLinkProtocolV2.SIGNATURE_START_TIME) * 100 282 | signer.signature = signer.calculate(key) 283 | 284 | return result 285 | } 286 | 287 | private calculateTruncatedPayloadLength(buffer: Buffer): number { 288 | let result = buffer.length 289 | 290 | for (let i = buffer.length - MavLinkProtocol.CHECKSUM_LENGTH - 1; i >= MavLinkProtocolV2.PAYLOAD_OFFSET; i--) { 291 | result = i 292 | if (buffer[i] !== 0) { 293 | result++ 294 | break 295 | } 296 | } 297 | 298 | return result - MavLinkProtocolV2.PAYLOAD_OFFSET 299 | } 300 | 301 | header(buffer: Buffer, timestamp?: bigint): MavLinkPacketHeader { 302 | this.log.trace('Reading header from buffer (len:', buffer.length, ')') 303 | 304 | const startByte = buffer.readUInt8(0) 305 | if (startByte !== MavLinkProtocolV2.START_BYTE) { 306 | throw new Error(`Invalid start byte (expected: ${MavLinkProtocolV2.START_BYTE}, got ${startByte})`) 307 | } 308 | 309 | const result = new MavLinkPacketHeader() 310 | result.timestamp = timestamp || null 311 | result.magic = startByte 312 | result.payloadLength = buffer.readUInt8(1) 313 | result.incompatibilityFlags = buffer.readUInt8(2) 314 | result.compatibilityFlags = buffer.readUInt8(3) 315 | result.seq = buffer.readUInt8(4) 316 | result.sysid = buffer.readUInt8(5) 317 | result.compid = buffer.readUInt8(6) 318 | result.msgid = buffer.readUIntLE(7, 3) 319 | 320 | return result 321 | } 322 | 323 | /** 324 | * Deserialize packet checksum 325 | */ 326 | crc(buffer: Buffer): uint16_t { 327 | this.log.trace('Reading crc from buffer (len:', buffer.length, ')') 328 | 329 | const plen = buffer.readUInt8(1) 330 | return buffer.readUInt16LE(MavLinkProtocolV2.PAYLOAD_OFFSET + plen) 331 | } 332 | 333 | payload(buffer: Buffer): Buffer { 334 | this.log.trace('Reading payload from buffer (len:', buffer.length, ')') 335 | 336 | const plen = buffer.readUInt8(1) 337 | const payload = buffer.slice(MavLinkProtocolV2.PAYLOAD_OFFSET, MavLinkProtocolV2.PAYLOAD_OFFSET + plen) 338 | const padding = Buffer.from(new Uint8Array(255 - payload.length)) 339 | return Buffer.concat([payload, padding]) 340 | } 341 | 342 | signature(buffer: Buffer, header: MavLinkPacketHeader): MavLinkPacketSignature | null { 343 | this.log.trace('Reading signature from buffer (len:', buffer.length, ')') 344 | 345 | if (header.incompatibilityFlags & MavLinkProtocolV2.IFLAG_SIGNED) { 346 | return new MavLinkPacketSignature(buffer) 347 | } else { 348 | return null 349 | } 350 | } 351 | } 352 | 353 | /** 354 | * Registry of known protocols by STX 355 | */ 356 | const KNOWN_PROTOCOLS_BY_STX = { 357 | [MavLinkProtocolV1.START_BYTE]: MavLinkProtocolV1, 358 | [MavLinkProtocolV2.START_BYTE]: MavLinkProtocolV2, 359 | } 360 | 361 | /** 362 | * MavLink packet signature definition 363 | */ 364 | export class MavLinkPacketSignature { 365 | static SIGNATURE_LENGTH = 13 366 | 367 | /** 368 | * Calculate key based on secret passphrase 369 | * 370 | * @param passphrase secret to generate the key 371 | * @returns key as a buffer 372 | */ 373 | static key(passphrase: string) { 374 | return createHash('sha256') 375 | .update(passphrase) 376 | .digest() 377 | } 378 | 379 | constructor(private readonly buffer: Buffer) { } 380 | 381 | private get offset() { 382 | return this.buffer.length - MavLinkPacketSignature.SIGNATURE_LENGTH 383 | } 384 | 385 | /** 386 | * Get the linkId from signature 387 | */ 388 | get linkId() { 389 | return this.buffer.readUInt8(this.offset) 390 | } 391 | 392 | /** 393 | * Set the linkId in signature 394 | */ 395 | set linkId(value: uint8_t) { 396 | this.buffer.writeUInt8(value, this.offset) 397 | } 398 | 399 | /** 400 | * Get the timestamp from signature 401 | */ 402 | get timestamp() { 403 | return this.buffer.readUIntLE(this.offset + 1, 6) 404 | } 405 | 406 | /** 407 | * Set the linkId in signature 408 | */ 409 | set timestamp(value: number) { 410 | this.buffer.writeUIntLE(value, this.offset + 1, 6) 411 | } 412 | 413 | /** 414 | * Get the signature from signature 415 | */ 416 | get signature() { 417 | return this.buffer.slice(this.offset + 7, this.offset + 7 + 6).toString('hex') 418 | } 419 | 420 | /** 421 | * Set the signature in signature 422 | */ 423 | set signature(value: string) { 424 | this.buffer.write(value, this.offset + 7, 'hex') 425 | } 426 | 427 | /** 428 | * Calculates signature of the packet buffer using the provided secret. 429 | * The secret is converted to a hash using the sha256 algorithm which matches 430 | * the way Mission Planner creates keys. 431 | * 432 | * @param key the secret key (Buffer) 433 | * @returns calculated signature value 434 | */ 435 | calculate(key: Buffer) { 436 | const hash = createHash('sha256') 437 | .update(key) 438 | .update(this.buffer.slice(0, this.buffer.length - 6)) 439 | .digest('hex') 440 | .substr(0, 12) 441 | 442 | return hash 443 | } 444 | 445 | /** 446 | * Checks the signature of the packet buffer against a given secret 447 | * The secret is converted to a hash using the sha256 algorithm which matches 448 | * the way Mission Planner creates keys. 449 | * 450 | * @param key key 451 | * @returns true if the signature matches, false otherwise 452 | */ 453 | matches(key: Buffer) { 454 | return this.calculate(key) === this.signature 455 | } 456 | 457 | toString() { 458 | return `linkid: ${this.linkId}, timestamp ${this.timestamp}, signature ${this.signature}` 459 | } 460 | } 461 | 462 | /** 463 | * MavLink packet definition 464 | */ 465 | export class MavLinkPacket { 466 | constructor( 467 | readonly buffer: Buffer, 468 | readonly header: MavLinkPacketHeader = new MavLinkPacketHeader(), 469 | readonly payload: Buffer = Buffer.from(new Uint8Array(255)), 470 | readonly crc: uint16_t = 0, 471 | readonly protocol: MavLinkProtocol = new MavLinkProtocolV1(), 472 | readonly signature: MavLinkPacketSignature | null = null, 473 | ) { } 474 | 475 | /** 476 | * Debug information about the packet 477 | * 478 | * @returns string representing debug information about a packet 479 | */ 480 | debug() { 481 | return 'Packet (' 482 | // @ts-ignore 483 | + `proto: ${this.protocol.name}, ` 484 | + `sysid: ${this.header.sysid}, ` 485 | + `compid: ${this.header.compid}, ` 486 | + `msgid: ${this.header.msgid}, ` 487 | + `seq: ${this.header.seq}, ` 488 | + `plen: ${this.header.payloadLength}, ` 489 | + `crc: ${hex(this.crc, 4)}` 490 | + this.signatureToString(this.signature) 491 | + ')' 492 | } 493 | 494 | private signatureToString(signature?: MavLinkPacketSignature | null) { 495 | return signature ? `, ${signature.toString()}` : '' 496 | } 497 | } 498 | 499 | /** 500 | * This enum describes the different ways validation of a buffer can end 501 | */ 502 | enum PacketValidationResult { VALID, INVALID, UNKNOWN } 503 | 504 | type BufferCallback = (buffer: Buffer) => void 505 | 506 | /** 507 | * A transform stream that splits the incoming data stream into chunks containing full MavLink messages 508 | */ 509 | export class MavLinkPacketSplitter extends Transform { 510 | protected readonly log = Logger.getLogger(this) 511 | 512 | private buffer = Buffer.from([]) 513 | private onCrcError: BufferCallback | null = null 514 | private readonly magicNumbers: Record 515 | private timestamp: bigint | null = null 516 | private _validPackagesCount = 0 517 | private _unknownPackagesCount = 0 518 | private _invalidPackagesCount = 0 519 | 520 | /** 521 | * @param opts options to pass on to the Transform constructor 522 | * @param verbose print diagnostic information 523 | * @param onCrcError callback executed if there is a CRC error (mostly for debugging) 524 | */ 525 | constructor( 526 | opts = {}, 527 | { 528 | onCrcError = () => { }, 529 | magicNumbers = MSG_ID_MAGIC_NUMBER 530 | }: { 531 | onCrcError?: BufferCallback, 532 | magicNumbers?: Record 533 | } = {} 534 | ) { 535 | super({ ...opts, objectMode: true }) 536 | this.onCrcError = onCrcError 537 | this.magicNumbers = magicNumbers 538 | } 539 | 540 | _transform(chunk: Buffer, encoding: string, callback: TransformCallback) { 541 | this.buffer = Buffer.concat([this.buffer, chunk]) 542 | 543 | while (this.buffer.byteLength > 0) { 544 | const offset = this.findStartOfPacket(this.buffer) 545 | if (offset === null) { 546 | // start of the package was not found - need more data 547 | break 548 | } 549 | 550 | // if the current offset is exactly the size of the timestamp field from tlog then read it. 551 | if (offset >= 8) { 552 | this.timestamp = this.buffer.readBigUInt64BE(offset - 8) / 1000n 553 | } else { 554 | this.timestamp = null 555 | } 556 | // fast-forward the buffer to the first start byte 557 | if (offset > 0) { 558 | this.buffer = this.buffer.slice(offset) 559 | } 560 | 561 | this.log.debug('Found potential packet start at', offset) 562 | 563 | // get protocol this buffer is encoded with 564 | const Protocol = this.getPacketProtocol(this.buffer) 565 | 566 | this.log.debug('Packet protocol is', Protocol.NAME) 567 | 568 | // check if the buffer contains at least the minimum size of data 569 | if (this.buffer.length < Protocol.PAYLOAD_OFFSET + MavLinkProtocol.CHECKSUM_LENGTH) { 570 | // current buffer shorter than the shortest message - skipping 571 | this.log.debug('Current buffer shorter than the shortest message - skipping') 572 | break 573 | } 574 | 575 | // check if the current buffer contains the entire message 576 | const expectedBufferLength = this.readPacketLength(this.buffer, Protocol) 577 | this.log.debug('Expected buffer length:', expectedBufferLength, `(${hex(expectedBufferLength)})`) 578 | if (this.buffer.length < expectedBufferLength) { 579 | // current buffer is not fully retrieved yet - skipping 580 | this.log.debug('Current buffer is not fully retrieved yet - skipping') 581 | break 582 | } else { 583 | this.log.debug('Current buffer length:', this.buffer.length, `(${hex(this.buffer.length, 4)})`) 584 | } 585 | 586 | // retrieve the buffer based on payload size 587 | const buffer = this.buffer.slice(0, expectedBufferLength) 588 | this.log.debug('Recognized buffer length:', buffer.length, `(${hex(buffer.length, 2)})`) 589 | 590 | switch (this.validatePacket(buffer, Protocol)) { 591 | case PacketValidationResult.VALID: 592 | this.log.debug('Found a valid packet') 593 | this._validPackagesCount++ 594 | this.push({ buffer, timestamp: this.timestamp }) 595 | // truncate the buffer to remove the current message 596 | this.buffer = this.buffer.slice(expectedBufferLength) 597 | break 598 | case PacketValidationResult.INVALID: 599 | this.log.debug('Found an invalid packet - skipping') 600 | this._invalidPackagesCount++ 601 | // truncate the buffer to remove the wrongly identified STX 602 | this.buffer = this.buffer.slice(1) 603 | break 604 | case PacketValidationResult.UNKNOWN: 605 | this.log.debug('Found an unknown packet - skipping') 606 | this._unknownPackagesCount++ 607 | // truncate the buffer to remove the current message 608 | this.buffer = this.buffer.slice(expectedBufferLength) 609 | break 610 | } 611 | } 612 | 613 | callback(null) 614 | } 615 | 616 | protected findStartOfPacket(buffer: Buffer, offset: number = 0) { 617 | const stxv1 = buffer.indexOf(MavLinkProtocolV1.START_BYTE, offset) 618 | const stxv2 = buffer.indexOf(MavLinkProtocolV2.START_BYTE, offset) 619 | 620 | if (stxv1 >= 0 && stxv2 >= 0) { 621 | // in the current buffer both STX v1 and v2 are found - get the first one 622 | if (stxv1 < stxv2) { 623 | return stxv1 624 | } else { 625 | return stxv2 626 | } 627 | } else if (stxv1 >= 0) { 628 | // in the current buffer STX v1 is found 629 | return stxv1 630 | } else if (stxv2 >= 0) { 631 | // in the current buffer STX v2 is found 632 | return stxv2 633 | } else { 634 | // no STX found 635 | return null 636 | } 637 | } 638 | 639 | private getPacketProtocol(buffer: Buffer) { 640 | return KNOWN_PROTOCOLS_BY_STX[buffer.readUInt8(0)] || null 641 | } 642 | 643 | private readPacketLength(buffer: Buffer, Protocol: MavLinkProtocolConstructor) { 644 | // check if the current buffer contains the entire message 645 | const payloadLength = buffer.readUInt8(1) 646 | return Protocol.PAYLOAD_OFFSET 647 | + payloadLength 648 | + MavLinkProtocol.CHECKSUM_LENGTH 649 | + (this.isV2Signed(buffer) ? MavLinkPacketSignature.SIGNATURE_LENGTH : 0) 650 | } 651 | 652 | private validatePacket(buffer: Buffer, Protocol: MavLinkProtocolConstructor) { 653 | const protocol = new Protocol() 654 | const header = protocol.header(buffer) 655 | const magic = this.magicNumbers[header.msgid] 656 | if (magic !== null && magic !== undefined) { 657 | const crc = protocol.crc(buffer) 658 | const trim = this.isV2Signed(buffer) 659 | ? MavLinkPacketSignature.SIGNATURE_LENGTH + MavLinkProtocol.CHECKSUM_LENGTH 660 | : MavLinkProtocol.CHECKSUM_LENGTH 661 | const crc2 = x25crc(buffer, 1, trim, magic) 662 | if (crc === crc2) { 663 | // this is a proper message that is known and has been validated for corrupted data 664 | return PacketValidationResult.VALID 665 | } else { 666 | // CRC mismatch 667 | const message = [ 668 | `CRC error; expected: ${crc2} (${hex(crc2, 4)}), got ${crc} (${hex(crc, 4)});`, 669 | `msgid: ${header.msgid} (${hex(header.msgid)}),`, 670 | `seq: ${header.seq} (${hex(header.seq)}),`, 671 | `plen: ${header.payloadLength} (${hex(header.payloadLength)}),`, 672 | `magic: ${magic} (${hex(magic)})`, 673 | ] 674 | this.log.warn(message.join(' ')) 675 | if (this.onCrcError) this.onCrcError(buffer) 676 | 677 | return PacketValidationResult.INVALID 678 | } 679 | } else { 680 | // unknown message (as in not generated from the XML sources) 681 | this.log.debug(`Unknown message with id ${header.msgid} (magic number not found) - skipping`) 682 | 683 | return PacketValidationResult.UNKNOWN 684 | } 685 | } 686 | 687 | /** 688 | * Checks if the buffer contains the entire message with signature 689 | * 690 | * @param buffer buffer with the message 691 | */ 692 | private isV2Signed(buffer: Buffer) { 693 | const protocol = buffer.readUInt8(0) 694 | if (protocol === MavLinkProtocolV2.START_BYTE) { 695 | const flags = buffer.readUInt8(2) 696 | return !!(flags & MavLinkProtocolV2.IFLAG_SIGNED) 697 | } 698 | } 699 | 700 | /** 701 | * Number of invalid packages 702 | */ 703 | get validPackages() { 704 | return this._validPackagesCount 705 | } 706 | 707 | /** 708 | * Reset the number of valid packages 709 | */ 710 | resetValidPackagesCount() { 711 | this._validPackagesCount = 0 712 | } 713 | 714 | /** 715 | * Number of invalid packages 716 | */ 717 | get invalidPackages() { 718 | return this._invalidPackagesCount 719 | } 720 | 721 | /** 722 | * Reset the number of invalid packages 723 | */ 724 | resetInvalidPackagesCount() { 725 | this._invalidPackagesCount = 0 726 | } 727 | 728 | /** 729 | * Number of invalid packages 730 | */ 731 | get unknownPackagesCount() { 732 | return this._unknownPackagesCount 733 | } 734 | 735 | /** 736 | * Reset the number of invalid packages 737 | */ 738 | resetUnknownPackagesCount() { 739 | this._unknownPackagesCount = 0 740 | } 741 | } 742 | 743 | export class MavLinkTLogPacketSplitter extends MavLinkPacketSplitter { 744 | _transform(chunk: Buffer, encoding: string, callback: TransformCallback) { 745 | return super._transform(chunk, encoding, callback) 746 | } 747 | 748 | protected findStartOfPacket(buffer: Buffer, offset: number = 0) { 749 | // Finding the start of packet in TLog requires locating the start byte 750 | // at an offset greater than 751 | let offset1 = offset 752 | while (true) { 753 | const start = super.findStartOfPacket(buffer, offset1) 754 | if (start === null) return null 755 | if (start < 8) offset1 = start + 1 756 | else if (offset1 >= buffer.length - 1) return null 757 | else return start 758 | } 759 | } 760 | } 761 | 762 | /** 763 | * A transform stream that takes a buffer with data and converts it to MavLinkPacket object 764 | */ 765 | export class MavLinkPacketParser extends Transform { 766 | protected readonly log = Logger.getLogger(this) 767 | 768 | constructor(opts = {}) { 769 | super({ ...opts, objectMode: true }) 770 | } 771 | 772 | private getProtocol(buffer: Buffer): MavLinkProtocol { 773 | const startByte = buffer.readUInt8(0) 774 | switch (startByte) { 775 | case MavLinkProtocolV1.START_BYTE: 776 | return new MavLinkProtocolV1() 777 | case MavLinkProtocolV2.START_BYTE: 778 | return new MavLinkProtocolV2() 779 | default: 780 | throw new Error(`Unknown protocol '${hex(startByte)}'`) 781 | } 782 | } 783 | 784 | _transform({ buffer = Buffer.from([]), timestamp = null, ...rest } = {}, encoding: string, callback: TransformCallback) { 785 | const protocol = this.getProtocol(buffer) 786 | const header = protocol.header(buffer, timestamp || undefined) 787 | const payload = protocol.payload(buffer) 788 | const crc = protocol.crc(buffer) 789 | const signature = protocol instanceof MavLinkProtocolV2 790 | ? protocol.signature(buffer, header) 791 | : null 792 | 793 | const packet = new MavLinkPacket(buffer, header, payload, crc, protocol, signature) 794 | 795 | callback(null, packet) 796 | } 797 | } 798 | 799 | /** 800 | * Creates a MavLink packet stream reader that is reading packets from the given input 801 | * 802 | * @param input input stream to read from 803 | */ 804 | export function createMavLinkStream(input: Readable, { 805 | onCrcError, 806 | magicNumbers 807 | }: { 808 | onCrcError?: BufferCallback, 809 | magicNumbers?: Record 810 | } = {}) { 811 | return input 812 | .pipe(new MavLinkPacketSplitter({}, { onCrcError, magicNumbers })) 813 | .pipe(new MavLinkPacketParser()) 814 | } 815 | 816 | let seq = 0 817 | 818 | /** 819 | * Send a packet to the stream 820 | * 821 | * @param stream Stream to send the data to 822 | * @param msg message to serialize and send 823 | * @param protocol protocol to use (default: MavLinkProtocolV1) 824 | * @returns number of bytes sent 825 | */ 826 | export async function send(stream: Writable, msg: MavLinkData, protocol: MavLinkProtocol = new MavLinkProtocolV1()) { 827 | return new Promise((resolve, reject) => { 828 | const buffer = protocol.serialize(msg, seq++) 829 | seq &= 255 830 | stream.write(buffer, err => { 831 | if (err) reject(err) 832 | else resolve(buffer.length) 833 | }) 834 | }) 835 | } 836 | 837 | /** 838 | * Send a signed packet to the stream. Signed packets are always V2 protocol 839 | * 840 | * @param stream Stream to send the data to 841 | * @param msg message to serialize and send 842 | * @param key key to sign the message with 843 | * @param linkId link id for the signature 844 | * @param sysid system id 845 | * @param compid component id 846 | * @param timestamp optional timestamp for packet signing (default: Date.now()) 847 | * @returns number of bytes sent 848 | */ 849 | export async function sendSigned( 850 | stream: Writable, 851 | msg: MavLinkData, 852 | key: Buffer, 853 | linkId: uint8_t = 1, sysid: uint8_t = MavLinkProtocol.SYS_ID, compid: uint8_t = MavLinkProtocol.COMP_ID, 854 | timestamp = Date.now() 855 | ) { 856 | return new Promise((resolve, reject) => { 857 | const protocol = new MavLinkProtocolV2(sysid, compid, MavLinkProtocolV2.IFLAG_SIGNED) 858 | const b1 = protocol.serialize(msg, seq++) 859 | seq &= 255 860 | const b2 = protocol.sign(b1, linkId, key, timestamp) 861 | stream.write(b2, err => { 862 | if (err) reject(err) 863 | else resolve(b2.length) 864 | }) 865 | }) 866 | } 867 | -------------------------------------------------------------------------------- /lib/serialization.test.ts: -------------------------------------------------------------------------------- 1 | import { SERIALIZERS, DESERIALIZERS } from './serialization' 2 | 3 | describe('serialization', () => { 4 | describe('char', () => { 5 | it('will serialize char', () => { 6 | const b = Buffer.from([0]) 7 | // @ts-ignore 8 | SERIALIZERS['char'](42, b, 0) 9 | expect(b).toStrictEqual(Buffer.from([42])) 10 | }) 11 | it('will deserialize char', () => { 12 | const b = Buffer.from([42]) 13 | // @ts-ignore 14 | const result = DESERIALIZERS['char'](b, 0) 15 | expect(result).toBe('*') 16 | }) 17 | it('will serialize char[]', () => { 18 | const b = Buffer.from(new Array(3)) 19 | SERIALIZERS['char[]']('LOL', b, 0, 5) 20 | expect(b).toStrictEqual(Buffer.from([76, 79, 76])) 21 | }) 22 | it('will deserialize char[]', () => { 23 | const b = Buffer.from([76, 79, 76, 0, 0, 0, 0, 0]) 24 | const result = DESERIALIZERS['char[]'](b, 0, 5) 25 | expect(result).toBe('LOL') 26 | }) 27 | }) 28 | 29 | describe('uint8_t', () => { 30 | it('will serialize 8-bit unsigned int', () => { 31 | const b = Buffer.from([0]) 32 | // @ts-ignore 33 | SERIALIZERS['uint8_t'](2, b, 0) 34 | expect(b).toStrictEqual(Buffer.from([2])) 35 | }) 36 | it('will deserialize 8-bit unsigned int', () => { 37 | const b = Buffer.from([2]) 38 | // @ts-ignore 39 | const result = DESERIALIZERS['uint8_t'](b, 0) 40 | expect(result).toBe(2) 41 | }) 42 | it('will serialize array of 8-bit unsigned int', () => { 43 | const b = Buffer.from(new Array(3)) 44 | SERIALIZERS['uint8_t[]']([ 1, 2, 4 ], b, 0, 3) 45 | expect(b).toStrictEqual(Buffer.from([1, 2, 4])) 46 | }) 47 | it('will deserialize array of 8-bit unsigned int', () => { 48 | const b = Buffer.from([1, 2, 4]) 49 | const result = DESERIALIZERS['uint8_t[]'](b, 0, 3) 50 | expect(result).toStrictEqual([1, 2, 4]) 51 | }) 52 | }) 53 | 54 | describe('int8_t', () => { 55 | it('will serialize 8-bit signed int', () => { 56 | const b = Buffer.from([0]) 57 | // @ts-ignore 58 | SERIALIZERS['int8_t'](-2, b, 0) 59 | expect(b).toStrictEqual(Buffer.from([-2])) 60 | }) 61 | it('will deserialize 8-bit signed int', () => { 62 | const b = Buffer.from([-2]) 63 | // @ts-ignore 64 | const result = DESERIALIZERS['int8_t'](b, 0) 65 | expect(result).toBe(-2) 66 | }) 67 | it('will serialize array of 8-bit signed int', () => { 68 | const b = Buffer.from(new Array(3)) 69 | SERIALIZERS['int8_t[]']([ 1, -2, -5 ], b, 0, 3) 70 | expect(b).toStrictEqual(Buffer.from([1, -2, -5])) 71 | }) 72 | it('will deserialize array of 8-bit signed int', () => { 73 | const b = Buffer.from([255, 254, 252]) 74 | const result = DESERIALIZERS['int8_t[]'](b, 0, 3) 75 | expect(result).toStrictEqual([-1, -2, -4]) 76 | }) 77 | }) 78 | 79 | describe('uint16_t', () => { 80 | it('will serialize 16-bit unsigned int', () => { 81 | const b = Buffer.from([0, 0]) 82 | // @ts-ignore 83 | SERIALIZERS['uint16_t'](2, b, 0) 84 | expect(b).toStrictEqual(Buffer.from([2, 0])) 85 | }) 86 | it('will deserialize 16-bit unsigned int', () => { 87 | const b = Buffer.from([2, 0]) 88 | // @ts-ignore 89 | const result = DESERIALIZERS['uint16_t'](b, 0) 90 | expect(result).toBe(2) 91 | }) 92 | it('will serialize array of 16-bit unsigned int', () => { 93 | const b = Buffer.from(new Array(6)) 94 | SERIALIZERS['uint16_t[]']([ 1, 2, 4 ], b, 0, 3) 95 | expect(b).toStrictEqual(Buffer.from([1, 0, 2, 0, 4, 0])) 96 | }) 97 | it('will deserialize array of 16-bit unsigned int', () => { 98 | const b = Buffer.from([1, 0, 2, 0, 4, 0]) 99 | const result = DESERIALIZERS['uint16_t[]'](b, 0, 3) 100 | expect(result).toStrictEqual([1, 2, 4]) 101 | }) 102 | }) 103 | 104 | describe('int16_t', () => { 105 | it('will serialize 16-bit signed int', () => { 106 | const b = Buffer.from([0, 0]) 107 | // @ts-ignore 108 | SERIALIZERS['int16_t'](-2, b, 0) 109 | expect(b).toStrictEqual(Buffer.from([254, 255])) 110 | }) 111 | it('will deserialize 16-bit signed int', () => { 112 | const b = Buffer.from([254, 255]) 113 | // @ts-ignore 114 | const result = DESERIALIZERS['int16_t'](b, 0) 115 | expect(result).toBe(-2) 116 | }) 117 | it('will serialize array of 16-bit signed int', () => { 118 | const b = Buffer.from(new Array(6)) 119 | SERIALIZERS['int16_t[]']([1, -2, -5], b, 0, 3) 120 | expect(b).toStrictEqual(Buffer.from([1, 0, 254, 255, 251, 255])) 121 | }) 122 | it('will deserialize array of 16-bit signed int', () => { 123 | const b = Buffer.from([1, 0, 254, 255, 251, 255]) 124 | const result = DESERIALIZERS['int16_t[]'](b, 0, 3) 125 | expect(result).toStrictEqual([1, -2, -5]) 126 | }) 127 | }) 128 | 129 | describe('uint32_t', () => { 130 | it('will serialize 32-bit unsigned int', () => { 131 | const b = Buffer.from([0, -1, -2, -3]) 132 | // @ts-ignore 133 | SERIALIZERS['uint32_t'](2, b, 0) 134 | expect(b).toStrictEqual(Buffer.from([2, 0, 0, 0])) 135 | }) 136 | it('will deserialize 32-bit unsigned int', () => { 137 | const b = Buffer.from([2, 0, 0, 0]) 138 | // @ts-ignore 139 | const result = DESERIALIZERS['uint32_t'](b, 0) 140 | expect(result).toBe(2) 141 | }) 142 | it('will serialize array of 32-bit unsigned int', () => { 143 | const b = Buffer.from(new Array(12)) 144 | SERIALIZERS['uint32_t[]']([ 1, 2, 4 ], b, 0, 3) 145 | expect(b).toStrictEqual(Buffer.from([1, 0, 0, 0, 2, 0, 0, 0, 4, 0, 0, 0])) 146 | }) 147 | it('will deserialize array of 32-bit unsigned int', () => { 148 | const b = Buffer.from([1, 0, 0, 0, 2, 0, 0, 0, 5, 0, 0, 0]) 149 | const result = DESERIALIZERS['uint32_t[]'](b, 0, 3) 150 | expect(result).toStrictEqual([1, 2, 5]) 151 | }) 152 | }) 153 | 154 | describe('int32_t', () => { 155 | it('will serialize 32-bit signed int', () => { 156 | const b = Buffer.from([0, 0, 0, 0]) 157 | // @ts-ignore 158 | SERIALIZERS['int32_t'](-2, b, 0) 159 | expect(b).toStrictEqual(Buffer.from([254, 255, 255, 255])) 160 | }) 161 | it('will deserialize 32-bit signed int', () => { 162 | const b = Buffer.from([254, 255, 255, 255]) 163 | // @ts-ignore 164 | const result = DESERIALIZERS['int32_t'](b, 0) 165 | expect(result).toBe(-2) 166 | }) 167 | it('will serialize array of 32-bit signed int', () => { 168 | const b = Buffer.from(new Array(12)) 169 | SERIALIZERS['int32_t[]']([ 1, -2, -5 ], b, 0, 3) 170 | expect(b).toStrictEqual(Buffer.from([1, 0, 0, 0, 254, 255, 255, 255, 251, 255, 255, 255])) 171 | }) 172 | it('will deserialize array of 32-bit signed int', () => { 173 | const b = Buffer.from([1, 0, 0, 0, 254, 255, 255, 255, 250, 255, 255, 255]) 174 | const result = DESERIALIZERS['int32_t[]'](b, 0, 3) 175 | expect(result).toStrictEqual([1, -2, -6]) 176 | }) 177 | }) 178 | 179 | describe('uint64_t', () => { 180 | it('will serialize 64-bit unsigned int', () => { 181 | const b = Buffer.from([0, -1, -2, -3, -4, -5, -6, -7]) 182 | // @ts-ignore 183 | SERIALIZERS['uint64_t'](2n, b, 0) 184 | expect(b).toStrictEqual(Buffer.from([2, 0, 0, 0, 0, 0, 0, 0])) 185 | }) 186 | it('will deserialize 64-bit unsigned int', () => { 187 | const b = Buffer.from([2, 0, 0, 0, 0, 0, 0, 0]) 188 | // @ts-ignore 189 | const result = DESERIALIZERS['uint64_t'](b, 0) 190 | expect(result).toBe(2n) 191 | }) 192 | it('will serialize array of 64-bit unsigned int', () => { 193 | const b = Buffer.from(new Array(24)) 194 | SERIALIZERS['uint64_t[]']([ 1n, 2n, 4n ], b, 0, 3) 195 | expect(b).toStrictEqual(Buffer.from([1, 0, 0, 0, 0, 0, 0, 0, 2, 0, 0, 0, 0, 0, 0, 0, 4, 0, 0, 0, 0, 0, 0, 0])) 196 | }) 197 | it('will deserialize array of 64-bit unsigned int', () => { 198 | const b = Buffer.from([1, 0, 0, 0, 0, 0, 0, 0, 2, 0, 0, 0, 0, 0, 0, 0, 5, 0, 0, 0, 0, 0, 0, 0, 0]) 199 | const result = DESERIALIZERS['uint64_t[]'](b, 0, 3) 200 | expect(result).toEqual([1n, 2n, 5n]) 201 | }) 202 | }) 203 | 204 | describe('int64_t', () => { 205 | it('will serialize 64-bit signed int', () => { 206 | const b = Buffer.from([0, 0, 0, 0, 0, 0, 0, 0]) 207 | // @ts-ignore 208 | SERIALIZERS['int64_t'](-2n, b, 0) 209 | expect(b).toStrictEqual(Buffer.from([254, 255, 255, 255, 255, 255, 255, 255])) 210 | }) 211 | it('will deserialize 64-bit signed int', () => { 212 | const b = Buffer.from([254, 255, 255, 255, 255, 255, 255, 255]) 213 | // @ts-ignore 214 | const result = DESERIALIZERS['int64_t'](b, 0) 215 | expect(result).toBe(-2n) 216 | }) 217 | it('will serialize array of 64-bit signed int', () => { 218 | const b = Buffer.from(new Array(24)) 219 | SERIALIZERS['int64_t[]']([ 1n, -2n, -5n ], b, 0, 3) 220 | expect(b).toStrictEqual(Buffer.from([1, 0, 0, 0, 0, 0, 0, 0, 254, 255, 255, 255, 255, 255, 255, 255, 251, 255, 255, 255, 255, 255, 255, 255])) 221 | }) 222 | it('will deserialize array of 64-bit signed int', () => { 223 | const b = Buffer.from([1, 0, 0, 0, 0, 0, 0, 0, 254, 255, 255, 255, 255, 255, 255, 255, 250, 255, 255, 255, 255, 255, 255, 255]) 224 | const result = DESERIALIZERS['int64_t[]'](b, 0, 3) 225 | expect(result).toEqual([1n, -2n, -6n]) 226 | }) 227 | }) 228 | 229 | describe('float', () => { 230 | it('will serialize float', () => { 231 | const b = Buffer.from([0, -1, -2, -3]) 232 | // @ts-ignore 233 | SERIALIZERS['float'](2.5, b, 0) 234 | expect(b).toStrictEqual(Buffer.from([0, 0, 32, 64])) 235 | }) 236 | it('will deserialize float', () => { 237 | const b = Buffer.from([0, 0, 32, 192]) 238 | // @ts-ignore 239 | const result = DESERIALIZERS['float'](b, 0) 240 | expect(result).toBe(-2.5) 241 | }) 242 | it('will serialize array of floats', () => { 243 | const b = Buffer.from(new Array(12)) 244 | SERIALIZERS['float[]']([ 1, 2, 4 ], b, 0, 3) 245 | expect(b).toStrictEqual(Buffer.from([0, 0, 128, 63, 0, 0, 0, 64, 0, 0, 128, 64])) 246 | }) 247 | it('will deserialize array of floats', () => { 248 | const b = Buffer.from([0, 0, 192, 63, 0, 0, 32, 192, 0, 0, 128, 64]) 249 | const result = DESERIALIZERS['float[]'](b, 0, 3) 250 | expect(result).toStrictEqual([1.5, -2.5, 4]) 251 | }) 252 | }) 253 | 254 | describe('double', () => { 255 | it('will serialize double', () => { 256 | const b = Buffer.from([0, -1, -2, -3, -4, -5, -6, -7]) 257 | // @ts-ignore 258 | SERIALIZERS['double'](2.34, b, 0) 259 | expect(b).toStrictEqual(Buffer.from([184, 30, 133, 235, 81, 184, 2, 64])) 260 | }) 261 | it('will deserialize double', () => { 262 | const b = Buffer.from([184, 30, 133, 235, 81, 184, 2, 64]) 263 | // @ts-ignore 264 | const result = DESERIALIZERS['double'](b, 0) 265 | expect(result).toBe(2.34) 266 | }) 267 | it('will serialize array of doubles', () => { 268 | const b = Buffer.from(new Array(24)) 269 | SERIALIZERS['double[]']([ 1.5, -2.5, 4 ], b, 0, 3) 270 | expect(b).toStrictEqual(Buffer.from([0, 0, 0, 0, 0, 0, 248, 63, 0, 0, 0, 0, 0, 0, 4, 192, 0, 0, 0, 0, 0, 0, 16, 64])) 271 | }) 272 | it('will deserialize array of doubles', () => { 273 | const b = Buffer.from([0, 0, 0, 0, 0, 0, 248, 63, 0, 0, 0, 0, 0, 0, 4, 192, 0, 0, 0, 0, 0, 0, 16, 64]) 274 | const result = DESERIALIZERS['double[]'](b, 0, 3) 275 | expect(result).toStrictEqual([1.5, -2.5, 4]) 276 | }) 277 | }) 278 | }) 279 | -------------------------------------------------------------------------------- /lib/serialization.ts: -------------------------------------------------------------------------------- 1 | import { 2 | int8_t, 3 | uint8_t, 4 | int16_t, 5 | uint16_t, 6 | int32_t, 7 | uint32_t, 8 | int64_t, 9 | uint64_t, 10 | float, 11 | double, 12 | } from 'mavlink-mappings' 13 | 14 | type Serializer = 15 | ((value: any, buffer: Buffer, offset: number) => void) | 16 | ((value: any, buffer: Buffer, offset: number, maxLen: number) => void) 17 | 18 | type Serializers = { [key: string]: Serializer } 19 | 20 | const SPECIAL_TYPES_SERIALIZERS: Serializers = { 21 | 'uint8_t_mavlink_version': (value: uint8_t, buffer: Buffer, offset: number) => buffer.writeUInt8(value, offset), 22 | } 23 | 24 | const SINGULAR_TYPES_SERIALIZERS: Serializers = { 25 | 'char' : (value: int8_t, buffer: Buffer, offset: number) => buffer.writeUInt8(value, offset), 26 | 'int8_t' : (value: int8_t, buffer: Buffer, offset: number) => buffer.writeInt8(value, offset), 27 | 'uint8_t' : (value: uint8_t, buffer: Buffer, offset: number) => buffer.writeUInt8(value, offset), 28 | 'int16_t' : (value: int16_t, buffer: Buffer, offset: number) => buffer.writeInt16LE(value, offset), 29 | 'uint16_t': (value: uint16_t, buffer: Buffer, offset: number) => buffer.writeUInt16LE(value, offset), 30 | 'int32_t' : (value: int32_t, buffer: Buffer, offset: number) => buffer.writeInt32LE(value, offset), 31 | 'uint32_t': (value: uint32_t, buffer: Buffer, offset: number) => buffer.writeUInt32LE(value, offset), 32 | 'int64_t' : (value: int64_t, buffer: Buffer, offset: number) => buffer.writeBigInt64LE(value, offset), 33 | 'uint64_t': (value: uint64_t, buffer: Buffer, offset: number) => buffer.writeBigUInt64LE(value, offset), 34 | 'float' : (value: float, buffer: Buffer, offset: number) => buffer.writeFloatLE(value, offset), 35 | 'double' : (value: double, buffer: Buffer, offset: number) => buffer.writeDoubleLE(value, offset), 36 | } 37 | 38 | const ARRAY_TYPES_SERIALIZERS: Serializers = { 39 | 'char[]': (value: string, buffer: Buffer, offset: number, maxLen: number) => { 40 | for (let i = 0; i < value.length && i < maxLen; i++) { 41 | const code = value.charCodeAt(i) 42 | buffer.writeUInt8(code, offset + i) 43 | } 44 | }, 45 | 'int8_t[]': (value: uint8_t[], buffer: Buffer, offset: number, maxLen: number) => { 46 | for (let i = 0; i < value.length && i < maxLen; i++) { 47 | buffer.writeInt8(value[i], offset + i) 48 | } 49 | }, 50 | 'uint8_t[]': (value: uint8_t[], buffer: Buffer, offset: number, maxLen: number) => { 51 | for (let i = 0; i < value.length && i < maxLen; i++) { 52 | buffer.writeUInt8(value[i], offset + i) 53 | } 54 | }, 55 | 'int16_t[]': (value: uint16_t[], buffer: Buffer, offset: number, maxLen: number) => { 56 | for (let i = 0; i < value.length && i < maxLen; i++) { 57 | buffer.writeInt16LE(value[i], offset + i * 2) 58 | } 59 | }, 60 | 'uint16_t[]': (value: uint16_t[], buffer: Buffer, offset: number, maxLen: number) => { 61 | for (let i = 0; i < value.length && i < maxLen; i++) { 62 | buffer.writeUInt16LE(value[i], offset + i * 2) 63 | } 64 | }, 65 | 'int32_t[]': (value: uint32_t[], buffer: Buffer, offset: number, maxLen: number) => { 66 | for (let i = 0; i < value.length && i < maxLen; i++) { 67 | buffer.writeInt32LE(value[i], offset + i * 4) 68 | } 69 | }, 70 | 'uint32_t[]': (value: uint32_t[], buffer: Buffer, offset: number, maxLen: number) => { 71 | for (let i = 0; i < value.length && i < maxLen; i++) { 72 | buffer.writeUInt32LE(value[i], offset + i * 4) 73 | } 74 | }, 75 | 'int64_t[]': (value: uint64_t[], buffer: Buffer, offset: number, maxLen: number) => { 76 | for (let i = 0; i < value.length && i < maxLen; i++) { 77 | buffer.writeBigInt64LE(value[i], offset + i * 8) 78 | } 79 | }, 80 | 'uint64_t[]': (value: uint64_t[], buffer: Buffer, offset: number, maxLen: number) => { 81 | for (let i = 0; i < value.length && i < maxLen; i++) { 82 | buffer.writeBigUInt64LE(value[i], offset + i * 8) 83 | } 84 | }, 85 | 'float[]': (value: float[], buffer: Buffer, offset: number, maxLen: number) => { 86 | for (let i = 0; i < value.length && i < maxLen; i++) { 87 | buffer.writeFloatLE(value[i], offset + i * 4) 88 | } 89 | }, 90 | 'double[]': (value: double[], buffer: Buffer, offset: number, maxLen: number) => { 91 | for (let i = 0; i < value.length && i < maxLen; i++) { 92 | buffer.writeDoubleLE(value[i], offset + i * 8) 93 | } 94 | }, 95 | } 96 | 97 | /** 98 | * A dictionary containing functions that serialize a certain value based on the field type 99 | */ 100 | export const SERIALIZERS = { 101 | ...SPECIAL_TYPES_SERIALIZERS, 102 | ...SINGULAR_TYPES_SERIALIZERS, 103 | ...ARRAY_TYPES_SERIALIZERS, 104 | } 105 | 106 | type Deserializer = ((buffer: Buffer, offset: number) => any) | ((buffer: Buffer, offset: number, length: number) => any) 107 | type Deserializers = { [key: string]: Deserializer } 108 | 109 | const SPECIAL_DESERIALIZERS: Deserializers = { 110 | 'uint8_t_mavlink_version': (buffer: Buffer, offset: number) => buffer.readUInt8(offset), 111 | } 112 | 113 | const SINGULAR_TYPES_DESERIALIZERS: Deserializers = { 114 | 'char' : (buffer: Buffer, offset: number) => String.fromCharCode(buffer.readUInt8(offset)), 115 | 'int8_t' : (buffer: Buffer, offset: number) => buffer.readInt8(offset), 116 | 'uint8_t' : (buffer: Buffer, offset: number) => buffer.readUInt8(offset), 117 | 'int16_t' : (buffer: Buffer, offset: number) => buffer.readInt16LE(offset), 118 | 'uint16_t': (buffer: Buffer, offset: number) => buffer.readUInt16LE(offset), 119 | 'int32_t' : (buffer: Buffer, offset: number) => buffer.readInt32LE(offset), 120 | 'uint32_t': (buffer: Buffer, offset: number) => buffer.readUInt32LE(offset), 121 | 'int64_t' : (buffer: Buffer, offset: number) => buffer.readBigInt64LE(offset), 122 | 'uint64_t': (buffer: Buffer, offset: number) => buffer.readBigUInt64LE(offset), 123 | 'float' : (buffer: Buffer, offset: number) => buffer.readFloatLE(offset), 124 | 'double' : (buffer: Buffer, offset: number) => buffer.readDoubleLE(offset), 125 | } 126 | 127 | const ARRAY_TYPES_DESERIALIZERS: Deserializers = { 128 | 'char[]': (buffer: Buffer, offset: number, length: number) => { 129 | let result = '' 130 | for (let i = 0; i < length; i++) { 131 | const charCode = buffer.readUInt8(offset + i) 132 | if (charCode !== 0) { 133 | result += String.fromCharCode(charCode) 134 | } else { 135 | break 136 | } 137 | } 138 | return result 139 | }, 140 | 'int8_t[]': (buffer: Buffer, offset: number, length: number) => { 141 | const result = new Array(length) 142 | for (let i = 0; i < length; i++) result[i] = buffer.readInt8(offset + i) 143 | return result 144 | }, 145 | 'uint8_t[]': (buffer: Buffer, offset: number, length: number) => { 146 | const result = new Array(length) 147 | for (let i = 0; i < length; i++) result[i] = buffer.readUInt8(offset + i) 148 | return result 149 | }, 150 | 'int16_t[]': (buffer: Buffer, offset: number, length: number) => { 151 | const result = new Array(length) 152 | for (let i = 0; i < length; i++) result[i] = buffer.readInt16LE(offset + i * 2) 153 | return result 154 | }, 155 | 'uint16_t[]': (buffer: Buffer, offset: number, length: number) => { 156 | const result = new Array(length) 157 | for (let i = 0; i < length; i++) result[i] = buffer.readUInt16LE(offset + i * 2) 158 | return result 159 | }, 160 | 'int32_t[]': (buffer: Buffer, offset: number, length: number) => { 161 | const result = new Array(length) 162 | for (let i = 0; i < length; i++) result[i] = buffer.readInt32LE(offset + i * 4) 163 | return result 164 | }, 165 | 'uint32_t[]': (buffer: Buffer, offset: number, length: number) => { 166 | const result = new Array(length) 167 | for (let i = 0; i < length; i++) result[i] = buffer.readUInt32LE(offset + i * 4) 168 | return result 169 | }, 170 | 'int64_t[]': (buffer: Buffer, offset: number, length: number) => { 171 | const result = new Array(length) 172 | for (let i = 0; i < length; i++) result[i] = buffer.readBigInt64LE(offset + i * 8) 173 | return result 174 | }, 175 | 'uint64_t[]': (buffer: Buffer, offset: number, length: number) => { 176 | const result = new Array(length) 177 | for (let i = 0; i < length; i++) result[i] = buffer.readBigUInt64LE(offset + i * 8) 178 | return result 179 | }, 180 | 'float[]': (buffer: Buffer, offset: number, length: number) => { 181 | const result = new Array(length) 182 | for (let i = 0; i < length; i++) result[i] = buffer.readFloatLE(offset + i * 4) 183 | return result 184 | }, 185 | 'double[]': (buffer: Buffer, offset: number, length: number) => { 186 | const result = new Array(length) 187 | for (let i = 0; i < length; i++) result[i] = buffer.readDoubleLE(offset + i * 8) 188 | return result 189 | }, 190 | } 191 | 192 | /** 193 | * A dictionary containing functions that deserialize a certain value based on the field type 194 | */ 195 | export const DESERIALIZERS: Deserializers = { 196 | ...SPECIAL_DESERIALIZERS, 197 | ...SINGULAR_TYPES_DESERIALIZERS, 198 | ...ARRAY_TYPES_DESERIALIZERS, 199 | } 200 | -------------------------------------------------------------------------------- /lib/utils.ts: -------------------------------------------------------------------------------- 1 | /** 2 | * Convert a number to hexadecimal representation with a minimum 3 | * number of characters and optional prefix (0x by default) 4 | * 5 | * @param n value to convert 6 | * @param len length of the converted string (without prefix) 7 | * @param prefix prefix to prepend the generated string with 8 | */ 9 | export function hex(n: number, len: number = 2, prefix = '0x') { 10 | return `${prefix}${n.toString(16).padStart(len, '0')}` 11 | } 12 | 13 | /** 14 | * Dump a buffer in a readable form 15 | * 16 | * @param buffer buffer to dump 17 | * @param lineWidth width of the line, in bytes of buffer 18 | */ 19 | export function dump(buffer: Buffer, lineWidth = 16) { 20 | const line = [] 21 | let address = 0 22 | for (let i = 0; i < buffer.length; i++) { 23 | line.push(hex(buffer[i], 2, '0x')) 24 | if (line.length === lineWidth) { 25 | console.log(hex(address, 4), '|', line.join(' ')) 26 | address += lineWidth 27 | line.length = 0 28 | } 29 | } 30 | if (line.length > 0) { 31 | console.log(hex(address, 4), '|', line.join(' ')) 32 | } 33 | } 34 | 35 | /** 36 | * Sleep for a given number of milliseconds 37 | * 38 | * @param {number} ms of milliseconds to sleep 39 | */ 40 | export function sleep(ms: number) { 41 | return new Promise(resolve => setTimeout(resolve, ms)) 42 | } 43 | 44 | /** 45 | * Execute a callback every intervalms and if it will not return 46 | * a truthy value in the timeoutms then throw a Timeout exception. 47 | * This is a very useful utility that will allow you to specify how often 48 | * a particular expression should be evaluated and how long will it take to end 49 | * the execution without success. Great for time-sensitive operations. 50 | * 51 | * @param cb callback to call every intervalms. Waiting stops if the callback returns a truthy value. 52 | * @param timeout number of milliseconds that need to pass before the Timeout exception is thrown 53 | * @param interval number of milliseconds before re-running the callback 54 | */ 55 | export async function waitFor(cb: () => T, timeout = 10000, interval = 100): Promise { 56 | return new Promise((resolve, reject) => { 57 | const timeoutTimer = setTimeout(() => { 58 | cleanup() 59 | reject('Timeout') 60 | }, timeout) 61 | 62 | const intervalTimer = setInterval(() => { 63 | try { 64 | const result = cb() 65 | if (result) { 66 | cleanup() 67 | resolve(result) 68 | } 69 | } catch (error) { 70 | cleanup() 71 | reject(error) 72 | } 73 | }, interval) 74 | 75 | const cleanup = () => { 76 | clearTimeout(timeoutTimer) 77 | clearInterval(intervalTimer) 78 | } 79 | }) 80 | } -------------------------------------------------------------------------------- /package.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "node-mavlink", 3 | "version": "2.1.0", 4 | "author": "Matthias Hryniszak ", 5 | "license": "LGPL", 6 | "description": "MavLink definitions and parsing library", 7 | "keywords": [ 8 | "mavlink" 9 | ], 10 | "repository": { 11 | "type": "git", 12 | "url": "git+https://github.com/padcom/node-mavlink.git" 13 | }, 14 | "bugs": { 15 | "email": "padcom@gmail.com", 16 | "url": "https://github.com/padcom/node-mavlink/issues" 17 | }, 18 | "funding": { 19 | "type": "patreon", 20 | "url": "https://www.patreon.com/padcom" 21 | }, 22 | "main": "dist/index.js", 23 | "types": "dist/index.d.ts", 24 | "dependencies": { 25 | "mavlink-mappings": "^1.0.20-20240131-0" 26 | }, 27 | "scripts": { 28 | "clean": "rm -rf dist lib/*.js", 29 | "build": "tsc", 30 | "test": "jest", 31 | "test:batch": "tests/main.js e2e --input tests/data.mavlink", 32 | "dev": "jest --watch", 33 | "test:e2e": "./main.js e2e --input data.mavlink", 34 | "sanity-check": "./sanity-check.cjs && ./sanity-check.mjs", 35 | "prepublishOnly": "npm run clean && npm install && npm test && npm run build && npm run sanity-check" 36 | }, 37 | "devDependencies": { 38 | "@types/jest": "^27.4.1", 39 | "@types/node": "^15.14.9", 40 | "@types/xml2js": "^0.4.8", 41 | "@types/yargs": "^17.0.8", 42 | "jest": "^27.5.1", 43 | "minimize-js": "^1.3.0", 44 | "serialport": "^10.0.0", 45 | "ts-jest": "^27.1.4", 46 | "ts-node": "^10.6.0", 47 | "typescript": "^4.7.4", 48 | "wget-improved": "^3.2.1", 49 | "xml2js": "^0.4.23", 50 | "yargs": "^17.3.1" 51 | } 52 | } 53 | -------------------------------------------------------------------------------- /sanity-check-minimal.cjs: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env node 2 | 3 | const { join } = require('node:path') 4 | const { createReadStream } = require('node:fs') 5 | const { createMavLinkStream } = require('.') 6 | 7 | const port = createReadStream(join(__dirname, 'examples/GH-5.bin')) 8 | const parser = createMavLinkStream(port) 9 | parser.on('data', packet => console.log(packet.debug())) 10 | -------------------------------------------------------------------------------- /sanity-check-minimal.mjs: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env node 2 | 3 | import { join } from 'node:path' 4 | import { fileURLToPath } from 'node:url' 5 | import { createReadStream } from 'node:fs' 6 | import { createMavLinkStream } from './dist/index.js' 7 | 8 | const __dirname = fileURLToPath(new URL('.', import.meta.url)) 9 | const port = createReadStream(join(__dirname, 'examples/GH-5.bin')) 10 | const parser = createMavLinkStream(port) 11 | parser.on('data', packet => console.log(packet.debug())) 12 | -------------------------------------------------------------------------------- /sanity-check.cjs: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env node 2 | 3 | const { join } = require('node:path') 4 | const { createReadStream } = require('node:fs') 5 | const { createMavLinkStream } = require('.') 6 | const { minimal, common, ardupilotmega } = require('.') 7 | 8 | const REGISTRY = { 9 | ...minimal.REGISTRY, 10 | ...common.REGISTRY, 11 | ...ardupilotmega.REGISTRY, 12 | } 13 | 14 | const port = createReadStream(join(__dirname, 'examples/GH-5.bin')) 15 | const parser = createMavLinkStream(port) 16 | parser.on('data', packet => { 17 | const clazz = REGISTRY[packet.header.msgid] 18 | if (clazz) { 19 | const data = packet.protocol.data(packet.payload, clazz) 20 | console.log(packet.header) 21 | console.log(data) 22 | } else { 23 | console.log(packet.debug()) 24 | } 25 | }) 26 | -------------------------------------------------------------------------------- /sanity-check.mjs: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env node 2 | 3 | import { join } from 'node:path' 4 | import { fileURLToPath } from 'node:url' 5 | import { createReadStream } from 'node:fs' 6 | import { createMavLinkStream } from './dist/index.js' 7 | import { minimal, common, ardupilotmega } from './dist/index.js' 8 | 9 | const REGISTRY = { 10 | ...minimal.REGISTRY, 11 | ...common.REGISTRY, 12 | ...ardupilotmega.REGISTRY, 13 | } 14 | 15 | const __dirname = fileURLToPath(new URL('.', import.meta.url)) 16 | const port = createReadStream(join(__dirname, 'examples/GH-5.bin')) 17 | const parser = createMavLinkStream(port) 18 | parser.on('data', packet => { 19 | const clazz = REGISTRY[packet.header.msgid] 20 | if (clazz) { 21 | const data = packet.protocol.data(packet.payload, clazz) 22 | console.log(packet.header) 23 | console.log(data) 24 | } else { 25 | console.log(packet.debug()) 26 | } 27 | }) 28 | -------------------------------------------------------------------------------- /tests/data.mavlink: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ArduPilot/node-mavlink/52735f2f4d3945a983dd370ccc8e209c18ea0397/tests/data.mavlink -------------------------------------------------------------------------------- /tests/main.js: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env -S node 2 | 3 | const yargs = require('yargs') 4 | const { existsSync, createReadStream } = require('fs') 5 | const { minimal, common, ardupilotmega } = require('mavlink-mappings') 6 | const { createMavLinkStream, Logger, LogLevel } = require('..') 7 | const { dump } = require('..') 8 | 9 | Logger.on('log', ({ context, level, message }) => { 10 | if (level <= LogLevel.error) { 11 | console.log(`${new Date().toISOString()} ${context} [${LogLevel[level]}]`, ...message) 12 | } 13 | }) 14 | 15 | async function configure() { 16 | return yargs(process.argv.slice(2)) 17 | .command('e2e', 'Execute end to end serialization/deserialization verification', 18 | yargs => yargs.positional('input', { 19 | description: 'Input file (- for stdin)', 20 | default: '-' 21 | }), 22 | argv => { 23 | if (argv.input !== '-' && !existsSync(argv.input)) { 24 | console.error(`error: ${argv.input} not found`) 25 | process.exit(1) 26 | } 27 | } 28 | ) 29 | .help() 30 | .alias('help', 'h') 31 | .parse() 32 | } 33 | 34 | async function main() { 35 | const config = await configure() 36 | 37 | const command = config._[0] 38 | if (command === 'e2e') { 39 | const REGISTRY = { 40 | ...minimal.REGISTRY, 41 | ...common.REGISTRY, 42 | ...ardupilotmega.REGISTRY, 43 | } 44 | 45 | const input = config.input === '-' ? process.stdin : createReadStream(config.input) 46 | const reader = createMavLinkStream(input, dump) 47 | 48 | reader.on('data', packet => { 49 | const clazz = REGISTRY[packet.header.msgid] 50 | if (clazz) { 51 | packet.protocol.data(packet.payload, clazz) 52 | } else { 53 | console.log('< (unknown)', packet.debug()) 54 | } 55 | }) 56 | } 57 | } 58 | 59 | main() 60 | -------------------------------------------------------------------------------- /tsconfig.json: -------------------------------------------------------------------------------- 1 | { 2 | "compilerOptions": { 3 | /* Visit https://aka.ms/tsconfig.json to read more about this file */ 4 | 5 | /* Projects */ 6 | // "incremental": true, /* Enable incremental compilation */ 7 | // "composite": true, /* Enable constraints that allow a TypeScript project to be used with project references. */ 8 | // "tsBuildInfoFile": "./", /* Specify the folder for .tsbuildinfo incremental compilation files. */ 9 | // "disableSourceOfProjectReferenceRedirect": true, /* Disable preferring source files instead of declaration files when referencing composite projects */ 10 | // "disableSolutionSearching": true, /* Opt a project out of multi-project reference checking when editing. */ 11 | // "disableReferencedProjectLoad": true, /* Reduce the number of projects loaded automatically by TypeScript. */ 12 | 13 | /* Language and Environment */ 14 | "target": "ES2022", /* Set the JavaScript language version for emitted JavaScript and include compatible library declarations. */ 15 | "lib": [ /* Specify a set of bundled library declaration files that describe the target runtime environment. */ 16 | "ES2020", 17 | "DOM" 18 | ], 19 | // "jsx": "preserve", /* Specify what JSX code is generated. */ 20 | // "experimentalDecorators": true, /* Enable experimental support for TC39 stage 2 draft decorators. */ 21 | // "emitDecoratorMetadata": true, /* Emit design-type metadata for decorated declarations in source files. */ 22 | // "jsxFactory": "", /* Specify the JSX factory function used when targeting React JSX emit, e.g. 'React.createElement' or 'h' */ 23 | // "jsxFragmentFactory": "", /* Specify the JSX Fragment reference used for fragments when targeting React JSX emit e.g. 'React.Fragment' or 'Fragment'. */ 24 | // "jsxImportSource": "", /* Specify module specifier used to import the JSX factory functions when using `jsx: react-jsx*`.` */ 25 | // "reactNamespace": "", /* Specify the object invoked for `createElement`. This only applies when targeting `react` JSX emit. */ 26 | // "noLib": true, /* Disable including any library files, including the default lib.d.ts. */ 27 | // "useDefineForClassFields": true, /* Emit ECMAScript-standard-compliant class fields. */ 28 | 29 | /* Modules */ 30 | "module": "None", /* Specify what module code is generated. */ 31 | // "rootDir": "./", /* Specify the root folder within your source files. */ 32 | "moduleResolution": "NodeNext", /* Specify how TypeScript looks up a file from a given module specifier. */ 33 | // "baseUrl": "./", /* Specify the base directory to resolve non-relative module names. */ 34 | // "paths": {}, /* Specify a set of entries that re-map imports to additional lookup locations. */ 35 | // "rootDirs": [], /* Allow multiple folders to be treated as one when resolving modules. */ 36 | // "typeRoots": [], /* Specify multiple folders that act like `./node_modules/@types`. */ 37 | // "types": [], /* Specify type package names to be included without being referenced in a source file. */ 38 | // "allowUmdGlobalAccess": true, /* Allow accessing UMD globals from modules. */ 39 | // "resolveJsonModule": true, /* Enable importing .json files */ 40 | // "noResolve": true, /* Disallow `import`s, `require`s or ``s from expanding the number of files TypeScript should add to a project. */ 41 | 42 | /* JavaScript Support */ 43 | // "allowJs": true, /* Allow JavaScript files to be a part of your program. Use the `checkJS` option to get errors from these files. */ 44 | // "checkJs": true, /* Enable error reporting in type-checked JavaScript files. */ 45 | // "maxNodeModuleJsDepth": 1, /* Specify the maximum folder depth used for checking JavaScript files from `node_modules`. Only applicable with `allowJs`. */ 46 | 47 | /* Emit */ 48 | "declaration": true, /* Generate .d.ts files from TypeScript and JavaScript files in your project. */ 49 | "declarationMap": true, /* Create sourcemaps for d.ts files. */ 50 | // "emitDeclarationOnly": true, /* Only output d.ts files and not JavaScript files. */ 51 | // "sourceMap": true, /* Create source map files for emitted JavaScript files. */ 52 | // "outFile": "./", /* Specify a file that bundles all outputs into one JavaScript file. If `declaration` is true, also designates a file that bundles all .d.ts output. */ 53 | "outDir": "./dist", /* Specify an output folder for all emitted files. */ 54 | // "removeComments": true, /* Disable emitting comments. */ 55 | // "noEmit": true, /* Disable emitting files from a compilation. */ 56 | // "importHelpers": true, /* Allow importing helper functions from tslib once per project, instead of including them per-file. */ 57 | // "importsNotUsedAsValues": "remove", /* Specify emit/checking behavior for imports that are only used for types */ 58 | // "downlevelIteration": true, /* Emit more compliant, but verbose and less performant JavaScript for iteration. */ 59 | // "sourceRoot": "", /* Specify the root path for debuggers to find the reference source code. */ 60 | // "mapRoot": "", /* Specify the location where debugger should locate map files instead of generated locations. */ 61 | // "inlineSourceMap": true, /* Include sourcemap files inside the emitted JavaScript. */ 62 | // "inlineSources": true, /* Include source code in the sourcemaps inside the emitted JavaScript. */ 63 | // "emitBOM": true, /* Emit a UTF-8 Byte Order Mark (BOM) in the beginning of output files. */ 64 | // "newLine": "crlf", /* Set the newline character for emitting files. */ 65 | // "stripInternal": true, /* Disable emitting declarations that have `@internal` in their JSDoc comments. */ 66 | // "noEmitHelpers": true, /* Disable generating custom helper functions like `__extends` in compiled output. */ 67 | // "noEmitOnError": true, /* Disable emitting files if any type checking errors are reported. */ 68 | // "preserveConstEnums": true, /* Disable erasing `const enum` declarations in generated code. */ 69 | // "declarationDir": "./", /* Specify the output directory for generated declaration files. */ 70 | // "preserveValueImports": true, /* Preserve unused imported values in the JavaScript output that would otherwise be removed. */ 71 | 72 | /* Interop Constraints */ 73 | // "isolatedModules": true, /* Ensure that each file can be safely transpiled without relying on other imports. */ 74 | // "allowSyntheticDefaultImports": true, /* Allow 'import x from y' when a module doesn't have a default export. */ 75 | //"esModuleInterop": true, /* Emit additional JavaScript to ease support for importing CommonJS modules. This enables `allowSyntheticDefaultImports` for type compatibility. */ 76 | // "preserveSymlinks": true, /* Disable resolving symlinks to their realpath. This correlates to the same flag in node. */ 77 | "forceConsistentCasingInFileNames": true, /* Ensure that casing is correct in imports. */ 78 | 79 | /* Type Checking */ 80 | "strict": true, /* Enable all strict type-checking options. */ 81 | // "noImplicitAny": true, /* Enable error reporting for expressions and declarations with an implied `any` type.. */ 82 | // "strictNullChecks": true, /* When type checking, take into account `null` and `undefined`. */ 83 | // "strictFunctionTypes": true, /* When assigning functions, check to ensure parameters and the return values are subtype-compatible. */ 84 | // "strictBindCallApply": true, /* Check that the arguments for `bind`, `call`, and `apply` methods match the original function. */ 85 | // "strictPropertyInitialization": true, /* Check for class properties that are declared but not set in the constructor. */ 86 | // "noImplicitThis": true, /* Enable error reporting when `this` is given the type `any`. */ 87 | // "useUnknownInCatchVariables": true, /* Type catch clause variables as 'unknown' instead of 'any'. */ 88 | // "alwaysStrict": true, /* Ensure 'use strict' is always emitted. */ 89 | // "noUnusedLocals": true, /* Enable error reporting when a local variables aren't read. */ 90 | // "noUnusedParameters": true, /* Raise an error when a function parameter isn't read */ 91 | // "exactOptionalPropertyTypes": true, /* Interpret optional property types as written, rather than adding 'undefined'. */ 92 | // "noImplicitReturns": true, /* Enable error reporting for codepaths that do not explicitly return in a function. */ 93 | // "noFallthroughCasesInSwitch": true, /* Enable error reporting for fallthrough cases in switch statements. */ 94 | // "noUncheckedIndexedAccess": true, /* Include 'undefined' in index signature results */ 95 | // "noImplicitOverride": true, /* Ensure overriding members in derived classes are marked with an override modifier. */ 96 | // "noPropertyAccessFromIndexSignature": true, /* Enforces using indexed accessors for keys declared using an indexed type */ 97 | // "allowUnusedLabels": true, /* Disable error reporting for unused labels. */ 98 | // "allowUnreachableCode": true, /* Disable error reporting for unreachable code. */ 99 | 100 | /* Completeness */ 101 | // "skipDefaultLibCheck": true, /* Skip type checking .d.ts files that are included with TypeScript. */ 102 | "skipLibCheck": true /* Skip type checking all .d.ts files. */ 103 | }, 104 | "exclude": [ 105 | "**/*.test.ts", 106 | "**/*.d.ts", 107 | "examples/**/*" 108 | ] 109 | } 110 | --------------------------------------------------------------------------------