├── .dockerignore ├── .github └── workflows │ ├── deploy.yml │ └── test.yml ├── .gitignore ├── Cargo.toml ├── Dockerfile ├── HOWTO-change-mavlink-msg.md ├── README.md ├── build.rs ├── doc └── logo.png ├── examples └── websocket_client.py ├── license ├── src ├── cli.rs ├── data.rs ├── endpoints.rs ├── html │ ├── index.html │ └── watcher.html ├── main.rs ├── mavlink_vehicle.rs ├── message_information.rs ├── server.rs ├── vehicle_handler.rs └── websocket_manager.rs └── tests └── run.py /.dockerignore: -------------------------------------------------------------------------------- 1 | Dockerfile 2 | README.md -------------------------------------------------------------------------------- /.github/workflows/deploy.yml: -------------------------------------------------------------------------------- 1 | name: Deploy mavlink2rest 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | build: 7 | runs-on: ${{ matrix.os }} 8 | strategy: 9 | fail-fast: false 10 | matrix: 11 | include: 12 | - os: macos-latest 13 | TARGET: aarch64-apple-darwin 14 | 15 | - os: macos-latest 16 | TARGET: x86_64-apple-darwin 17 | 18 | - os: ubuntu-latest 19 | TARGET: arm-unknown-linux-musleabihf 20 | 21 | - os: ubuntu-latest 22 | TARGET: aarch64-unknown-linux-musl 23 | 24 | - os: ubuntu-latest 25 | TARGET: armv7-unknown-linux-musleabihf 26 | 27 | - os: ubuntu-latest 28 | TARGET: x86_64-unknown-linux-musl 29 | 30 | - os: windows-latest 31 | TARGET: x86_64-pc-windows-msvc 32 | EXTENSION: .exe 33 | 34 | steps: 35 | - name: Building ${{ matrix.TARGET }} 36 | run: echo "${{ matrix.TARGET }}" 37 | 38 | - uses: actions/checkout@master 39 | - uses: actions-rs/toolchain@v1.0.1 40 | with: 41 | toolchain: stable 42 | target: ${{ matrix.TARGET }} 43 | override: true 44 | 45 | - uses: actions-rs/cargo@v1 46 | with: 47 | use-cross: true 48 | command: build 49 | args: --verbose --release --target=${{ matrix.TARGET }} 50 | 51 | - name: Rename 52 | run: cp target/${{ matrix.TARGET }}/release/mavlink2rest${{ matrix.EXTENSION }} mavlink2rest-${{ matrix.TARGET }}${{ matrix.EXTENSION }} 53 | 54 | - uses: actions/upload-artifact@master 55 | with: 56 | name: mavlink2rest-${{ matrix.TARGET }}${{ matrix.EXTENSION }} 57 | path: mavlink2rest-${{ matrix.TARGET }}${{ matrix.EXTENSION }} 58 | 59 | - uses: svenstaro/upload-release-action@v2 60 | name: Upload binaries to release 61 | if: ${{ github.event_name == 'push' }} 62 | with: 63 | repo_token: ${{ secrets.GITHUB_TOKEN }} 64 | file: mavlink2rest-${{ matrix.TARGET }}${{ matrix.EXTENSION }} 65 | asset_name: mavlink2rest-${{ matrix.TARGET }}${{ matrix.EXTENSION }} 66 | tag: ${{ github.ref }} 67 | prerelease: ${{ !startsWith(github.ref, 'refs/tags/') }} 68 | overwrite: true 69 | -------------------------------------------------------------------------------- /.github/workflows/test.yml: -------------------------------------------------------------------------------- 1 | name: Test 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | pull_request: 7 | branches: [ master ] 8 | 9 | env: 10 | CARGO_TERM_COLOR: always 11 | 12 | jobs: 13 | build: 14 | 15 | runs-on: ubuntu-latest 16 | 17 | steps: 18 | - uses: actions/checkout@v2 19 | - uses: actions/setup-python@v2 20 | with: 21 | python-version: '3.8' 22 | - name: Check code style 23 | run: cargo fmt -- --check 24 | - name: Build 25 | run: cargo build --verbose 26 | - name: Run SITL & MAVLink2Rest 27 | if: ${{ false }} # Disable step until TCP connection is fixed 28 | timeout-minutes: 5 29 | run: | 30 | pip install --user aiohttp asyncio requests 31 | wget https://firmware.ardupilot.org/Sub/stable-4.0.2/SITL_x86_64_linux_gnu/ardusub 32 | chmod +x ardusub 33 | ./ardusub --model vectored & 34 | sleep 5 35 | cargo run -- --connect tcpout:0.0.0.0:5760 & 36 | sleep 5 37 | ./tests/run.py 38 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | /target 2 | **/*.rs.bk 3 | /src/html 4 | Cargo.lock -------------------------------------------------------------------------------- /Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "mavlink2rest" 3 | version = "0.11.23" 4 | description = "A simple REST API for MAVLink" 5 | readme = "README.md" 6 | license = "MIT" 7 | authors = ["Patrick José Pereira "] 8 | edition = "2018" 9 | path = "build.rs" 10 | 11 | documentation = "https://github.com/patrickelectric/mavlink2rest" 12 | homepage = "https://github.com/patrickelectric/mavlink2rest" 13 | repository = "https://github.com/patrickelectric/mavlink2rest" 14 | 15 | [[bin]] 16 | bench = false 17 | path = "src/main.rs" 18 | name = "mavlink2rest" 19 | 20 | # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html 21 | 22 | [dependencies] 23 | actix = "0.10" 24 | actix-cors = "0.5" 25 | actix-files = "0.6.2" 26 | actix-rt = "2.1" 27 | actix-web = "3.3" 28 | actix-web-actors = "3.0" 29 | chrono = { version = "0.4", features = ["serde"] } 30 | clap = "2.33.3" 31 | derivative = "2.1.1" 32 | include_dir = "0.7" 33 | lazy_static = "1.4.0" 34 | log = "0.4" 35 | env_logger = "0.8" 36 | mavlink = { version = "0.10.11", features = [ "ardupilotmega", "emit-extensions"] } 37 | paperclip = { version = "0.8", features = ["actix3", "v3", "paperclip-actix", "swagger-ui"] } 38 | regex = "1" 39 | serde = "1.0.115" 40 | serde_derive = "1.0.115" 41 | serde_json = "1.0.57" 42 | json5 = "0.4.1" 43 | 44 | [build-dependencies] 45 | reqwest = {version = "0.11", default-features = false, features = ["blocking", "rustls-tls"]} 46 | vergen = { version = "7", features = ["git"] } 47 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | # Build 2 | FROM rust:alpine as builder 3 | ARG TARGET_ARCH=x86_64-unknown-linux-musl 4 | 5 | RUN apk add --no-cache musl-dev alpine-sdk 6 | WORKDIR /usr/src/mavlink2rest 7 | COPY . . 8 | 9 | RUN rustup target add ${TARGET_ARCH} 10 | 11 | RUN cargo build --release --target=${TARGET_ARCH} 12 | 13 | 14 | # Runtime environment 15 | FROM alpine 16 | WORKDIR /root/ 17 | 18 | ARG TARGET_ARCH=x86_64-unknown-linux-musl 19 | 20 | COPY --from=builder /usr/src/mavlink2rest/target/${TARGET_ARCH}/release/mavlink2rest ./mavlink2rest 21 | 22 | ENV MAVLINK_SRC="udpin:0.0.0.0:14550" 23 | ENV SERVER_PORT="0.0.0.0:8088" 24 | ENV EXTRA_ARGS="" 25 | 26 | RUN chmod +x mavlink2rest 27 | 28 | ENTRYPOINT ./mavlink2rest -c ${MAVLINK_SRC} -s ${SERVER_PORT} ${EXTRA_ARGS} 29 | -------------------------------------------------------------------------------- /HOWTO-change-mavlink-msg.md: -------------------------------------------------------------------------------- 1 | ### Updating MAVLink Message Set 2 | 3 | To update the MAVLink message set, you will need to rebuild both mavlink2rest and [rust-mavlink](https://github.com/mavlink/rust-mavlink). As of August 2023, this process is not straightforward and requires following specific instructions. 4 | 5 | Firstly, you need to set up a working RUST environment. Follow the official instructions at https://www.rust-lang.org/ to set up your Rust environment. 6 | 7 | Next, obtain the source code of rust-mavlink and update its MAVLink submodule to acquire the correct MAVLink revision: 8 | ```sh 9 | git clone git@github.com:mavlink/rust-mavlink.git --recursive 10 | ``` 11 | 12 | Retrieve the source code of mavlink2rest as well: 13 | ```sh 14 | git clone https://github.com/khancyr/mavlink2rest --recursive 15 | ``` 16 | 17 | Select the version of mavlink2rest you desire. Open the `Cargo.toml` file to determine the corresponding version of rust-mavlink. Then, checkout the desired version of rust-mavlink, for example: 18 | ```sh 19 | cd rust-mavlink/ 20 | git checkout 0.10.2 21 | ``` 22 | 23 | Now, you can edit the MAVLink message definitions located in `mavlink/message_definitions/v1.0`. 24 | 25 | Compile rust-mavlink with the desired message set you wish to support. In this example, we'll use the default `ardupilotmega.xml`: 26 | ```sh 27 | cargo install --path . --features="ardupilotmega emit-extensions" 28 | ``` 29 | 30 | Once the compilation is finished, return to the `Cargo.toml` file of mavlink2rest and update the path to the local rust-mavlink library. 31 | 32 | Before: 33 | ```toml 34 | mavlink = { git = "https://github.com/mavlink/rust-mavlink", rev = "0.10.2", features = [ "ardupilotmega", "emit-extensions"] } 35 | ``` 36 | 37 | After: 38 | ```toml 39 | mavlink = { path="/home/khancyr/Workspace/rust-mavlink", features = [ "ardupilotmega", "emit-extensions"] } 40 | ``` 41 | 42 | This change points to the updated rust-mavlink library containing the updated MAVLink messages. 43 | 44 | Finally, compile mavlink2rest: 45 | ```sh 46 | cargo install --path . 47 | ``` 48 | 49 | Your newly compiled mavlink2rest will now comprehend your newly added messages. 50 | 51 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ![](./doc/logo.png) 2 | 3 | [![Deploy mavlink2rest](https://github.com/mavlink/mavlink2rest/actions/workflows/deploy.yml/badge.svg)](https://github.com/mavlink/mavlink2rest/actions/workflows/deploy.yml) 4 | [![Test](https://github.com/mavlink/mavlink2rest/actions/workflows/test.yml/badge.svg)](https://github.com/mavlink/mavlink2rest/actions/workflows/test.yml) 5 | [![Cargo download](https://img.shields.io/crates/d/mavlink2rest)](https://crates.io/crates/mavlink2rest) 6 | [![Crate info](https://img.shields.io/crates/v/mavlink2rest.svg)](https://crates.io/crates/mavlink2rest) 7 | [![Documentation](https://docs.rs/mavlink2rest/badge.svg)](https://docs.rs/mavlink2rest) 8 | 9 | `mavlink2rest` is a tool that offers a RESTful API over the MAVLink protocol, facilitating seamless communication between unmanned systems and web applications. The tool supports the ArduPilotMega dialect, iCAROUS, and UAVionix, making it an ideal solution for developers who want to build custom interfaces for unmanned systems. 10 | 11 | The current version supports the **ardupilotmega** dialect, that includes **common**, **icarous** and **uavionix**. 12 | 13 | ## Grab it 14 | ### Downloads :package: 15 | 16 | [Latest builds](https://github.com/mavlink/mavlink2rest/releases/latest): 17 | - :computer: [**Windows**](https://github.com/mavlink/mavlink2rest/releases/latest/download/mavlink2rest-x86_64-pc-windows-msvc.exe) 18 | - :apple: [**MacOS**](https://github.com/mavlink/mavlink2rest/releases/latest/download/mavlink2rest-x86_64-apple-darwin) 19 | - :penguin: [**Linux**](https://github.com/mavlink/mavlink2rest/releases/latest/download/mavlink2rest-x86_64-unknown-linux-musl) 20 | - :strawberry: [**Raspberry**](https://github.com/mavlink/mavlink2rest/releases/latest/download/mavlink2rest-arm-unknown-linux-musleabihf) 21 | - [ARMv6 binary](https://github.com/mavlink/mavlink2rest/releases/latest/download/mavlink2rest-arm-unknown-linux-musleabihf), [ARMv7](https://github.com/mavlink/mavlink2rest/releases/latest/download/mavlink2rest-armv7-unknown-linux-musleabihf) is also available under the project releases. 22 | 23 | For others or different releases, check the [releases menu](https://github.com/mavlink/mavlink2rest/releases). 24 | 25 | ### Install :zap: 26 | If you prefer, you can install via cargo, if you don't know what it is, use the [download section](https://github.com/mavlink/mavlink2rest#downloads-package). 27 | - :gear: Cargo Install: `cargo install mavlink2rest` 28 | 29 | ## Help 30 | Capabilities via the command line: 31 | ``` 32 | USAGE: 33 | mavlink2rest [FLAGS] [OPTIONS] 34 | 35 | FLAGS: 36 | -h, --help Prints help information 37 | -V, --version Prints version information 38 | -v, --verbose Be verbose 39 | 40 | OPTIONS: 41 | --component-id 42 | Sets the component ID for this service, for more information, check: 43 | https://mavlink.io/en/messages/common.html#MAV_COMPONENT [default: 0] 44 | -c, --connect :> 45 | Sets the mavlink connection string [default: udpin:0.0.0.0:14550] 46 | 47 | --mavlink 48 | Sets the mavlink version used to communicate [default: 2] [possible values: 1, 2] 49 | 50 | -s, --server 51 | Sets the IP and port that the rest server will be provided [default: 0.0.0.0:8088] 52 | 53 | --system-id Sets system ID for this service. [default: 255] 54 | ``` 55 | # Using Docker with mavlink2rest 56 | 57 | You can also use the `mavlink2rest` with docker, the following command will start the service with the default settings: 58 | ```sh 59 | docker run --rm --init -p 8088:8088 -p 14550:14550/udp --name mavlink2rest mavlink/mavlink2rest 60 | ``` 61 | 62 | The Dockerfile defines several environment variables that you can override at runtime: 63 | 64 | MAVLINK_SRC: The MAVLink source connection string. Default is udpin:127.0.0.1:14550. 65 | SERVER_PORT: The IP and port for the REST server. Default is 0.0.0.0:8088. 66 | EXTRA_ARGS: Any additional command line arguments you want to pass to mavlink2rest. 67 | To customize these settings, use the -e flag with docker run: 68 | 69 | ```sh 70 | docker run --rm --init\ 71 | -p 8088:8088 \ 72 | -p 14551:14551/udp \ 73 | -e MAVLINK_SRC="udpin:0.0.0.0:14551" \ 74 | -e SERVER_PORT="0.0.0.0:8088" \ 75 | --name mavlink2rest mavlink/mavlink2rest 76 | ``` 77 | 78 | to build the docker image locally, you can use the following command: 79 | ```sh 80 | docker build --build-arg TARGET_ARCH=x86_64-unknown-linux-musl -t mavlink/mavlink2rest . 81 | ``` 82 | 83 | ## Endpoints 84 | 85 | ### Pages 86 | * Main webpage: `GET /` 87 | * Provides information about mavlink2rest and available messages. 88 | * Swagger: `GET /docs` 89 | * Provides information about mavlink2rest endpoints for the REST API. 90 | 91 | ### API 92 | * MAVLink JSON: 93 | * `GET /v1/mavlink|/v1/mavlink/*`. The output is a JSON that you get each nested key individually, E.g: 94 | * http://0.0.0.0:8088/v1/mavlink/vehicles/1/components/1/messages/ATTITUDE 95 | * http://0.0.0.0:8088/v1/mavlink/vehicles/1/components/1/messages/ATTITUDE/roll 96 | * http://0.0.0.0:8088/v1/mavlink/vehicles/1/components/1/messages/ATTITUDE/status/time/last_update 97 | * Any MAVLink message will contain a normal message definition, as described in `GET /v1/helper/mavlink?name=`.. 98 | * http://0.0.0.0:8088/v1/helper/mavlink?name=HEARTBEAT 99 | ```js 100 | { 101 | "header": { 102 | "system_id": 255, 103 | "component_id": 0, 104 | "sequence": 0 105 | }, 106 | "message": { 107 | "type": "HEARTBEAT", 108 | "custom_mode": 0, 109 | "mavtype": { 110 | "type": "MAV_TYPE_GENERIC" 111 | }, 112 | "autopilot": { 113 | "type": "MAV_AUTOPILOT_GENERIC" 114 | }, 115 | "base_mode": { 116 | "bits": 128 117 | }, 118 | "system_status": { 119 | "type": "MAV_STATE_UNINIT" 120 | }, 121 | "mavlink_version": 0 122 | } 123 | } 124 | ``` 125 | > :sunglasses: This is really hand when creating messages. 126 | * ... and a **status** structure defined as: 127 | ```js 128 | "status": { 129 | "time": { 130 | "counter": 2981, 131 | "first_update": "2024-05-31T11:59:44.313941926-03:00", 132 | "frequency": 10.037036895751953, 133 | "last_update": "2024-05-31T12:04:42.214212884-03:00" 134 | } 135 | } 136 | ``` 137 | * `POST /mavlink`. Sends the message to a specific vehicle. 138 | * For more information about the MAVLink message definition: https://mavlink.io/en/guide/serialization.html 139 | * **header**: Is the mavlink header definition with `system_id`, `component_id` and `sequence`. 140 | * **message**: A valid mavlink [message](https://mavlink.io/en/messages/common.html), for more information check `GET /v1/helper/mavlink?name=`. 141 | * Check [ARM/DISARM example](https://github.com/mavlink/mavlink2rest#examples). 142 | 143 | * `GET /v1/helper/mavlink?name=MAVLINK_MESSAGE_NAME`: Helper endpoint to create JSON compatible MAVLink messages, where `MAVLINK_MESSAGE_NAME` is the mavlink message name. E.g: 144 | * http://0.0.0.0:8088/v1/helper/mavlink?name=COMMAND_LONG 145 | ```js 146 | { 147 | "header": { 148 | "system_id": 255, 149 | "component_id": 0, 150 | "sequence": 0 151 | }, 152 | "message": { 153 | "type": "COMMAND_LONG", 154 | "param1": 0.0, 155 | "param2": 0.0, 156 | "param3": 0.0, 157 | "param4": 0.0, 158 | "param5": 0.0, 159 | "param6": 0.0, 160 | "param7": 0.0, 161 | "command": { 162 | "type": "MAV_CMD_NAV_WAYPOINT" // Random value 163 | }, 164 | "target_system": 0, 165 | "target_component": 0, 166 | "confirmation": 0 167 | } 168 | } 169 | ``` 170 | * Information: 171 | * `GET /info`, provides information about the service version. 172 | * http://0.0.0.0:8088/info 173 | ```js 174 | { 175 | "version": 0, 176 | "service": { 177 | "name": "mavlink2rest", 178 | "version": "0.10.0", 179 | "sha": "bd7667d", 180 | "build_date": "2021-03-03", 181 | "authors": "Author " 182 | } 183 | } 184 | ``` 185 | 186 | 187 | #### Examples 188 | 189 | ##### Get all messages: 190 | ```sh 191 | curl --request GET http://0.0.0.0:8088/v1/mavlink 192 | # The output is huge, you can get it here: [https://gist.github.com/patrickelectric/26a407c4e7749cdaa58d06b52212cb1e](https://gist.github.com/patrickelectric/26a407c4e7749cdaa58d06b52212cb1e) 193 | ``` 194 | 195 | ##### Get attitude: 196 | ```sh 197 | curl --request GET http://0.0.0.0:8088/v1/mavlink/vehicles/1/components/1/messages/ATTITUDE 198 | ``` 199 | ```js 200 | { 201 | "message": { 202 | "pitch": 0.11506611853837967, 203 | "pitchspeed": 0.00003909762017428875, 204 | "roll": 0.02339238114655018, 205 | "rollspeed": 0.00035849903360940516, 206 | "time_boot_ms": 87110407, 207 | "type": "ATTITUDE", 208 | "yaw": -2.4364013671875, 209 | "yawspeed": 0.000020137056708335876 210 | }, 211 | "status": { 212 | "time": { 213 | "counter": 22750, 214 | "first_update": "2024-05-31T11:59:44.313941926-03:00", 215 | "frequency": 5.495169162750244, 216 | "last_update": "2024-05-31T13:08:45.196118069-03:00" 217 | } 218 | } 219 | } 220 | ```` 221 | 222 | ##### Get time of last *ATTITUDE* message: 223 | ```sh 224 | curl --request GET http://0.0.0.0:8088/v1/mavlink/ATTITUDE/status/time/last_update 225 | ``` 226 | ```js 227 | "2020-03-28T14:28:51.577853-03:00" 228 | ``` 229 | 230 | ##### Get a message structure example: 231 | ```sh 232 | curl --request GET http://0.0.0.0:8088/v1/helper/mavlink?name=ATTITUDE 233 | ``` 234 | ```js 235 | { 236 | "header": { 237 | "system_id": 255, 238 | "component_id": 0, 239 | "sequence": 0 240 | }, 241 | "message": { 242 | "type": "ATTITUDE", 243 | "time_boot_ms": 0, 244 | "roll": 0.0, 245 | "pitch": 0.0, 246 | "yaw": 0.0, 247 | "rollspeed": 0.0, 248 | "pitchspeed": 0.0, 249 | "yawspeed": 0.0 250 | } 251 | } 252 | ``` 253 | 254 | ##### Request vehicle to be [armed](https://mavlink.io/en/messages/common.html#MAV_CMD_COMPONENT_ARM_DISARM): 255 | ```sh 256 | # ARM: param1 is 1.0 257 | curl --request POST http://0.0.0.0:8088/v1/mavlink -H "Content-Type: application/json" --data \ 258 | '{ 259 | "header": { 260 | "system_id": 255, 261 | "component_id": 240, 262 | "sequence": 0 263 | }, 264 | "message": { 265 | "type":"COMMAND_LONG", 266 | "param1": 1.0, 267 | "param2": 0.0,"param3":0.0,"param4":0.0,"param5":0.0,"param6":0.0,"param7":0.0, 268 | "command": { 269 | "type": "MAV_CMD_COMPONENT_ARM_DISARM" 270 | }, 271 | "target_system": 1, 272 | "target_component": 1, 273 | "confirmation": 1 274 | } 275 | }' 276 | ``` 277 | 278 | ##### Request vehicle to be [disarmed](https://mavlink.io/en/messages/common.html#MAV_CMD_COMPONENT_ARM_DISARM): 279 | ```sh 280 | # ARM: param1 is 0.0 281 | curl --request POST http://0.0.0.0:8088/v1/mavlink -H "Content-Type: application/json" --data \ 282 | '{ 283 | "header": { 284 | "system_id": 255, 285 | "component_id": 240, 286 | "sequence": 0 287 | }, 288 | "message": { 289 | "type":"COMMAND_LONG", 290 | "param1": 0.0, 291 | "param2": 0.0,"param3":0.0,"param4":0.0,"param5":0.0,"param6":0.0,"param7":0.0, 292 | "command": { 293 | "type": "MAV_CMD_COMPONENT_ARM_DISARM" 294 | }, 295 | "target_system": 1, 296 | "target_component": 1, 297 | "confirmation": 1 298 | } 299 | }' 300 | ``` 301 | 302 | > Note: For any invalid `GET`, you'll receive a 404 response with the error message. 303 | > Note: The endpoints that allow `GET` and provides a JSON output, also allow the usage of the query parameter `pretty` with a boolean value `true` or `false`, E.g: http://0.0.0.0:8088/helper/mavlink?name=COMMAND_LONG&pretty=true 304 | 305 | ### Websocket 306 | 307 | It's also possible to connect multiple websockets with the following path `/ws/mavlink`, the endpoint also accepts the query parameter `filter`, the filter value should be a regex that matches MAVLink message names, E.g: `/ws/mavlink?filter=.*` for all messages, `/ws/mavlink?filter=RC_.*` will match **RC_CHANNELS_RAW** and **RC_CHANNELS**, resulting in the following output: 308 | ```json 309 | { // First message 310 | "header": { 311 | "component_id": 1, 312 | "sequence": 98, 313 | "system_id": 1 314 | }, 315 | "message": { 316 | "chan10_raw": 0, 317 | "chan11_raw": 0, 318 | "chan12_raw": 0, 319 | "chan13_raw": 0, 320 | "chan14_raw": 0, 321 | "chan15_raw": 0, 322 | "chan16_raw": 0, 323 | "chan17_raw": 0, 324 | "chan18_raw": 0, 325 | "chan1_raw": 1500, 326 | "chan2_raw": 1500, 327 | "chan3_raw": 1500, 328 | "chan4_raw": 1500, 329 | "chan5_raw": 1500, 330 | "chan6_raw": 1500, 331 | "chan7_raw": 1500, 332 | "chan8_raw": 1500, 333 | "chan9_raw": 0, 334 | "chancount": 16, 335 | "message_information": { 336 | "counter": 3732, 337 | "frequency": 4.0, 338 | "time": { 339 | "first_message": "2020-09-01T20:36:24.088099-03:00", 340 | "last_message": "2020-09-01T20:51:57.278901-03:00" 341 | } 342 | }, 343 | "rssi": 0, 344 | "time_boot_ms": 3122812, 345 | "type": "RC_CHANNELS" 346 | } 347 | } 348 | { // Second message 349 | "header": { 350 | "component_id": 1, 351 | "sequence": 98, 352 | "system_id": 1 353 | }, 354 | "message": { 355 | "chan1_raw": 1500, 356 | "chan2_raw": 1500, 357 | "chan3_raw": 1500, 358 | "chan4_raw": 1500, 359 | "chan5_raw": 1500, 360 | "chan6_raw": 1500, 361 | "chan7_raw": 1500, 362 | "chan8_raw": 1500, 363 | "message_information": { 364 | "counter": 3732, 365 | "frequency": 4.0, 366 | "time": { 367 | "first_message": "2020-09-01T20:36:24.088310-03:00", 368 | "last_message": "2020-09-01T20:51:57.279438-03:00" 369 | } 370 | }, 371 | "port": 0, 372 | "rssi": 0, 373 | "time_boot_ms": 3122812, 374 | "type": "RC_CHANNELS_RAW" 375 | } 376 | } 377 | ``` 378 | For a demonstration, please check the example under the examples filder: `websocket_client.py` 379 | 380 | # Benchmark 381 | The following benchmarks were extracted from a raspberry pi 3 connected to a pixhawk running ArduSub. 382 | - In idle. 383 | ``` 384 | 6% CPU usage 385 | ``` 386 | - 1 client requesting all mavlink messages at 10Hz 387 | ``` 388 | 9% CPU usage 389 | ``` 390 | - 1 client requesting all mavlink messages at 100Hz 391 | ``` 392 | 20% CPU usage (~5% each core) 393 | ``` 394 | - 1 websocket with no filters 395 | ``` 396 | 11% CPU usage 397 | ``` 398 | - 5 websockets with no filters 399 | ``` 400 | 24% CPU usage (14% @ 1 core, ~3% @ 3 cores) 401 | ``` 402 | - 20 websockets with filter only for **ATTITUDE** message (receiving at 10Hz) 403 | ``` 404 | 9% CPU usage 405 | ``` 406 | - 20 websockets with filter only for **NAMED_VALUE_FLOAT** message (receiving at 70Hz) 407 | ``` 408 | 17% CPU usage (9% @ 1 core, ~2% @ 3 cores) 409 | ``` 410 | - 20 websockets with no filters 411 | ``` 412 | 48% CPU usage (20% @ 1 core, ~9% @ 3 cores) 413 | ``` 414 | - 1 client requesting all mavlink messages 1000 times 415 | ``` 416 | 60% CPU usage (~15% each core) 417 | Time taken for tests 3.7 seconds 418 | Total requests 1000 419 | Successful requests 1000 420 | Failed requests 0 421 | Requests per second 273.60 [#/sec] 422 | Median time per request 3ms 423 | Average time per request 4ms 424 | ``` 425 | 426 | - 10 clients requesting all mavlink messages, 100 requests for each client. 427 | ``` 428 | 140% CPU usage (~46% each core) 429 | Time taken for tests 1.4 seconds 430 | Total requests 1000 431 | Successful requests 1000 432 | Failed requests 0 433 | Requests per second 733.14 [#/sec] 434 | Median time per request 13ms 435 | Average time per request 13ms 436 | Sample standard deviation 3ms 437 | ``` 438 | 439 | - 100 clients requesting all mavlink messages, 1000 requests for each client. 440 | ``` 441 | 140% CPU usage (~46% each core) 442 | Time taken for tests 13.8 seconds 443 | Total requests 10000 444 | Successful requests 10000 445 | Failed requests 0 446 | Requests per second 725.83 [#/sec] 447 | Median time per request 132ms 448 | Average time per request 137ms 449 | Sample standard deviation 54ms 450 | ``` 451 | -------------------------------------------------------------------------------- /build.rs: -------------------------------------------------------------------------------- 1 | use std::{fs::File, io, path::Path}; 2 | 3 | use vergen::{vergen, Config}; 4 | 5 | fn main() { 6 | // Generate the 'cargo:' key output 7 | vergen(Config::default()).expect("Something is wrong!"); 8 | 9 | let artifacts_dir = Path::new(env!("CARGO_MANIFEST_DIR")).join("src/html/"); 10 | std::fs::create_dir_all(&artifacts_dir).expect("failed to create a dir"); 11 | 12 | for remote_file in [ 13 | "https://unpkg.com/vue@3.0.5/dist/vue.global.js", 14 | "https://unpkg.com/highlight.js@10.6.0/styles/github.css", 15 | "https://cdnjs.cloudflare.com/ajax/libs/highlight.js/10.6.0/highlight.min.js", 16 | ] { 17 | download_file(remote_file, &artifacts_dir); 18 | } 19 | } 20 | 21 | fn download_file(remote_file: &str, dir: &Path) { 22 | let mut resp = reqwest::blocking::get(remote_file) 23 | .unwrap_or_else(|_| panic!("Failed to download vue file: {}", remote_file)); 24 | 25 | let filename = remote_file.split('/').last().unwrap(); 26 | let file_path = dir.join(filename); 27 | let mut output_file = File::create(&file_path) 28 | .unwrap_or_else(|_| panic!("Failed to create artifact file: {:?}", file_path)); 29 | 30 | io::copy(&mut resp, &mut output_file).expect("Failed to copy content."); 31 | } 32 | -------------------------------------------------------------------------------- /doc/logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/patrickelectric/mavlink2rest/5ec6c705d81e06a8ebb688649de1738de31458a1/doc/logo.png -------------------------------------------------------------------------------- /examples/websocket_client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import argparse 3 | import asyncio 4 | import aiohttp 5 | 6 | 7 | async def start_client(url: str) -> None: 8 | print("Connecting to: ", url) 9 | async with aiohttp.ClientSession() as session: 10 | ws = await session.ws_connect(url, autoclose=False, autoping=False) 11 | 12 | async def dispatch(): 13 | while True: 14 | msg = await ws.receive() 15 | 16 | if msg.type == aiohttp.WSMsgType.TEXT: 17 | print("Text: ", msg.data.strip()) 18 | elif msg.type == aiohttp.WSMsgType.BINARY: 19 | print("Binary: ", msg.data) 20 | elif msg.type == aiohttp.WSMsgType.PING: 21 | await ws.pong() 22 | elif msg.type == aiohttp.WSMsgType.PONG: 23 | print("Pong received") 24 | else: 25 | if msg.type == aiohttp.WSMsgType.CLOSE: 26 | await ws.close() 27 | elif msg.type == aiohttp.WSMsgType.ERROR: 28 | print("Error during receive %s" % ws.exception()) 29 | elif msg.type == aiohttp.WSMsgType.CLOSED: 30 | pass 31 | 32 | break 33 | 34 | await dispatch() 35 | 36 | 37 | ARGS = argparse.ArgumentParser( 38 | description="websocket console client for wssrv.py example." 39 | ) 40 | ARGS.add_argument( 41 | "--filter", 42 | action="store", 43 | dest="filter", 44 | default=".*", 45 | help="Regex filter or message name used on websocket: ATTITUDE,HEARTBEAT,RAW_IMU", 46 | ) 47 | ARGS.add_argument( 48 | "--url", 49 | action="store", 50 | dest="url", 51 | default="http://blueos.local:6040", 52 | help="Websocket address, follow the format: http://0.0.0.0:8088", 53 | ) 54 | 55 | if __name__ == "__main__": 56 | args = ARGS.parse_args() 57 | 58 | loop = asyncio.new_event_loop() 59 | loop.run_until_complete( 60 | start_client( 61 | f"{args.url}/v1/ws/mavlink?filter={args.filter}" 62 | ) 63 | ) 64 | -------------------------------------------------------------------------------- /license: -------------------------------------------------------------------------------- 1 | Copyright (c) 2012-2023 Scott Chacon and others 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining 4 | a copy of this software and associated documentation files (the 5 | "Software"), to deal in the Software without restriction, including 6 | without limitation the rights to use, copy, modify, merge, publish, 7 | distribute, sublicense, and/or sell copies of the Software, and to 8 | permit persons to whom the Software is furnished to do so, subject to 9 | the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be 12 | included in all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 15 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 16 | MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 17 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE 18 | LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 19 | OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 20 | WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 21 | -------------------------------------------------------------------------------- /src/cli.rs: -------------------------------------------------------------------------------- 1 | use lazy_static::lazy_static; 2 | use std::sync::Arc; 3 | 4 | #[derive(Debug)] 5 | struct Manager<'a> { 6 | clap_matches: clap::ArgMatches<'a>, 7 | } 8 | 9 | lazy_static! { 10 | static ref MANAGER: Arc> = Arc::new(Manager::new()); 11 | } 12 | 13 | impl Manager<'_> { 14 | fn new() -> Self { 15 | Self { 16 | clap_matches: get_clap_matches(), 17 | } 18 | } 19 | } 20 | 21 | pub fn init() { 22 | MANAGER.as_ref(); 23 | } 24 | 25 | pub fn is_verbose() -> bool { 26 | return MANAGER.as_ref().clap_matches.is_present("verbose"); 27 | } 28 | 29 | pub fn mavlink_connection_string() -> &'static str { 30 | return MANAGER.as_ref().clap_matches.value_of("connect").unwrap(); 31 | } 32 | 33 | pub fn server_address() -> &'static str { 34 | return MANAGER.as_ref().clap_matches.value_of("server").unwrap(); 35 | } 36 | 37 | pub fn default_api_version() -> u8 { 38 | return MANAGER 39 | .as_ref() 40 | .clap_matches 41 | .value_of("default-api-version") 42 | .unwrap() 43 | .parse::() 44 | .unwrap(); 45 | } 46 | 47 | pub fn mavlink_version() -> u8 { 48 | return MANAGER 49 | .as_ref() 50 | .clap_matches 51 | .value_of("mavlink") 52 | .unwrap() 53 | .parse::() 54 | .unwrap(); 55 | } 56 | 57 | pub fn mavlink_system_and_component_id() -> (u8, u8) { 58 | let system_id = MANAGER 59 | .as_ref() 60 | .clap_matches 61 | .value_of("system_id") 62 | .unwrap() 63 | .parse::() 64 | .expect("System ID should be a value between 1-255."); 65 | 66 | let component_id = MANAGER 67 | .as_ref() 68 | .clap_matches 69 | .value_of("component_id") 70 | .unwrap() 71 | .parse::() 72 | .expect("Component ID should be a value between 1-255."); 73 | 74 | (system_id, component_id) 75 | } 76 | 77 | pub fn mavlink_send_initial_heartbeats() -> bool { 78 | return MANAGER 79 | .as_ref() 80 | .clap_matches 81 | .is_present("send-initial-heartbeats"); 82 | } 83 | 84 | //TODO: Move to the top 85 | fn get_clap_matches<'a>() -> clap::ArgMatches<'a> { 86 | let version = format!( 87 | "{} ({})", 88 | env!("VERGEN_GIT_SEMVER"), 89 | env!("VERGEN_BUILD_TIMESTAMP") 90 | ); 91 | 92 | let matches = clap::App::new(env!("CARGO_PKG_NAME")) 93 | .version(version.as_str()) 94 | .about("MAVLink to REST API!") 95 | .author(env!("CARGO_PKG_AUTHORS")) 96 | .arg( 97 | clap::Arg::with_name("connect") 98 | .short("c") 99 | .long("connect") 100 | .value_name("TYPE::") 101 | .help("Sets the mavlink connection string") 102 | .takes_value(true) 103 | .default_value("udpin:0.0.0.0:14550"), 104 | ) 105 | .arg( 106 | clap::Arg::with_name("server") 107 | .short("s") 108 | .long("server") 109 | .value_name("IP:PORT") 110 | .help("Sets the IP and port that the rest server will be provided") 111 | .takes_value(true) 112 | .default_value("0.0.0.0:8088"), 113 | ) 114 | .arg( 115 | clap::Arg::with_name("mavlink") 116 | .long("mavlink") 117 | .value_name("VERSION") 118 | .help("Sets the mavlink version used to communicate") 119 | .takes_value(true) 120 | .possible_values(&["1", "2"]) 121 | .default_value("2"), 122 | ) 123 | .arg( 124 | clap::Arg::with_name("system_id") 125 | .long("system-id") 126 | .value_name("SYSTEM_ID") 127 | .help("Sets system ID for this service.") 128 | .takes_value(true) 129 | .default_value("255"), 130 | ) 131 | .arg( 132 | clap::Arg::with_name("component_id") 133 | .long("component-id") 134 | .value_name("COMPONENT_ID") 135 | .help("Sets the component ID for this service, for more information, check: https://mavlink.io/en/messages/common.html#MAV_COMPONENT") 136 | .takes_value(true) 137 | .default_value("0"), 138 | ) 139 | .arg( 140 | clap::Arg::with_name("default-api-version") 141 | .long("default-api-version") 142 | .value_name("DEFAULT_API_VERSION") 143 | .help("Sets the default version used by the REST API, this will remove the prefix used by its path.") 144 | .takes_value(true) 145 | .possible_values(&["1"]) 146 | .default_value("1"), 147 | ) 148 | .arg( 149 | clap::Arg::with_name("verbose") 150 | .short("v") 151 | .long("verbose") 152 | .help("Be verbose") 153 | .takes_value(false), 154 | ) 155 | .arg( 156 | clap::Arg::with_name("send-initial-heartbeats") 157 | .long("send-initial-heartbeats") 158 | .help("Send a burst of initial heartbeats to the autopilot spaced by 0.1 seconds to wake up MAVLink connection (useful for PX4-like autopilots).") 159 | .takes_value(false), 160 | ); 161 | 162 | return matches.get_matches(); 163 | } 164 | 165 | #[cfg(test)] 166 | mod tests { 167 | use super::*; 168 | 169 | #[test] 170 | fn default_arguments() { 171 | assert!(!is_verbose()); 172 | assert_eq!(mavlink_connection_string(), "udpin:0.0.0.0:14550"); 173 | assert_eq!(server_address(), "0.0.0.0:8088"); 174 | assert_eq!(mavlink_version(), 2); 175 | assert_eq!(default_api_version(), 1); 176 | } 177 | } 178 | -------------------------------------------------------------------------------- /src/data.rs: -------------------------------------------------------------------------------- 1 | use std::collections::HashMap; 2 | use std::sync::{Arc, Mutex}; 3 | 4 | use lazy_static::lazy_static; 5 | use mavlink::{self, Message}; 6 | use serde::{Deserialize, Serialize}; 7 | 8 | //TODO: break all this types to a new file 9 | #[derive(Clone, Debug, Deserialize, Serialize)] 10 | struct Temporal { 11 | first_update: chrono::DateTime, 12 | last_update: chrono::DateTime, 13 | counter: i64, 14 | frequency: f32, 15 | } 16 | 17 | impl Default for Temporal { 18 | fn default() -> Self { 19 | Self { 20 | first_update: chrono::Local::now(), 21 | last_update: chrono::Local::now(), 22 | counter: 1, 23 | frequency: 0.0, 24 | } 25 | } 26 | } 27 | 28 | impl Temporal { 29 | fn update(&mut self) { 30 | self.last_update = chrono::Local::now(); 31 | self.counter = self.counter.wrapping_add(1); 32 | self.frequency = 33 | (self.counter as f32) / ((self.last_update - self.first_update).num_seconds() as f32); 34 | } 35 | } 36 | 37 | #[derive(Default, Clone, Debug, Deserialize, Serialize)] 38 | struct Status { 39 | time: Temporal, 40 | } 41 | 42 | impl Status { 43 | fn update(&mut self) -> &mut Self { 44 | self.time.update(); 45 | self 46 | } 47 | } 48 | 49 | #[derive(Clone, Debug, Deserialize, Serialize)] 50 | pub struct MAVLinkMessage { 51 | pub header: mavlink::MavHeader, 52 | pub message: T, 53 | } 54 | 55 | #[derive(Clone, Debug, Deserialize, Serialize)] 56 | struct MAVLinkMessageStatus { 57 | message: mavlink::ardupilotmega::MavMessage, 58 | status: Status, 59 | } 60 | 61 | impl MAVLinkMessageStatus { 62 | fn update(&mut self, message: &MAVLinkMessage) { 63 | self.message = message.message.clone(); 64 | self.status.update(); 65 | } 66 | } 67 | 68 | #[derive(Clone, Debug, Deserialize, Serialize)] 69 | struct MAVLinkVehicleComponentData { 70 | id: u8, 71 | messages: HashMap, 72 | } 73 | 74 | impl MAVLinkVehicleComponentData { 75 | fn update(&mut self, message: &MAVLinkMessage) { 76 | let message_name = message.message.message_name().to_string(); 77 | self.messages 78 | .entry(message_name) 79 | .or_insert(MAVLinkMessageStatus { 80 | message: message.message.clone(), 81 | status: Status::default(), 82 | }) 83 | .update(message); 84 | } 85 | } 86 | 87 | #[derive(Clone, Debug, Deserialize, Serialize)] 88 | struct MAVLinkVehicleData { 89 | id: u8, 90 | components: HashMap, 91 | } 92 | 93 | impl MAVLinkVehicleData { 94 | fn update(&mut self, message: &MAVLinkMessage) { 95 | let component_id = message.header.component_id; 96 | self.components 97 | .entry(component_id) 98 | .or_insert(MAVLinkVehicleComponentData { 99 | id: component_id, 100 | messages: HashMap::new(), 101 | }) 102 | .update(message); 103 | } 104 | } 105 | 106 | #[derive(Clone, Debug, Default, Deserialize, Serialize)] 107 | pub struct MAVLinkVehiclesData { 108 | vehicles: HashMap, 109 | } 110 | 111 | impl MAVLinkVehiclesData { 112 | //TODO: Move message to reference 113 | fn update(&mut self, message: MAVLinkMessage) { 114 | let vehicle_id = message.header.system_id; 115 | self.vehicles 116 | .entry(vehicle_id) 117 | .or_insert(MAVLinkVehicleData { 118 | id: vehicle_id, 119 | components: HashMap::new(), 120 | }) 121 | .update(&message); 122 | } 123 | 124 | pub fn pointer(&self, path: &str) -> String { 125 | if path.is_empty() { 126 | return serde_json::to_string_pretty(self).unwrap(); 127 | } 128 | 129 | let path = format!("/{path}"); 130 | 131 | dbg!(&path); 132 | 133 | if path == "/vehicles" { 134 | return serde_json::to_string_pretty(&self.vehicles).unwrap(); 135 | }; 136 | 137 | let value = serde_json::to_value(self).unwrap(); 138 | return match value.pointer(&path) { 139 | Some(content) => serde_json::to_string_pretty(content).unwrap(), 140 | None => "None".into(), 141 | }; 142 | } 143 | } 144 | 145 | #[derive(Debug)] 146 | struct Data { 147 | messages: Arc>, 148 | } 149 | 150 | lazy_static! { 151 | static ref DATA: Data = Data { 152 | messages: Arc::new(Mutex::new(MAVLinkVehiclesData::default())), 153 | }; 154 | } 155 | 156 | pub fn update((header, message): (mavlink::MavHeader, mavlink::ardupilotmega::MavMessage)) { 157 | DATA.messages 158 | .lock() 159 | .unwrap() 160 | .update(MAVLinkMessage { header, message }); 161 | } 162 | 163 | pub fn messages() -> MAVLinkVehiclesData { 164 | let messages = DATA.messages.lock().unwrap(); 165 | messages.clone() 166 | } 167 | -------------------------------------------------------------------------------- /src/endpoints.rs: -------------------------------------------------------------------------------- 1 | use std::path::Path; 2 | 3 | use actix_web::{ 4 | web::{self, Json}, 5 | HttpRequest, HttpResponse, 6 | }; 7 | use actix_web_actors::ws; 8 | use include_dir::{include_dir, Dir}; 9 | use paperclip::actix::{api_v2_operation, Apiv2Schema}; 10 | use serde::{Deserialize, Serialize}; 11 | 12 | use super::data; 13 | use super::mavlink_vehicle::MAVLinkVehicleArcMutex; 14 | use super::websocket_manager::WebsocketActor; 15 | 16 | use log::*; 17 | use mavlink::Message; 18 | 19 | static HTML_DIST: Dir<'_> = include_dir!("src/html"); 20 | 21 | #[derive(Apiv2Schema, Serialize, Debug, Default)] 22 | pub struct InfoContent { 23 | /// Name of the program 24 | name: String, 25 | /// Version/tag 26 | version: String, 27 | /// Git SHA 28 | sha: String, 29 | build_date: String, 30 | /// Authors name 31 | authors: String, 32 | } 33 | 34 | #[derive(Apiv2Schema, Serialize, Debug, Default)] 35 | pub struct Info { 36 | /// Version of the REST API 37 | version: u32, 38 | /// Service information 39 | service: InfoContent, 40 | } 41 | 42 | #[derive(Apiv2Schema, Deserialize)] 43 | pub struct WebsocketQuery { 44 | /// Regex filter to selected the desired MAVLink messages by name 45 | filter: Option, 46 | } 47 | 48 | #[derive(Apiv2Schema, Deserialize)] 49 | pub struct MAVLinkHelperQuery { 50 | /// MAVLink message name, possible options are here: https://docs.rs/mavlink/0.10.0/mavlink/#modules 51 | name: String, 52 | } 53 | 54 | fn load_html_file(filename: &str) -> Option { 55 | if let Some(file) = HTML_DIST.get_file(filename) { 56 | return Some(file.contents_utf8().unwrap().to_string()); 57 | } 58 | 59 | None 60 | } 61 | 62 | pub fn root(req: HttpRequest) -> HttpResponse { 63 | let mut filename = req.match_info().query("filename"); 64 | if filename.is_empty() { 65 | filename = "index.html"; 66 | } 67 | 68 | if let Some(content) = load_html_file(filename) { 69 | let extension = Path::new(&filename) 70 | .extension() 71 | .and_then(std::ffi::OsStr::to_str) 72 | .unwrap_or(""); 73 | let mime = actix_files::file_extension_to_mime(extension).to_string(); 74 | 75 | return HttpResponse::Ok().content_type(mime).body(content); 76 | }; 77 | 78 | return HttpResponse::NotFound() 79 | .content_type("text/plain") 80 | .body("File does not exist"); 81 | } 82 | 83 | #[api_v2_operation] 84 | /// Provides information about the API and this program 85 | pub async fn info() -> Json { 86 | let info = Info { 87 | version: 0, 88 | service: InfoContent { 89 | name: env!("CARGO_PKG_NAME").into(), 90 | version: env!("VERGEN_GIT_SEMVER").into(), 91 | sha: env!("VERGEN_GIT_SHA").into(), 92 | build_date: env!("VERGEN_BUILD_TIMESTAMP").into(), 93 | authors: env!("CARGO_PKG_AUTHORS").into(), 94 | }, 95 | }; 96 | 97 | Json(info) 98 | } 99 | 100 | #[api_v2_operation] 101 | /// Provides an object containing all MAVLink messages received by the service 102 | pub async fn mavlink(req: HttpRequest) -> actix_web::Result { 103 | let path = req.match_info().query("path"); 104 | let message = data::messages().pointer(path); 105 | ok_response(message).await 106 | } 107 | 108 | pub fn parse_query(message: &T) -> String { 109 | let error_message = 110 | "Not possible to parse mavlink message, please report this issue!".to_string(); 111 | serde_json::to_string_pretty(&message).unwrap_or(error_message) 112 | } 113 | 114 | #[api_v2_operation] 115 | /// Returns a MAVLink message matching the given message name 116 | pub async fn helper_mavlink( 117 | _req: HttpRequest, 118 | query: web::Query, 119 | ) -> actix_web::Result { 120 | let message_name = query.into_inner().name; 121 | 122 | let result = match mavlink::ardupilotmega::MavMessage::message_id_from_name(&message_name) { 123 | Ok(id) => mavlink::Message::default_message_from_id(id), 124 | Err(error) => Err(error), 125 | }; 126 | 127 | match result { 128 | Ok(result) => { 129 | let msg = match result { 130 | mavlink::ardupilotmega::MavMessage::common(msg) => { 131 | parse_query(&data::MAVLinkMessage { 132 | header: mavlink::MavHeader::default(), 133 | message: msg, 134 | }) 135 | } 136 | msg => parse_query(&data::MAVLinkMessage { 137 | header: mavlink::MavHeader::default(), 138 | message: msg, 139 | }), 140 | }; 141 | 142 | ok_response(msg).await 143 | } 144 | Err(content) => not_found_response(parse_query(&content)).await, 145 | } 146 | } 147 | 148 | #[api_v2_operation] 149 | #[allow(clippy::await_holding_lock)] 150 | /// Send a MAVLink message for the desired vehicle 151 | pub async fn mavlink_post( 152 | data: web::Data, 153 | _req: HttpRequest, 154 | bytes: web::Bytes, 155 | ) -> actix_web::Result { 156 | let json_string = match String::from_utf8(bytes.to_vec()) { 157 | Ok(content) => content, 158 | Err(err) => { 159 | return not_found_response(format!("Failed to parse input as UTF-8 string: {err:?}")) 160 | .await; 161 | } 162 | }; 163 | 164 | debug!("MAVLink post received: {json_string}"); 165 | 166 | match json5::from_str::>(&json_string) 167 | { 168 | Ok(content) => match data.lock().unwrap().send(&content.header, &content.message) { 169 | Ok(_result) => { 170 | data::update((content.header, content.message)); 171 | return HttpResponse::Ok().await; 172 | } 173 | Err(err) => { 174 | return not_found_response(format!("Failed to send message: {err:?}")).await 175 | } 176 | }, 177 | Err(err) => { 178 | debug!("Failed to parse ardupilotmega message: {err:?}"); 179 | } 180 | } 181 | 182 | match json5::from_str::>(&json_string) { 183 | Ok(content) => { 184 | let content_ardupilotmega = mavlink::ardupilotmega::MavMessage::common(content.message); 185 | match data 186 | .lock() 187 | .unwrap() 188 | .send(&content.header, &content_ardupilotmega) 189 | { 190 | Ok(_result) => { 191 | data::update((content.header, content_ardupilotmega)); 192 | return HttpResponse::Ok().await; 193 | } 194 | Err(err) => { 195 | return not_found_response(format!("Failed to send message: {err:?}")).await; 196 | } 197 | } 198 | } 199 | Err(err) => { 200 | let error_message = 201 | format!("Failed to parse message, not a valid MAVLinkMessage: {err:?}"); 202 | debug!("{error_message}"); 203 | return not_found_response(error_message).await; 204 | } 205 | } 206 | } 207 | 208 | #[api_v2_operation] 209 | /// Websocket used to receive and send MAVLink messages asynchronously 210 | pub async fn websocket( 211 | req: HttpRequest, 212 | query: web::Query, 213 | stream: web::Payload, 214 | ) -> Result { 215 | let filter = match query.into_inner().filter { 216 | Some(filter) => filter, 217 | _ => ".*".to_owned(), 218 | }; 219 | 220 | debug!("New websocket with filter {:#?}", &filter); 221 | 222 | ws::start(WebsocketActor::new(filter), &req, stream) 223 | } 224 | 225 | async fn not_found_response(message: String) -> actix_web::Result { 226 | HttpResponse::NotFound() 227 | .content_type("application/json") 228 | .body(message) 229 | .await 230 | } 231 | 232 | async fn ok_response(message: String) -> actix_web::Result { 233 | HttpResponse::Ok() 234 | .content_type("application/json") 235 | .body(message) 236 | .await 237 | } 238 | -------------------------------------------------------------------------------- /src/html/index.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | MAVLink2Rest 4 | 5 | 6 | 7 |
8 |
9 |
10 |
11 |
12 | {{info.service.name}} - {{info.service.version}} - 13 | {{info.service.sha.substring(0, 7)}} - {{info.service.build_date.substring(0, 10)}}
14 | By: {{formatAuthors(info.service.authors)}}
15 | Check the mavlink path for the data
16 | You can also check nested paths: 17 | mavlink/vehicles/{system_id}/components/{component_id}/messages/HEARTBEAT/message/mavtype/type

20 | 21 | Watcher 22 | is a helper tool where you can check message information in real time.
23 | You can use the query parameter 'path' to visualize specific information, like: 24 | 25 | watcher.html?path=mavlink/vehicles/1/components/1/messages/HEARTBEAT/message/mavtype/type 26 |

27 |
28 |
29 | 30 | 31 | 80 |
81 |
82 |
83 | 84 | 137 | 138 | 139 | -------------------------------------------------------------------------------- /src/html/watcher.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | JSON WebSocket Example 5 | 6 | 7 | 8 | 9 | 10 |
11 |
Last update: {{time.toLocaleString()}}
12 |
Message: {{messageName}}
13 |
14 |
15 | 123 | 124 | 125 | -------------------------------------------------------------------------------- /src/main.rs: -------------------------------------------------------------------------------- 1 | mod cli; 2 | mod data; 3 | mod endpoints; 4 | mod mavlink_vehicle; 5 | mod server; 6 | mod websocket_manager; 7 | 8 | use std::sync::{Arc, Mutex}; 9 | 10 | use data::MAVLinkMessage; 11 | use log::*; 12 | 13 | fn main() -> std::io::Result<()> { 14 | let log_filter = if cli::is_verbose() { "debug" } else { "warn" }; 15 | env_logger::Builder::from_env(env_logger::Env::default().default_filter_or(log_filter)).init(); 16 | cli::init(); 17 | 18 | let mavlink_version = match cli::mavlink_version() { 19 | 1 => mavlink::MavlinkVersion::V1, 20 | 2 => mavlink::MavlinkVersion::V2, 21 | _ => panic!("Invalid mavlink version."), 22 | }; 23 | 24 | let (system_id, component_id) = cli::mavlink_system_and_component_id(); 25 | let vehicle = mavlink_vehicle::MAVLinkVehicleHandle::::new( 26 | cli::mavlink_connection_string(), 27 | mavlink_version, 28 | system_id, 29 | component_id, 30 | cli::mavlink_send_initial_heartbeats(), 31 | ); 32 | 33 | let inner_vehicle = vehicle.mavlink_vehicle.clone(); 34 | server::run(cli::server_address(), &inner_vehicle); 35 | 36 | //TODO: Do inside endpoint and use web::Data ? 37 | websocket_manager::manager() 38 | .lock() 39 | .unwrap() 40 | .new_message_callback = Some(Arc::new(move |value| { 41 | ws_callback(inner_vehicle.clone(), value) 42 | })); 43 | 44 | loop { 45 | std::thread::sleep(std::time::Duration::from_secs(1)); 46 | 47 | while let Ok((header, message)) = vehicle.thread_rx_channel.recv() { 48 | debug!("Received: {:#?} {:#?}", header, message); 49 | websocket_manager::send(&MAVLinkMessage { 50 | header, 51 | message: message.clone(), 52 | }); 53 | data::update((header, message)); 54 | } 55 | } 56 | } 57 | 58 | fn ws_callback( 59 | inner_vehicle: Arc>>, 60 | value: &str, 61 | ) -> String { 62 | if let Ok(content @ MAVLinkMessage:: { .. }) = 63 | serde_json::from_str(value) 64 | { 65 | let result = inner_vehicle 66 | .lock() 67 | .unwrap() 68 | .send(&content.header, &content.message); 69 | if result.is_ok() { 70 | data::update((content.header, content.message)); 71 | } 72 | 73 | format!("{result:?}") 74 | } else if let Ok(content @ MAVLinkMessage:: { .. }) = 75 | serde_json::from_str(value) 76 | { 77 | let content_ardupilotmega = mavlink::ardupilotmega::MavMessage::common(content.message); 78 | let result = inner_vehicle 79 | .lock() 80 | .unwrap() 81 | .send(&content.header, &content_ardupilotmega); 82 | if result.is_ok() { 83 | data::update((content.header, content_ardupilotmega)); 84 | } 85 | 86 | format!("{result:?}") 87 | } else { 88 | String::from("Could not convert input message.") 89 | } 90 | } 91 | -------------------------------------------------------------------------------- /src/mavlink_vehicle.rs: -------------------------------------------------------------------------------- 1 | use std::sync::{mpsc, Arc, Mutex}; 2 | 3 | use log::*; 4 | 5 | pub type MAVLinkVehicleArcMutex = Arc>>; 6 | 7 | #[derive(Clone)] 8 | pub struct MAVLinkVehicle { 9 | //TODO: Check if Arc + Sync + Send>>, 11 | header: Arc>, 12 | } 13 | 14 | impl MAVLinkVehicle { 15 | pub fn send(&self, header: &mavlink::MavHeader, message: &M) -> std::io::Result { 16 | let result = self.vehicle.send(header, message); 17 | 18 | // Convert from mavlink error to io error 19 | match result { 20 | Err(mavlink::error::MessageWriteError::Io(error)) => Err(error), 21 | Ok(something) => Ok(something), 22 | } 23 | } 24 | } 25 | 26 | #[allow(dead_code)] 27 | pub struct MAVLinkVehicleHandle { 28 | //TODO: Check if we can use vehicle here directly 29 | pub mavlink_vehicle: Arc>>, 30 | heartbeat_thread: std::thread::JoinHandle<()>, 31 | receive_message_thread: std::thread::JoinHandle<()>, 32 | //TODO: Add a channel for errors 33 | pub thread_rx_channel: std::sync::mpsc::Receiver<(mavlink::MavHeader, M)>, 34 | } 35 | 36 | impl MAVLinkVehicle { 37 | fn new( 38 | mavlink_connection_string: &str, 39 | version: mavlink::MavlinkVersion, 40 | system_id: u8, 41 | component_id: u8, 42 | ) -> Self { 43 | let mut vehicle = mavlink::connect(mavlink_connection_string).unwrap(); 44 | vehicle.set_protocol_version(version); 45 | let header = mavlink::MavHeader { 46 | system_id, 47 | component_id, 48 | sequence: 0, 49 | }; 50 | 51 | Self { 52 | vehicle: Arc::new(vehicle), 53 | header: Arc::new(Mutex::new(header)), 54 | } 55 | } 56 | } 57 | 58 | impl< 59 | M: 'static + mavlink::Message + std::fmt::Debug + From + Send, 60 | > MAVLinkVehicleHandle 61 | { 62 | pub fn new( 63 | connection_string: &str, 64 | version: mavlink::MavlinkVersion, 65 | system_id: u8, 66 | component_id: u8, 67 | send_initial_heartbeats: bool, 68 | ) -> Self { 69 | let mavlink_vehicle: Arc>> = Arc::new(Mutex::new( 70 | MAVLinkVehicle::::new(connection_string, version, system_id, component_id), 71 | )); 72 | 73 | // PX4 requires a initial heartbeat to be sent to wake up the connection, otherwise it will 74 | // not send any messages 75 | if send_initial_heartbeats { 76 | // From testing, its better to wait a bit before sending the initial heartbeats since 77 | // when sending right away, some heartbeats are lost 78 | std::thread::sleep(std::time::Duration::from_secs(2)); 79 | // Even though one heartbeat is enough, from testing seems like some times the first 80 | // heartbeat is lost, so send a small burst to make sure the connection one go through 81 | // and the connection is woken up 82 | for _ in 0..5 { 83 | send_heartbeat(mavlink_vehicle.clone()); 84 | std::thread::sleep(std::time::Duration::from_millis(100)); 85 | } 86 | } 87 | 88 | let heartbeat_mavlink_vehicle = mavlink_vehicle.clone(); 89 | let receive_message_mavlink_vehicle = mavlink_vehicle.clone(); 90 | 91 | let (tx_channel, rx_channel) = mpsc::channel::<(mavlink::MavHeader, M)>(); 92 | 93 | Self { 94 | mavlink_vehicle, 95 | heartbeat_thread: std::thread::spawn(move || heartbeat_loop(heartbeat_mavlink_vehicle)), 96 | receive_message_thread: std::thread::spawn(move || { 97 | receive_message_loop(receive_message_mavlink_vehicle, tx_channel); 98 | }), 99 | thread_rx_channel: rx_channel, 100 | } 101 | } 102 | } 103 | 104 | fn receive_message_loop< 105 | M: mavlink::Message + std::fmt::Debug + From, 106 | >( 107 | mavlink_vehicle: Arc>>, 108 | channel: std::sync::mpsc::Sender<(mavlink::MavHeader, M)>, 109 | ) { 110 | let mavlink_vehicle = mavlink_vehicle.as_ref().lock().unwrap(); 111 | 112 | let vehicle = mavlink_vehicle.vehicle.clone(); 113 | drop(mavlink_vehicle); 114 | let vehicle = vehicle.as_ref(); 115 | loop { 116 | match vehicle.recv() { 117 | Ok((header, msg)) => { 118 | if let Err(error) = channel.send((header, msg)) { 119 | error!("Failed to send message though channel: {:#?}", error); 120 | } 121 | } 122 | Err(error) => { 123 | error!("Recv error: {:?}", error); 124 | if let mavlink::error::MessageReadError::Io(error) = error { 125 | if error.kind() == std::io::ErrorKind::UnexpectedEof { 126 | // We're probably running a file, time to exit! 127 | std::process::exit(0); 128 | }; 129 | } 130 | } 131 | } 132 | } 133 | } 134 | 135 | fn send_heartbeat>( 136 | mavlink_vehicle: Arc>>, 137 | ) { 138 | let mavlink_vehicle = mavlink_vehicle.as_ref().lock().unwrap(); 139 | let vehicle = mavlink_vehicle.vehicle.clone(); 140 | let mut header = mavlink_vehicle.header.lock().unwrap(); 141 | if let Err(error) = vehicle.as_ref().send(&header, &heartbeat_message().into()) { 142 | error!("Failed to send heartbeat: {:?}", error); 143 | } 144 | header.sequence = header.sequence.wrapping_add(1); 145 | } 146 | 147 | fn heartbeat_loop>( 148 | mavlink_vehicle: Arc>>, 149 | ) { 150 | loop { 151 | std::thread::sleep(std::time::Duration::from_secs(1)); 152 | send_heartbeat(mavlink_vehicle.clone()); 153 | } 154 | } 155 | 156 | pub fn heartbeat_message() -> mavlink::common::MavMessage { 157 | mavlink::common::MavMessage::HEARTBEAT(mavlink::common::HEARTBEAT_DATA { 158 | custom_mode: 0, 159 | mavtype: mavlink::common::MavType::MAV_TYPE_ONBOARD_CONTROLLER, 160 | autopilot: mavlink::common::MavAutopilot::MAV_AUTOPILOT_INVALID, 161 | base_mode: mavlink::common::MavModeFlag::default(), 162 | system_status: mavlink::common::MavState::MAV_STATE_STANDBY, 163 | mavlink_version: 0x3, 164 | }) 165 | } 166 | -------------------------------------------------------------------------------- /src/message_information.rs: -------------------------------------------------------------------------------- 1 | use serde::Serialize; 2 | 3 | #[derive(Serialize, Debug, Copy, Clone)] 4 | struct Time { 5 | first_message: chrono::DateTime, 6 | last_message: chrono::DateTime, 7 | } 8 | 9 | #[derive(Serialize, Debug, Copy, Clone)] 10 | pub struct MessageInformation { 11 | counter: u32, 12 | frequency: f32, 13 | time: Time, 14 | } 15 | 16 | impl Default for MessageInformation { 17 | fn default() -> MessageInformation { 18 | MessageInformation { 19 | counter: 0, 20 | frequency: 0.0, 21 | time: Time { 22 | first_message: chrono::Local::now(), 23 | last_message: chrono::Local::now(), 24 | }, 25 | } 26 | } 27 | } 28 | 29 | impl MessageInformation { 30 | pub fn update(&mut self) { 31 | self.counter += 1; 32 | self.time.last_message = chrono::Local::now(); 33 | self.frequency = (self.counter as f32) 34 | / ((self.time.last_message - self.time.first_message).num_seconds() as f32); 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /src/server.rs: -------------------------------------------------------------------------------- 1 | use super::endpoints; 2 | use super::mavlink_vehicle::MAVLinkVehicleArcMutex; 3 | 4 | use paperclip::actix::{web, web::Scope, OpenApiExt}; 5 | 6 | use actix_cors::Cors; 7 | use actix_web::{ 8 | error::{ErrorBadRequest, JsonPayloadError}, 9 | rt::System, 10 | App, HttpRequest, HttpServer, 11 | }; 12 | 13 | use crate::cli; 14 | use log::*; 15 | 16 | fn json_error_handler(error: JsonPayloadError, _: &HttpRequest) -> actix_web::Error { 17 | warn!("Problem with json: {}", error.to_string()); 18 | match error { 19 | JsonPayloadError::Overflow => JsonPayloadError::Overflow.into(), 20 | _ => ErrorBadRequest(error.to_string()), 21 | } 22 | } 23 | 24 | fn add_v1_paths(scope: Scope) -> Scope { 25 | scope 26 | .route("/helper/mavlink", web::get().to(endpoints::helper_mavlink)) 27 | .route("/mavlink", web::get().to(endpoints::mavlink)) 28 | .route("/mavlink", web::post().to(endpoints::mavlink_post)) 29 | .route(r"/mavlink/{path:.*}", web::get().to(endpoints::mavlink)) 30 | .service(web::resource("/ws/mavlink").route(web::get().to(endpoints::websocket))) 31 | } 32 | 33 | // Start REST API server with the desired address 34 | pub fn run(server_address: &str, mavlink_vehicle: &MAVLinkVehicleArcMutex) { 35 | let server_address = server_address.to_string(); 36 | let mavlink_vehicle = mavlink_vehicle.clone(); 37 | println!("Server running: http://{server_address}"); 38 | 39 | // Start HTTP server thread 40 | let _ = System::new("http-server"); 41 | HttpServer::new(move || { 42 | let v1 = add_v1_paths(web::scope("/v1")); 43 | let default = match cli::default_api_version() { 44 | 1 => add_v1_paths(web::scope("")), 45 | _ => unreachable!("CLI should only allow supported values."), 46 | }; 47 | App::new() 48 | .wrap(Cors::permissive()) 49 | // Record services and routes for paperclip OpenAPI plugin for Actix. 50 | .wrap_api() 51 | //TODO Add middle man to print all http events 52 | .data(web::JsonConfig::default().error_handler(json_error_handler)) 53 | .data(mavlink_vehicle.clone()) 54 | //TODO: Add cors 55 | .route("/", web::get().to(endpoints::root)) 56 | .with_json_spec_at("/docs.json") 57 | .with_swagger_ui_at("/docs") 58 | .route( 59 | r"/{filename:.*(\.html|\.js|\.css)}", 60 | web::get().to(endpoints::root), 61 | ) 62 | .route("/info", web::get().to(endpoints::info)) 63 | // Be sure to have default as the latest endpoint, otherwise it does not work 64 | .service(v1) 65 | .service(default) 66 | .build() 67 | }) 68 | .bind(server_address) 69 | .unwrap() 70 | .run(); 71 | } 72 | -------------------------------------------------------------------------------- /src/vehicle_handler.rs: -------------------------------------------------------------------------------- 1 | use std::sync::{Arc, Mutex}; 2 | use std::thread; 3 | use std::time::Duration; 4 | 5 | use serde_json::json; 6 | 7 | use crate::message_information::MessageInformation; 8 | 9 | // TODO: Break this in shared and not mutex shared variables 10 | pub struct InnerVehicle { 11 | pub channel: Arc< 12 | Box< 13 | (dyn mavlink::MavConnection 14 | + std::marker::Send 15 | + std::marker::Sync 16 | + 'static), 17 | >, 18 | >, 19 | pub messages: Arc>, 20 | verbose: Arc, 21 | pub new_message_callback: Option>, 22 | } 23 | 24 | pub struct Vehicle { 25 | pub inner: Arc>, 26 | } 27 | 28 | impl Vehicle { 29 | // Move arguments to struct 30 | pub fn new( 31 | connection_string: &str, 32 | mavlink_version: mavlink::MavlinkVersion, 33 | verbose: bool, 34 | ) -> Vehicle { 35 | let mut mavlink_communication = 36 | mavlink::connect(connection_string).expect("Unable to connect!"); 37 | mavlink_communication.set_protocol_version(mavlink_version); 38 | Vehicle { 39 | inner: Arc::new(Mutex::new(InnerVehicle { 40 | channel: Arc::new(mavlink_communication), 41 | messages: Arc::new(Mutex::new(json!({"mavlink":{}}))), 42 | verbose: Arc::new(verbose), 43 | new_message_callback: Default::default(), 44 | })), 45 | } 46 | } 47 | 48 | pub fn run(&mut self) { 49 | let inner = Arc::clone(&self.inner); 50 | let inner = inner.lock().unwrap(); 51 | InnerVehicle::heartbeat_loop(&inner); 52 | InnerVehicle::parser_loop(&inner); 53 | let _ = inner.channel.send_default(&InnerVehicle::request_stream()); 54 | } 55 | } 56 | 57 | impl InnerVehicle { 58 | fn heartbeat_loop(inner: &InnerVehicle) { 59 | let vehicle = inner.channel.clone(); 60 | thread::spawn(move || loop { 61 | let res = vehicle.send_default(&InnerVehicle::heartbeat_message()); 62 | if res.is_err() { 63 | println!("Failed to send heartbeat"); 64 | } 65 | thread::sleep(Duration::from_secs(1)); 66 | }); 67 | } 68 | 69 | fn parser_loop(inner: &InnerVehicle) { 70 | let verbose = Arc::clone(&inner.verbose); 71 | let vehicle = inner.channel.clone(); 72 | let messages_ref = Arc::clone(&inner.messages); 73 | let callback = match &inner.new_message_callback { 74 | Some(callback) => Some(Arc::clone(&callback)), 75 | _ => None, 76 | }; 77 | 78 | let mut messages_information: std::collections::HashMap< 79 | std::string::String, 80 | MessageInformation, 81 | > = std::collections::HashMap::new(); 82 | 83 | thread::spawn(move || loop { 84 | match vehicle.recv() { 85 | Ok((_header, msg)) => { 86 | let value = serde_json::to_value(&msg).unwrap(); 87 | let mut msgs = messages_ref.lock().unwrap(); 88 | // Remove " from string 89 | let msg_type = value["type"].to_string().replace("\"", ""); 90 | msgs["mavlink"][&msg_type] = value; 91 | if *verbose { 92 | println!("Got: {}", msg_type); 93 | } 94 | 95 | // Update message_information 96 | let message_information = messages_information 97 | .entry(msg_type.clone()) 98 | .or_insert_with(MessageInformation::default); 99 | message_information.update(); 100 | 101 | msgs["mavlink"][&msg_type]["message_information"] = 102 | serde_json::to_value(messages_information[&msg_type]).unwrap(); 103 | 104 | if callback.is_some() { 105 | callback.as_ref().unwrap()(&msgs["mavlink"][&msg_type], &msg_type); 106 | } 107 | } 108 | Err(error) => { 109 | println!("recv error: {:?}", error); 110 | if let mavlink::error::MessageReadError::Io(error) = error { 111 | if error.kind() == std::io::ErrorKind::UnexpectedEof { 112 | // We're probably running a file, time to exit! 113 | std::process::exit(0); 114 | }; 115 | } 116 | } 117 | } 118 | }); 119 | } 120 | 121 | pub fn heartbeat_message() -> mavlink::ardupilotmega::MavMessage { 122 | mavlink::ardupilotmega::MavMessage::common({ 123 | mavlink::common::MavMessage::HEARTBEAT(mavlink::common::HEARTBEAT_DATA { 124 | custom_mode: 0, 125 | mavtype: mavlink::common::MavType::MAV_TYPE_QUADROTOR, // TODO: Move this to something else 126 | autopilot: mavlink::common::MavAutopilot::MAV_AUTOPILOT_ARDUPILOTMEGA, 127 | base_mode: mavlink::common::MavModeFlag::empty(), 128 | system_status: mavlink::common::MavState::MAV_STATE_STANDBY, 129 | mavlink_version: 0x3, 130 | }) 131 | }) 132 | } 133 | 134 | pub fn request_stream() -> mavlink::ardupilotmega::MavMessage { 135 | mavlink::ardupilotmega::MavMessage::common({ 136 | mavlink::common::MavMessage::REQUEST_DATA_STREAM( 137 | mavlink::common::REQUEST_DATA_STREAM_DATA { 138 | target_system: 0, 139 | target_component: 0, 140 | req_stream_id: 0, 141 | req_message_rate: 10, 142 | start_stop: 1, 143 | }, 144 | ) 145 | }) 146 | } 147 | } 148 | -------------------------------------------------------------------------------- /src/websocket_manager.rs: -------------------------------------------------------------------------------- 1 | use actix::{Actor, Addr, AsyncContext, Handler, Message, StreamHandler}; //TODO: Check include orders 2 | use actix_web_actors::ws; 3 | use derivative::Derivative; 4 | use lazy_static::lazy_static; 5 | use mavlink::Message as MavMessage; 6 | use regex::Regex; 7 | use serde::Serialize; 8 | use std::sync::{Arc, Mutex}; 9 | 10 | use crate::MAVLinkMessage; 11 | 12 | pub struct StringMessage(String); 13 | 14 | impl Message for StringMessage { 15 | type Result = (); 16 | } 17 | 18 | #[derive(Serialize, Debug)] 19 | pub struct WebsocketError { 20 | pub error: String, 21 | } 22 | 23 | #[derive(Debug)] 24 | pub struct WebsocketActorContent { 25 | pub actor: Addr, 26 | pub re: Option, 27 | } 28 | 29 | #[derive(Derivative, Default)] 30 | #[derivative(Debug)] 31 | #[allow(clippy::type_complexity)] 32 | pub struct WebsocketManager { 33 | pub clients: Vec, 34 | #[derivative(Debug = "ignore")] 35 | pub new_message_callback: Option String + Send + Sync>>, 36 | } 37 | 38 | impl WebsocketManager { 39 | pub fn send(&self, value: &serde_json::Value, name: &str) { 40 | if self.clients.is_empty() { 41 | return; 42 | } 43 | 44 | let string = serde_json::to_string_pretty(value).unwrap(); 45 | for client in &self.clients { 46 | let is_match = client.re.as_ref().map_or(false, |regx| regx.is_match(name)); 47 | if is_match { 48 | client.actor.do_send(StringMessage(string.clone())); 49 | } 50 | } 51 | } 52 | } 53 | 54 | lazy_static! { 55 | static ref MANAGER: Arc> = 56 | Arc::new(Mutex::new(WebsocketManager::default())); 57 | } 58 | 59 | pub fn manager() -> Arc> { 60 | MANAGER.clone() 61 | } 62 | 63 | pub fn send(message: &MAVLinkMessage) { 64 | let name = message.message.message_name(); 65 | let value = serde_json::to_value(message).unwrap(); 66 | MANAGER.lock().unwrap().send(&value, name); 67 | } 68 | 69 | #[derive(Debug)] 70 | pub struct WebsocketActor { 71 | server: Arc>, 72 | pub filter: String, 73 | } 74 | 75 | impl WebsocketActor { 76 | pub fn new(message_filter: String) -> Self { 77 | Self { 78 | server: MANAGER.clone(), 79 | filter: message_filter, 80 | } 81 | } 82 | } 83 | 84 | impl Handler for WebsocketActor { 85 | type Result = (); 86 | 87 | fn handle(&mut self, message: StringMessage, context: &mut Self::Context) { 88 | context.text(message.0); 89 | } 90 | } 91 | 92 | impl Actor for WebsocketActor { 93 | type Context = ws::WebsocketContext; 94 | } 95 | 96 | impl StreamHandler> for WebsocketActor { 97 | fn started(&mut self, ctx: &mut Self::Context) { 98 | println!("Starting websocket, add itself in manager."); 99 | self.server 100 | .lock() 101 | .unwrap() 102 | .clients 103 | .push(WebsocketActorContent { 104 | actor: ctx.address(), 105 | re: Regex::new(&self.filter).ok(), 106 | }); 107 | } 108 | 109 | fn finished(&mut self, ctx: &mut Self::Context) { 110 | println!("Finishing websocket, remove itself from manager."); 111 | self.server 112 | .lock() 113 | .unwrap() 114 | .clients 115 | .retain(|x| x.actor != ctx.address()); 116 | } 117 | 118 | fn handle(&mut self, msg: Result, ctx: &mut Self::Context) { 119 | match msg { 120 | Ok(ws::Message::Ping(msg)) => ctx.pong(&msg), 121 | Ok(ws::Message::Text(text)) => { 122 | let text = match &self.server.lock().unwrap().new_message_callback { 123 | Some(callback) => callback(&text), 124 | None => serde_json::to_string(&WebsocketError { 125 | error: "MAVLink callback does not exist.".to_string(), 126 | }) 127 | .unwrap(), 128 | }; 129 | ctx.text(text); 130 | } 131 | Ok(ws::Message::Close(msg)) => ctx.close(msg), 132 | _ => (), 133 | } 134 | } 135 | } 136 | -------------------------------------------------------------------------------- /tests/run.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import aiohttp 4 | import asyncio 5 | import json 6 | import requests 7 | import time 8 | 9 | API = "http://0.0.0.0:8088/v1" 10 | 11 | def vehicle_mode(): 12 | response = requests.get(f"{API}/mavlink/vehicles/1/components/1/messages/HEARTBEAT").json() 13 | return response["message"]["base_mode"]["bits"] 14 | 15 | def set_arm(arm: int): 16 | arm_message = { 17 | "header": { 18 | "system_id": 1, 19 | "component_id": 1, 20 | "sequence": 0 21 | }, 22 | "message": { 23 | "type":"COMMAND_LONG", 24 | "param1":arm, 25 | "param2":0.0,"param3":0.0,"param4":0.0,"param5":0.0,"param6":0.0,"param7":0.0, 26 | "command":{ 27 | "type":"MAV_CMD_COMPONENT_ARM_DISARM" 28 | }, 29 | "target_system":0, 30 | "target_component":0, 31 | "confirmation":0 32 | } 33 | } 34 | 35 | response = requests.post(f"{API}/mavlink", json=arm_message) 36 | return response.status_code == requests.codes.ok 37 | 38 | async def start_client(url: str, amount: int) -> None: 39 | ws = await aiohttp.ClientSession().ws_connect(url, autoclose=False, autoping=False) 40 | 41 | async def dispatch(): 42 | msgs = [] 43 | while len(msgs) < amount: 44 | msg = await ws.receive() 45 | 46 | if msg.type == aiohttp.WSMsgType.TEXT: 47 | msgs += [json.loads(msg.data.strip())] 48 | elif msg.type == aiohttp.WSMsgType.BINARY: 49 | pass 50 | elif msg.type == aiohttp.WSMsgType.PING: 51 | await ws.pong() 52 | elif msg.type == aiohttp.WSMsgType.PONG: 53 | pass 54 | else: 55 | if msg.type == aiohttp.WSMsgType.CLOSE: 56 | await ws.close() 57 | elif msg.type == aiohttp.WSMsgType.ERROR: 58 | print("Error during receive %s" % ws.exception()) 59 | elif msg.type == aiohttp.WSMsgType.CLOSED: 60 | pass 61 | 62 | break 63 | return msgs 64 | 65 | return await dispatch() 66 | 67 | 68 | print("Test info..") 69 | response = requests.get(f"{API}/info").json() 70 | 71 | assert(response["version"] >= 0), "Info version is invalid." 72 | assert(response["service"]["name"] == "mavlink2rest"), "Invalid service name." 73 | assert(len(response["service"]["sha"]) != 0), "Invalid sha length." 74 | 75 | print("Test heartbeat..") 76 | response = requests.get(f"{API}/mavlink/vehicles/1/components/1/messages/HEARTBEAT").json() 77 | 78 | assert(response["message"]["type"] == "HEARTBEAT"), "Message type is incorrect." 79 | assert(response["message"]["autopilot"]["type"]), "Autopilot type does not exist." 80 | assert(response["status"]["time"]["frequency"] > 0.95), "Heartbeat frequency is wrong." 81 | 82 | print("Test ARM and DISARM..") 83 | assert(set_arm(0)), "Fail to send DISARM command" 84 | time.sleep(1) 85 | assert((vehicle_mode() & 128) == 0), "Vehicle appears to be ARMED." 86 | assert(set_arm(1)), "Fail to send ARM command" 87 | time.sleep(1) 88 | assert((vehicle_mode() & 128) != 0), "Failed to ARM vehicle." 89 | 90 | print("Test pretty..") 91 | response = requests.get(f"{API}/mavlink/vehicles/1/components/1/messages/HEARTBEAT") 92 | assert(response.text.count('\n') == 26), "Pretty heartbeat does not look correct." 93 | 94 | async def test_websocket_fetch_filter(): 95 | print("Test websocket..") 96 | msgs = await start_client(f"{API}/ws/mavlink", 30) 97 | assert(len(list(filter(lambda msg: msg["message"]["type"] == "HEARTBEAT", msgs))) != 30), "Failed to fetch more than one type of msg." 98 | msgs = await start_client(f"{API}/ws/mavlink?filter=HEARTBEAT", 30) 99 | assert(len(list(filter(lambda msg: msg["message"]["type"] == "HEARTBEAT", msgs))) == 30), "Filter failed." 100 | 101 | asyncio.run(test_websocket_fetch_filter()) 102 | --------------------------------------------------------------------------------