├── .clang-format ├── .gitattributes ├── .github └── FUNDING.yml ├── .gitignore ├── .gitmodules ├── .travis.yml ├── CMakeLists.txt ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── build └── raspbian │ ├── Dockerfile │ └── toolchain-arm-linux.cmake ├── hardware ├── enclosure1 │ ├── Back.stl │ ├── Base.stl │ ├── Front.stl │ └── Top.stl ├── rpi-radioroom.jpg └── rpi-radioroom.md ├── libs ├── inih │ ├── include │ │ ├── INIReader.h │ │ └── ini.h │ └── src │ │ ├── INIReader.cc │ │ └── ini.c ├── mavio │ ├── include │ │ ├── CircularBuffer.h │ │ ├── IridiumSBD.h │ │ ├── Logger.h │ │ ├── MAVLinkAutopilot.h │ │ ├── MAVLinkChannel.h │ │ ├── MAVLinkISBD.h │ │ ├── MAVLinkISBDChannel.h │ │ ├── MAVLinkLib.h │ │ ├── MAVLinkLogger.h │ │ ├── MAVLinkSerial.h │ │ ├── MAVLinkTCP.h │ │ ├── MAVLinkTCPChannel.h │ │ └── Serial.h │ └── src │ │ ├── IridiumSBD.cc │ │ ├── Logger.cc │ │ ├── MAVLinkAutopilot.cc │ │ ├── MAVLinkISBD.cc │ │ ├── MAVLinkISBDChannel.cc │ │ ├── MAVLinkLogger.cc │ │ ├── MAVLinkSerial.cc │ │ ├── MAVLinkTCP.cc │ │ ├── MAVLinkTCPChannel.cc │ │ └── Serial.cc └── timelib │ └── include │ └── timelib.h ├── pack ├── common │ └── etc │ │ ├── logrotate.d │ │ └── radioroom_lr.conf │ │ ├── rsyslog.d │ │ └── radioroom_log.conf │ │ └── systemd │ │ └── system │ │ └── radioroom.service ├── jetson │ └── etc │ │ ├── radioroom.conf │ │ └── systemd │ │ └── system │ │ └── rtmpstream.service └── raspbian │ └── etc │ ├── radioroom.conf │ └── sytetmd │ └── system │ └── rtmpstream.service ├── src ├── CameraHandler.cc ├── CameraHandler.h ├── Config.cc ├── Config.h ├── MAVLinkHandler.cc ├── MAVLinkHandler.h ├── MAVReport.cc ├── MAVReport.h ├── build.h.in └── radioroom.cc ├── tests ├── CircularBufferTest.cc ├── CustomSerializationTest.cc ├── MAVLinkAutopilotTest.cc ├── MAVLinkISBDChannelTest.cc ├── MAVLinkTCPChannelTest.cc └── TimeLibTest.cc ├── toolchain-arm-linux.cmake ├── toolchain-arm-windows.cmake ├── toolchain-jetson.cmake └── toolchain-x64-windows.cmake /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: Google 4 | AccessModifierOffset: -1 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveAssignments: false 7 | AlignConsecutiveDeclarations: false 8 | AlignEscapedNewlinesLeft: true 9 | AlignOperands: true 10 | AlignTrailingComments: true 11 | AllowAllParametersOfDeclarationOnNextLine: true 12 | AllowShortBlocksOnASingleLine: false 13 | AllowShortCaseLabelsOnASingleLine: false 14 | AllowShortFunctionsOnASingleLine: All 15 | AllowShortIfStatementsOnASingleLine: true 16 | AllowShortLoopsOnASingleLine: true 17 | AlwaysBreakAfterDefinitionReturnType: None 18 | AlwaysBreakAfterReturnType: None 19 | AlwaysBreakBeforeMultilineStrings: true 20 | AlwaysBreakTemplateDeclarations: true 21 | BinPackArguments: true 22 | BinPackParameters: true 23 | BraceWrapping: 24 | AfterClass: false 25 | AfterControlStatement: false 26 | AfterEnum: false 27 | AfterFunction: false 28 | AfterNamespace: false 29 | AfterObjCDeclaration: false 30 | AfterStruct: false 31 | AfterUnion: false 32 | BeforeCatch: false 33 | BeforeElse: false 34 | IndentBraces: false 35 | BreakBeforeBinaryOperators: None 36 | BreakBeforeBraces: Attach 37 | BreakBeforeTernaryOperators: true 38 | BreakConstructorInitializersBeforeComma: false 39 | BreakAfterJavaFieldAnnotations: false 40 | BreakStringLiterals: true 41 | ColumnLimit: 80 42 | CommentPragmas: '^ IWYU pragma:' 43 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 44 | ConstructorInitializerIndentWidth: 4 45 | ContinuationIndentWidth: 4 46 | Cpp11BracedListStyle: true 47 | DerivePointerAlignment: true 48 | DisableFormat: false 49 | ExperimentalAutoDetectBinPacking: false 50 | ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] 51 | IncludeCategories: 52 | - Regex: '^<.*\.h>' 53 | Priority: 1 54 | - Regex: '^<.*' 55 | Priority: 2 56 | - Regex: '.*' 57 | Priority: 3 58 | IncludeIsMainRegex: '([-_](test|unittest))?$' 59 | IndentCaseLabels: true 60 | IndentWidth: 2 61 | IndentWrappedFunctionNames: false 62 | JavaScriptQuotes: Leave 63 | JavaScriptWrapImports: true 64 | KeepEmptyLinesAtTheStartOfBlocks: false 65 | MacroBlockBegin: '' 66 | MacroBlockEnd: '' 67 | MaxEmptyLinesToKeep: 1 68 | NamespaceIndentation: None 69 | ObjCBlockIndentWidth: 2 70 | ObjCSpaceAfterProperty: false 71 | ObjCSpaceBeforeProtocolList: false 72 | PenaltyBreakBeforeFirstCallParameter: 1 73 | PenaltyBreakComment: 300 74 | PenaltyBreakFirstLessLess: 120 75 | PenaltyBreakString: 1000 76 | PenaltyExcessCharacter: 1000000 77 | PenaltyReturnTypeOnItsOwnLine: 200 78 | PointerAlignment: Left 79 | ReflowComments: true 80 | SortIncludes: true 81 | SpaceAfterCStyleCast: false 82 | SpaceAfterTemplateKeyword: true 83 | SpaceBeforeAssignmentOperators: true 84 | SpaceBeforeParens: ControlStatements 85 | SpaceInEmptyParentheses: false 86 | SpacesBeforeTrailingComments: 2 87 | SpacesInAngles: false 88 | SpacesInContainerLiterals: true 89 | SpacesInCStyleCastParentheses: false 90 | SpacesInParentheses: false 91 | SpacesInSquareBrackets: false 92 | Standard: Auto 93 | TabWidth: 8 94 | UseTab: Never 95 | ... 96 | 97 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | pack/* eol=lf 2 | * text=auto 3 | libs/* linguist-vendored 4 | -------------------------------------------------------------------------------- /.github/FUNDING.yml: -------------------------------------------------------------------------------- 1 | # These are supported funding model platforms 2 | 3 | github: # Replace with up to 4 GitHub Sponsors-enabled usernames e.g., [user1, user2] 4 | patreon: # Replace with a single Patreon username 5 | open_collective: # Replace with a single Open Collective username 6 | ko_fi: # Replace with a single Ko-fi username 7 | tidelift: # Replace with a single Tidelift platform-name/package-name e.g., npm/babel 8 | community_bridge: # Replace with a single Community Bridge project-name e.g., cloud-foundry 9 | liberapay: # Replace with a single Liberapay username 10 | issuehunt: # Replace with a single IssueHunt username 11 | otechie: # Replace with a single Otechie username 12 | custom: ['https://paypal.me/radioroom'] 13 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | *.smod 19 | 20 | # Compiled Static libraries 21 | *.lai 22 | *.la 23 | *.a 24 | *.lib 25 | 26 | # Executables 27 | *.exe 28 | *.exe.stackdump 29 | *.out 30 | *.app 31 | /.metadata/ 32 | /.settings/ 33 | /RadioRoom/bin/* 34 | /bin/* 35 | /debug/* 36 | /Default/ 37 | /RemoteSystemsTempFiles/ 38 | /build/CMakeFiles/ 39 | CMakeCache.txt 40 | CMakeSettings.json 41 | .cproject 42 | /.vscode 43 | /.vs 44 | /out -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "libs/mavlink/include"] 2 | path = libs/mavlink/include 3 | url = https://github.com/mavlink/c_library_v2 4 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: cpp 2 | before_script: 3 | # Install Raspbian toolchain 4 | - mkdir -p /home/travis/build/toolchain/raspbian 5 | - cd /home/travis/build/toolchain/raspbian 6 | - wget https://s3.amazonaws.com/RTI/Community/ports/toolchains/raspbian-toolchain-gcc-4.7.2-linux64.tar.gz 7 | - tar xf raspbian-toolchain-gcc-4.7.2-linux64.tar.gz 8 | - export PATH=/home/travis/build/toolchain/raspbian/raspbian-toolchain-gcc-4.7.2-linux64/bin:$PATH 9 | # Install Jetson toolchain 10 | - mkdir -p /home/travis/build/toolchain/jetson 11 | - cd /home/travis/build/toolchain/jetson 12 | - wget https://envirover.s3-us-west-2.amazonaws.com/build/toolchains/gcc-linaro-7.3.1-2018.05-x86_64_aarch64-linux-gnu.tar.xz 13 | - tar xf gcc-linaro-7.3.1-2018.05-x86_64_aarch64-linux-gnu.tar.xz 14 | - export PATH=/home/travis/build/toolchain/jetson/gcc-linaro-7.3.1-2018.05-x86_64_aarch64-linux-gnu/bin:$PATH 15 | # switch to the src directory 16 | - cd /home/travis/build/envirover/SPLRadioRoom 17 | script: 18 | # Build for Raspbian 19 | - mkdir bin 20 | - cd bin 21 | - cmake -G "Unix Makefiles" -DCMAKE_TOOLCHAIN_FILE:FILEPATH="../toolchain-arm-linux.cmake" -DBUILD_NUM=$TRAVIS_BUILD_NUMBER .. 22 | - make 23 | - cpack -G DEB 24 | - export RASPBIAN_PKG_FILE=$(ls *.deb) 25 | - cp $RASPBIAN_PKG_FILE /home/travis/build/ 26 | - cd .. 27 | - rm -r bin 28 | # Build for Jetson 29 | - mkdir bin 30 | - cd bin 31 | - cmake -G "Unix Makefiles" -DCMAKE_TOOLCHAIN_FILE:FILEPATH="../toolchain-jetson.cmake" -DBUILD_NUM=$TRAVIS_BUILD_NUMBER .. 32 | - make 33 | - cpack -G DEB 34 | - export JETSON_PKG_FILE=$(ls *.deb) 35 | - cp $JETSON_PKG_FILE /home/travis/build/ 36 | - cd .. 37 | before_deploy: 38 | - echo "Deploying $RASPBIAN_PKG_FILE and $JETSON_PKG_FILE to GitHub releases" 39 | deploy: 40 | provider: releases 41 | name: "UV Radio Room build $TRAVIS_BUILD_NUMBER" 42 | prerelease: true 43 | api_key: 44 | secure: jkOfxa7qoH3O50aJ12GFk4KBnwLHBAZvKG4Ka5x214ZEq1EV00ywuz1isN0qF+M2S1NJi0d/E58f4ndT1LQLgxcz3mcDcmo9v1EWFYEzfTL4o/BtV0IsrRy/pLg1t04QKHiy20kl8ViET293vBBla1yd72E04zlE6MjZMugifFdUoamHJkgMuY679JxE6o4KcXLxAwS3/lsrpCcdPEVj5DSuGUpyyGxKcoZW+cTTVc3zjBbjnLG9Hnle3zgIhC8hKHiCqy8uXtlgQN9m1E+P/Xn6LGAtO7Fa4Mlon9q1wbRXb+SRqG0sM8LmegORDHrFIvvuod0gCoIy1mbghnScAe9McUFeyVHQZ9ri7Xsb/G7fmpTAlPsf6EaKLm4m2gjXLec+W+thCPrtWUaoAdiq05d/ThP9mlOVbQ4A0O31oDSJJXctkb1OKgYrisBYY9dty4IKbVYkLA/+/QOi1EUAq9bspp77cUdhVx+MbrP3t982k/Ql8r3otmkIx4sbG404fnqxNauu5oe6zFgvratzOesYaL9p2dkkxCmzbY08pC1Uwn79GpTNMjvhNKVp6xi4AKR6+qDoMzXVZckpDrQnK/5yt4Fp4/5PRjGQhaayOx2vOLzZccOyiJLRZPgq3KAnSZRd4DnhzYFVPCuj91DLDo1jTsmgOj6rxjn1PHwguT8= 45 | file_glob: true 46 | file: 47 | - "/home/travis/build/${RASPBIAN_PKG_FILE}" 48 | - "/home/travis/build/${JETSON_PKG_FILE}" 49 | on: 50 | repo: envirover/SPLRadioRoom 51 | skip_cleanup: true 52 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.0) 2 | 3 | project(radioroom VERSION 2.5.0) 4 | 5 | set(CMAKE_BINARY_DIR ${CMAKE_SOURCE_DIR}/bin) 6 | set(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}) 7 | set(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}) 8 | 9 | set(CPACK_GENERATOR "DEB") 10 | # set(CPACK_DEBIAN_PACKAGE_ARCHITECTURE "armhf") 11 | set(CPACK_PACKAGE_NAME "radioroom") 12 | set(CPACK_DEBIAN_PACKAGE_MAINTAINER "Pavel Bobov") 13 | set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "Telemetry for MAVLink autopilots") 14 | set(CPACK_PACKAGE_VENDOR "Envirover") 15 | set(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/README.md") 16 | set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE") 17 | set(CPACK_PACKAGE_VERSION_MAJOR "${PROJECT_VERSION_MAJOR}") 18 | set(CPACK_PACKAGE_VERSION_MINOR "${PROJECT_VERSION_MINOR}") 19 | set(CPACK_PACKAGE_VERSION_PATCH "${PROJECT_VERSION_PATCH}") 20 | 21 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -std=c++11 -lpthread" ) 22 | 23 | configure_file("${PROJECT_SOURCE_DIR}/src/build.h.in" "${PROJECT_BINARY_DIR}/build.h") 24 | 25 | include_directories("${PROJECT_SOURCE_DIR}/libs/mavlink/include") 26 | include_directories("${PROJECT_SOURCE_DIR}/libs/mavio/include") 27 | include_directories("${PROJECT_SOURCE_DIR}/libs/inih/include") 28 | include_directories("${PROJECT_SOURCE_DIR}/libs/timelib/include") 29 | include_directories("${PROJECT_BINARY_DIR}") 30 | 31 | file(GLOB mavio_sources libs/mavio/src/*.cc) 32 | file(GLOB inih_sources libs/inih/src/*.cc libs/inih/src/*.c) 33 | file(GLOB sources src/*.c src/*.cc) 34 | 35 | add_library(mavio STATIC ${mavio_sources}) 36 | add_library(inih STATIC ${inih_sources}) 37 | 38 | add_executable(radioroom ${sources}) 39 | target_link_libraries(radioroom mavio) 40 | target_link_libraries(radioroom inih) 41 | 42 | add_executable(aptest tests/MAVLinkAutopilotTest.cc) 43 | add_executable(tltest tests/TimeLibTest.cc) 44 | add_executable(cbtest tests/CircularBufferTest.cc) 45 | add_executable(tcptest tests/MAVLinkTCPChannelTest.cc) 46 | add_executable(isbdtest tests/MAVLinkISBDChannelTest.cc) 47 | add_executable(srtest tests/CustomSerializationTest.cc) 48 | 49 | target_link_libraries(aptest mavio) 50 | target_link_libraries(cbtest mavio) 51 | target_link_libraries(tcptest mavio) 52 | target_link_libraries(isbdtest mavio) 53 | target_link_libraries(srtest mavio) 54 | 55 | install(TARGETS radioroom DESTINATION "/usr/sbin") 56 | 57 | # Install files common for all systems 58 | install(DIRECTORY "${PROJECT_SOURCE_DIR}/pack/common/etc/" DESTINATION "/etc" 59 | FILE_PERMISSIONS OWNER_WRITE OWNER_READ GROUP_READ WORLD_READ) 60 | 61 | # Install system-specific files 62 | install(DIRECTORY "${PROJECT_SOURCE_DIR}/pack/${CMAKE_SYSTEM_NAME}/etc/" DESTINATION "/etc" 63 | FILE_PERMISSIONS OWNER_WRITE OWNER_READ GROUP_READ WORLD_READ) 64 | 65 | include(CPack) 66 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | Contributing to Envirover Open Source Projects 2 | ========================================= 3 | 4 | Envirover welcomes contributions to our [open source projects on Github](https://github.com/envirover). 5 | 6 | Issues 7 | ------ 8 | 9 | Feel free to submit issues and enhancement requests. 10 | 11 | Contributing 12 | ------------ 13 | 14 | Please refer to each project's style guidelines and guidelines for submitting patches and additions. In general, we follow the "fork-and-pull" Git workflow. 15 | 16 | 1. **Fork** the repo on GitHub 17 | 2. **Clone** the project to your own machine 18 | 3. **Commit** changes to your own branch 19 | 4. **Push** your work back up to your fork 20 | 5. Submit a **Pull request** so that we can review your changes 21 | 22 | NOTE: Be sure to merge the latest from "upstream" before making a pull request! 23 | 24 | Copyright and Licensing 25 | ----------------------- 26 | 27 | Envirover does not require you to assign the copyright of your contributions, you retain the copyright. Envirover does require that you make your contributions available under the GNU General Public License in order to be included in the main repo. 28 | 29 | If appropriate, include the GNU General Public License summary at the top of each file along with the copyright info. If you are adding a new file that you wrote, include your name in the copyright notice in the license summary at the top of the file. 30 | 31 | ### License Summary 32 | 33 | You can copy and paste the GNU General Public License summary from below. 34 | 35 | ``` 36 | Copyright (C) 2017 Envirover 37 | 38 | SPLGroundControl is free software: you can redistribute it and/or modify 39 | it under the terms of the GNU General Public License as published by 40 | the Free Software Foundation, either version 3 of the License, or 41 | (at your option) any later version. 42 | 43 | SPLGroundControl is distributed in the hope that it will be useful, 44 | but WITHOUT ANY WARRANTY; without even the implied warranty of 45 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 46 | GNU General Public License for more details. 47 | 48 | You should have received a copy of the GNU General Public License 49 | along with SPLGroundControl. If not, see . 50 | ``` 51 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ![SPL System Architecture](https://s3-us-west-2.amazonaws.com/envirover/images/SPL-2.4.jpg) 2 | 3 | [![Build Status](https://api.travis-ci.com/envirover/SPLRadioRoom.svg?branch=master)](https://travis-ci.com/github/envirover/SPLRadioRoom) 4 | [![Join the chat at https://gitter.im/SPLRadioRoom/Lobby](https://badges.gitter.im/SPLRadioRoom/Lobby.svg)](https://gitter.im/SPLRadioRoom/Lobby?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge&utm_content=badge) 5 | 6 | # UV Radio Room 7 | 8 | UV Radio Room is a firmware for a companion computer of MAVLink-based autopilots such as ArduPilot or PX4 that provides telemetry over Iridium Short Burst Data (ISBD) satellite messaging system or/and an Internet connection. Together with [UV Hub](http://envirover.com/docs/uvhub.html) server it provides a two-way communication solution between unmanned vehicles and ground control stations such as QGroundControl or Mission Planer. 9 | 10 | ## System Requirements 11 | 12 | UV Radio Room system requires the following hardware and software: 13 | * Autopilot such as Pixhawk with [ArduPilot](http://ardupilot.org/) or [PX4](http://px4.io/) firmware; 14 | * Raspberry Pi computer with [Raspbian Stretch](https://www.raspberrypi.org/downloads/raspbian/) Desktop or Light; 15 | * Activated [RockBLOCK Mk2](http://www.rock7mobile.com/products-rockblock) or [RockBLOCK 9603](http://www.rock7mobile.com/products-rockblock-9603) Iridium satellite communication module with FTDI USB to UART cable. 16 | * Cellular or satellite Internet modem. 17 | 18 | See [hardware/rpi-radioroom.md](hardware/rpi-radioroom.md) for UV Radio Room hardware parts and STL files for a 3D-printed enclosure. 19 | 20 | ## Wiring 21 | 22 | UV Radio Room uses serial devices to communicate with autopilot and ISBD transceiver. Cellular or satellite Internet modems cold be connected using USB, Ethernet, or WiFi connections. 23 | 24 | ![Wiring](https://s3-us-west-2.amazonaws.com/envirover/images/RadioRoomWiring3.jpg) 25 | 26 | See the instructions on connecting companion computer to Pixhawk running ArduPilot and PX4 autopilots: 27 | 28 | * [ArduPilot Connecting the Pixhawk and RPi](http://ardupilot.org/dev/docs/raspberry-pi-via-mavlink.html) 29 | * [PX4 Companion Computer for Pixhawk class](https://dev.px4.io/en/companion_computer/pixhawk_companion.html) 30 | 31 | The safest bet is to use USB to TTL UART serial converter to connect both autopilot and transceiver to the Raspberry Pi's USB ports. Another, less straightforward option is to connect autopilot to the Raspberry Pi serial port using GPIO pins. 32 | 33 | It is recommended to connect RockBLOCK module using [FTDI USB to UART cable](https://www.rock7.com/shop-product-detail?productId=16) provided by Rock Seven Mobile. 3.3.V FTDI cables or chips from other vendors can also be used, but MaxPower field in the chip must be set to 500 mA to satisfy the power requirements of RockBLOCK (The default setting for MaxPower field is typically 100 mA). Alternatively, RockBLOCK could be powered by directly, and not through the FTDI chip. 34 | 35 | Connect the cellular or satellite Internet modem to Raspberry Pi as recommended by the modem manufacturer. The modems are not shown on the diagram. 36 | 37 | Though +5V TELEM pin on Pixhawk is rated for up to 2A peak power draw, it is recommended to power Raspberry Pi using a dedicated power source, especially if both RockBLOCK and a cellular modems are powered from the Raspberry Pi USB ports. 38 | 39 | ## Installing 40 | 41 | To install UV Radio Room on Raspberry Pi or NVIDIA Jetson: 42 | 43 | 1. Copy radioroom-2.5.0-raspbian.deb or radioroom-2.5.0-jetson.deb from https://github.com/envirover/SPLRadioRoom/releases to the machine. 44 | 2. Install radioroom-2.5.0-raspbian.deb package or radioroom-2.5.0-jetson.deb. 45 | 46 | ``$ sudo dpkg -i radioroom-2.5.0-raspbian.deb`` 47 | 48 | 3. Configure the reporting period and the serial device paths for autopilot in /etc/radioroom.conf. 49 | 4. If ISBD transceiver is used, in [isbd] section set enabled=true and specify the serial device paths of the ISBD transceiver in /etc/radioroom.conf. 50 | 5. If TCP/IP connection is used, in [tcp] section set enabled=true and specify the TCP server's IP address and port in /etc/radioroom.conf. 51 | 6. Start radioroom service. 52 | 53 | ``` 54 | $ sudo systemctl enable radioroom.service 55 | $ sudo systemctl start radioroom.service 56 | ``` 57 | 58 | By default the serial device paths are set to /dev/ttyACM0 for autopilot and to /dev/ttyUSB0 for ISBD transceiver. If auto_detect_serials property is set to true, UV Radio Room can auto-detect autopilot and ISBD if they are available on other serial and USB devices. To make the UV Radio Room startup faster and more reliable it is recommended to set the device paths correctly. 59 | 60 | USB device paths /dev/ttyUSB0, /dev/ttyUSB1, ... can swap after reboot. For USB devices it is recommended to use symlinks from /dev/serial/by-path or /dev/serial/by-path directories, that do not change with reboots. 61 | 62 | UV Radio Room periodically reports the vehicle's position, attitude, velocity, and other data using HIGH_LATENCY MAVLink message. The message size is 48 bytes. Each report consumes 1 RockBLOCK credit. The reporting period default value is 60 seconds. It can be changed by setting report_period configuration property in /etc/radioroom.conf. 63 | 64 | ### Configuring Camera Handlers 65 | 66 | UV Radio Room camera handlers execute custom Linux shell commands on MAVLink commands such as DO_DIGICAM_CONTROL, IMAGE_START_CAPTURE, IMAGE_STOP_CAPTURE, VIDEO_START_CAPTURE, and VIDEO_STOP_CAPTURE. 67 | 68 | The camera handlers are defined in [camera_handler] section in /etc/radioroom.conf file and executed when correspondent commands are received from one of the UV Radio Room's channel or when the autopilot confirms reaching of a mission item with a correspondent command. 69 | 70 | The Linux shell commands may include {{param1}}, ..., {{param8}} variables that during the command execution will be replaced by values of the correspondent MAVLink command parameters. 71 | 72 | ## Troubleshooting 73 | 74 | Run ``$ sudo systemctl status radioroom.service`` to check the status of radioroom service. 75 | 76 | If radioroom is properly wired and configured, the output should look like this: 77 | 78 | ```bash 79 | pi@raspberrypi:~ $ sudo systemctl status radioroom.service 80 | ● radioroom.service - UV Radio Room Service 81 | Loaded: loaded (/etc/systemd/system/radioroom.service; enabled; vendor preset: enabled) 82 | Active: activating (start) since Tue 2018-11-07 07:27:56 UTC; 6 days ago 83 | Docs: http://github.com/envirover/SPLRadioRoom 84 | Main PID: 254 (radioroom) 85 | CGroup: /system.slice/radioroom.service 86 | └─254 /usr/sbin/radioroom 87 | 88 | Nov 07 07:27:56 raspberrypi systemd[1]: Starting UV Radio Room Service... 89 | Nov 07 07:27:57 raspberrypi radioroom[254]: Starting UV Radio Room 2.5.0... 90 | Nov 07 07:27:57 raspberrypi radioroom[254]: Connecting to autopilot (/dev/ttyUSB0 57600)... 91 | Nov 07 07:27:58 raspberrypi radioroom[254]: Autopilot detected at serial device '/dev/ttyUSB0'. 92 | Nov 07 07:27:58 raspberrypi radioroom[254]: MAV type: 12, system id: 1, autopilot class: 3, firmware version: 3.5.0/255 93 | Nov 07 07:27:58 raspberrypi radioroom[254]: Connecting to ISBD transceiver (/dev/ttyUSB1 19200)... 94 | Nov 07 07:27:58 raspberrypi radioroom[254]: IRIDIUM 9600 Family SBD Transceiver (IMEA 123456789012345) detected at serial device '/dev/ttyUSB1'. 95 | Nov 07 07:27:58 raspberrypi radioroom[254]: UV Radio Room 2.5.0 started. 96 | ``` 97 | 98 | Log file of radioroom service is available at /var/log/radioroom.log. 99 | 100 | Add ``-v`` option to the radioroom command line in /etc/systemd/system/radioroom.sevice to enable verbose logging. Verbose logging makes radioroom service log all MAVLink messages sent and received from autopilot and ISBD transceiver. 101 | 102 | ## Building 103 | 104 | To build radioroom on Raspberry Pi. 105 | 106 | ```bash 107 | $ sudo apt-get install git cmake 108 | $ git clone https://github.com/envirover/SPLRadioRoom.git 109 | $ cd SPLRadioRoom 110 | $ mkdir bin 111 | $ cd bin 112 | $ cmake .. 113 | $ make 114 | ``` 115 | 116 | To create a debian package run ``$cpack ..`` command after that. 117 | 118 | To cross-compile on Windows. 119 | 1. Install git and clone SPLRadioRoom repo. 120 | 121 | ``$ git clone https://github.com/envirover/SPLRadioRoom.git`` 122 | 123 | 2. Install [Windows toolchain for Raspberry Pi](http://gnutoolchains.com/raspberry/). 124 | 3. Create 'bin' subdirectory inside SPLRadioRoom and change the current directory to it. 125 | 4. Run cmake using ../toolchain-arm-windows.cmake toolchain file. 126 | 127 | ``$ cmake -G "Unix Makefiles" -DCMAKE_BUILD_TYPE:STRING="" -DCMAKE_TOOLCHAIN_FILE:FILEPATH="../toolchain-arm-windows.cmake" ..`` 128 | 5. Run make. 129 | 130 | ``$ make`` 131 | 132 | For cross-compilation on Linux raspbian toolchain for Linux is required. toolchain-arm-linux.cmake should be specified as CMake toolchain file. 133 | 134 | ## Issues 135 | 136 | Find a bug or want to request a new feature? Please let us know by submitting an [issue](https://github.com/envirover/support/issues). 137 | 138 | ## Contributing 139 | 140 | Envirover welcomes contributions from anyone and everyone. Please see our [guidelines for contributing](https://github.com/envirover/SPLRadioRoom/blob/master/CONTRIBUTING.md). 141 | 142 | ## Licensing 143 | 144 | ``` 145 | Copyright (C) 2018 Envirover 146 | 147 | UV Radio Room is free software: you can redistribute it and/or modify 148 | it under the terms of the GNU General Public License as published by 149 | the Free Software Foundation, either version 3 of the License, or 150 | (at your option) any later version. 151 | 152 | UV Radio Room is distributed in the hope that it will be useful, 153 | but WITHOUT ANY WARRANTY; without even the implied warranty of 154 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 155 | GNU General Public License for more details. 156 | 157 | You should have received a copy of the GNU General Public License 158 | along with UV Radio Room. If not, see . 159 | ``` 160 | -------------------------------------------------------------------------------- /build/raspbian/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ubuntu:18.04 2 | 3 | # Install build essential 4 | RUN apt-get update && apt-get install -y cmake 5 | 6 | # Install Raspbian toolchain 7 | ADD https://s3.amazonaws.com/RTI/Community/ports/toolchains/raspbian-toolchain-gcc-4.7.2-linux64.tar.gz /build/toolchain/ 8 | RUN tar -xzf /build/toolchain/raspbian-toolchain-gcc-4.7.2-linux64.tar.gz -C /build/toolchain 9 | ENV PATH="/build/toolchain/raspbian-toolchain-gcc-4.7.2-linux64/bin:${PATH}" 10 | 11 | ADD toolchain-arm-linux.cmake /build/toolchain/ 12 | 13 | VOLUME /build/workspace 14 | WORKDIR /build/workspace 15 | 16 | CMD mkdir ./bin; cd ./bin; \ 17 | cmake -G "Unix Makefiles" -DCMAKE_TOOLCHAIN_FILE:FILEPATH="/build/toolchain/toolchain-arm-linux.cmake" -DBUILD_NUM=$TRAVIS_BUILD_NUMBER ..; \ 18 | make; \ 19 | cpack -G DEB 20 | -------------------------------------------------------------------------------- /build/raspbian/toolchain-arm-linux.cmake: -------------------------------------------------------------------------------- 1 | set(CMAKE_SYSTEM_NAME raspbian) 2 | 3 | set(CMAKE_C_COMPILER arm-linux-gnueabihf-gcc) 4 | set(CMAKE_CXX_COMPILER arm-linux-gnueabihf-g++) 5 | 6 | set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) 7 | set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) 8 | set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) 9 | set(CMAKE_FIND_ROOT_PATH_MODE_PACKAGE ONLY) 10 | -------------------------------------------------------------------------------- /hardware/enclosure1/Back.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/envirover/SPLRadioRoom/3c7c61a71268af851a264e422bc7cd4eea9226ba/hardware/enclosure1/Back.stl -------------------------------------------------------------------------------- /hardware/enclosure1/Base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/envirover/SPLRadioRoom/3c7c61a71268af851a264e422bc7cd4eea9226ba/hardware/enclosure1/Base.stl -------------------------------------------------------------------------------- /hardware/enclosure1/Front.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/envirover/SPLRadioRoom/3c7c61a71268af851a264e422bc7cd4eea9226ba/hardware/enclosure1/Front.stl -------------------------------------------------------------------------------- /hardware/enclosure1/Top.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/envirover/SPLRadioRoom/3c7c61a71268af851a264e422bc7cd4eea9226ba/hardware/enclosure1/Top.stl -------------------------------------------------------------------------------- /hardware/rpi-radioroom.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/envirover/SPLRadioRoom/3c7c61a71268af851a264e422bc7cd4eea9226ba/hardware/rpi-radioroom.jpg -------------------------------------------------------------------------------- /hardware/rpi-radioroom.md: -------------------------------------------------------------------------------- 1 | # UV RadioRoom with Raspberry PI 3 and RockBLOCK 9603 2 | 3 | See [enclosure1](enclosure1) folder for the STL files of the 3D printed enclosure. 4 | 5 | ![RPi RadioRoom](rpi-radioroom.jpg) 6 | 7 | # Bill of Materials 8 | 9 | |Part Name|Quantity| 10 | |---------|--------| 11 | |[Raspberry Pi 3 Model B+](https://www.raspberrypi.org/products/raspberry-pi-3-model-b-plus/)|1| 12 | |[RockBLOCK 9603](https://www.rock7.com/products/rockblock-9603-compact-plug-play-satellite-transmitter)|1| 13 | |[DROK 5A USB Voltage Regulator DC 9V-36V](https://www.droking.com/Power-Supply-Module-DC-9V-36V-to-5.2V-5A-Double-Output-Buck-Converter-USB-Charger-Voltage-Regulator-Adapter-Driver-Module)|1| 14 | |[Short FTDI USB Serial Cable for RockBLOCK 9603](https://www.amazon.com/Short-FTDI-Serial-Cable-RockBLOCK/dp/B08CXMQ1Y9)|1| 15 | |[TTL-232R-3V3-PCB](https://www.digikey.com/en/products/detail/ftdi-future-technology-devices-international-ltd/TTL-232R-3V3-PCB/1836396)|1| 16 | |[Sixfab Raspberry Pi 4G/LTE Cellular Modem Kit - Quectel EG25-G (Global)](https://sixfab.com/product/raspberry-pi-4g-lte-modem-kit/)|1| 17 | |6 Inches - 90 Degree USB to Micro USB Cable|1| 18 | |Tails cable with 6 pin DF13 series connector|1| 19 | |Self Tapping Screws M2.5 8mm|10| 20 | |Self Tapping Screws M2.5 6mm|2| 21 | |PETG or PLA filament|120g| 22 | -------------------------------------------------------------------------------- /libs/inih/include/INIReader.h: -------------------------------------------------------------------------------- 1 | // Read an INI file into easy-to-access name/value pairs. 2 | 3 | // inih and INIReader are released under the New BSD license (see LICENSE.txt). 4 | // Go to the project home page for more info: 5 | // 6 | // https://github.com/benhoyt/inih 7 | 8 | #ifndef __INIREADER_H__ 9 | #define __INIREADER_H__ 10 | 11 | #include 12 | #include 13 | 14 | // Read an INI file into easy-to-access name/value pairs. (Note that I've gone 15 | // for simplicity here rather than speed, but it should be pretty decent.) 16 | class INIReader 17 | { 18 | public: 19 | // Construct INIReader and parse given filename. See ini.h for more info 20 | // about the parsing. 21 | INIReader(const std::string& filename); 22 | 23 | // Return the result of ini_parse(), i.e., 0 on success, line number of 24 | // first error on parse error, or -1 on file open error. 25 | int ParseError() const; 26 | 27 | // Get a string value from INI file, returning default_value if not found. 28 | std::string Get(const std::string& section, const std::string& name, 29 | const std::string& default_value) const; 30 | 31 | // Get an integer (long) value from INI file, returning default_value if 32 | // not found or not a valid integer (decimal "1234", "-1234", or hex "0x4d2"). 33 | long GetInteger(const std::string& section, const std::string& name, long default_value) const; 34 | 35 | // Get a real (floating point double) value from INI file, returning 36 | // default_value if not found or not a valid floating point value 37 | // according to strtod(). 38 | double GetReal(const std::string& section, const std::string& name, double default_value) const; 39 | 40 | // Get a boolean value from INI file, returning default_value if not found or if 41 | // not a valid true/false value. Valid true values are "true", "yes", "on", "1", 42 | // and valid false values are "false", "no", "off", "0" (not case sensitive). 43 | bool GetBoolean(const std::string& section, const std::string& name, bool default_value) const; 44 | 45 | private: 46 | int _error; 47 | std::map _values; 48 | static std::string MakeKey(const std::string& section, const std::string& name); 49 | static int ValueHandler(void* user, const char* section, const char* name, 50 | const char* value); 51 | }; 52 | 53 | #endif // __INIREADER_H__ 54 | -------------------------------------------------------------------------------- /libs/inih/include/ini.h: -------------------------------------------------------------------------------- 1 | /* inih -- simple .INI file parser 2 | 3 | inih is released under the New BSD license (see LICENSE.txt). Go to the project 4 | home page for more info: 5 | 6 | https://github.com/benhoyt/inih 7 | 8 | */ 9 | 10 | #ifndef __INI_H__ 11 | #define __INI_H__ 12 | 13 | /* Make this header file easier to include in C++ code */ 14 | #ifdef __cplusplus 15 | extern "C" { 16 | #endif 17 | 18 | #include 19 | 20 | /* Nonzero if ini_handler callback should accept lineno parameter. */ 21 | #ifndef INI_HANDLER_LINENO 22 | #define INI_HANDLER_LINENO 0 23 | #endif 24 | 25 | /* Typedef for prototype of handler function. */ 26 | #if INI_HANDLER_LINENO 27 | typedef int (*ini_handler)(void* user, const char* section, 28 | const char* name, const char* value, 29 | int lineno); 30 | #else 31 | typedef int (*ini_handler)(void* user, const char* section, 32 | const char* name, const char* value); 33 | #endif 34 | 35 | /* Typedef for prototype of fgets-style reader function. */ 36 | typedef char* (*ini_reader)(char* str, int num, void* stream); 37 | 38 | /* Parse given INI-style file. May have [section]s, name=value pairs 39 | (whitespace stripped), and comments starting with ';' (semicolon). Section 40 | is "" if name=value pair parsed before any section heading. name:value 41 | pairs are also supported as a concession to Python's configparser. 42 | 43 | For each name=value pair parsed, call handler function with given user 44 | pointer as well as section, name, and value (data only valid for duration 45 | of handler call). Handler should return nonzero on success, zero on error. 46 | 47 | Returns 0 on success, line number of first error on parse error (doesn't 48 | stop on first error), -1 on file open error, or -2 on memory allocation 49 | error (only when INI_USE_STACK is zero). 50 | */ 51 | int ini_parse(const char* filename, ini_handler handler, void* user); 52 | 53 | /* Same as ini_parse(), but takes a FILE* instead of filename. This doesn't 54 | close the file when it's finished -- the caller must do that. */ 55 | int ini_parse_file(FILE* file, ini_handler handler, void* user); 56 | 57 | /* Same as ini_parse(), but takes an ini_reader function pointer instead of 58 | filename. Used for implementing custom or string-based I/O (see also 59 | ini_parse_string). */ 60 | int ini_parse_stream(ini_reader reader, void* stream, ini_handler handler, 61 | void* user); 62 | 63 | /* Same as ini_parse(), but takes a zero-terminated string with the INI data 64 | instead of a file. Useful for parsing INI data from a network socket or 65 | already in memory. */ 66 | int ini_parse_string(const char* string, ini_handler handler, void* user); 67 | 68 | /* Nonzero to allow multi-line value parsing, in the style of Python's 69 | configparser. If allowed, ini_parse() will call the handler with the same 70 | name for each subsequent line parsed. */ 71 | #ifndef INI_ALLOW_MULTILINE 72 | #define INI_ALLOW_MULTILINE 1 73 | #endif 74 | 75 | /* Nonzero to allow a UTF-8 BOM sequence (0xEF 0xBB 0xBF) at the start of 76 | the file. See http://code.google.com/p/inih/issues/detail?id=21 */ 77 | #ifndef INI_ALLOW_BOM 78 | #define INI_ALLOW_BOM 1 79 | #endif 80 | 81 | /* Nonzero to allow inline comments (with valid inline comment characters 82 | specified by INI_INLINE_COMMENT_PREFIXES). Set to 0 to turn off and match 83 | Python 3.2+ configparser behaviour. */ 84 | #ifndef INI_ALLOW_INLINE_COMMENTS 85 | #define INI_ALLOW_INLINE_COMMENTS 1 86 | #endif 87 | #ifndef INI_INLINE_COMMENT_PREFIXES 88 | #define INI_INLINE_COMMENT_PREFIXES ";" 89 | #endif 90 | 91 | /* Nonzero to use stack, zero to use heap (malloc/free). */ 92 | #ifndef INI_USE_STACK 93 | #define INI_USE_STACK 1 94 | #endif 95 | 96 | /* Stop parsing on first error (default is to keep parsing). */ 97 | #ifndef INI_STOP_ON_FIRST_ERROR 98 | #define INI_STOP_ON_FIRST_ERROR 0 99 | #endif 100 | 101 | /* Maximum line length for any line in INI file. */ 102 | #ifndef INI_MAX_LINE 103 | #define INI_MAX_LINE 1024 104 | #endif 105 | 106 | #ifdef __cplusplus 107 | } 108 | #endif 109 | 110 | #endif /* __INI_H__ */ 111 | -------------------------------------------------------------------------------- /libs/inih/src/INIReader.cc: -------------------------------------------------------------------------------- 1 | // Read an INI file into easy-to-access name/value pairs. 2 | 3 | // inih and INIReader are released under the New BSD license (see LICENSE.txt). 4 | // Go to the project home page for more info: 5 | // 6 | // https://github.com/benhoyt/inih 7 | 8 | #include 9 | #include 10 | #include 11 | #include "ini.h" 12 | #include "INIReader.h" 13 | 14 | using std::string; 15 | 16 | INIReader::INIReader(const string& filename) 17 | { 18 | _error = ini_parse(filename.c_str(), ValueHandler, this); 19 | } 20 | 21 | int INIReader::ParseError() const 22 | { 23 | return _error; 24 | } 25 | 26 | string INIReader::Get(const string& section, const string& name, const string& default_value) const 27 | { 28 | string key = MakeKey(section, name); 29 | // Use _values.find() here instead of _values.at() to support pre C++11 compilers 30 | return _values.count(key) ? _values.find(key)->second : default_value; 31 | } 32 | 33 | long INIReader::GetInteger(const string& section, const string& name, long default_value) const 34 | { 35 | string valstr = Get(section, name, ""); 36 | const char* value = valstr.c_str(); 37 | char* end; 38 | // This parses "1234" (decimal) and also "0x4D2" (hex) 39 | long n = strtol(value, &end, 0); 40 | return end > value ? n : default_value; 41 | } 42 | 43 | double INIReader::GetReal(const string& section, const string& name, double default_value) const 44 | { 45 | string valstr = Get(section, name, ""); 46 | const char* value = valstr.c_str(); 47 | char* end; 48 | double n = strtod(value, &end); 49 | return end > value ? n : default_value; 50 | } 51 | 52 | bool INIReader::GetBoolean(const string& section, const string& name, bool default_value) const 53 | { 54 | string valstr = Get(section, name, ""); 55 | // Convert to lower case to make string comparisons case-insensitive 56 | std::transform(valstr.begin(), valstr.end(), valstr.begin(), ::tolower); 57 | if (valstr == "true" || valstr == "yes" || valstr == "on" || valstr == "1") 58 | return true; 59 | else if (valstr == "false" || valstr == "no" || valstr == "off" || valstr == "0") 60 | return false; 61 | else 62 | return default_value; 63 | } 64 | 65 | string INIReader::MakeKey(const string& section, const string& name) 66 | { 67 | string key = section + "=" + name; 68 | // Convert to lower case to make section/name lookups case-insensitive 69 | std::transform(key.begin(), key.end(), key.begin(), ::tolower); 70 | return key; 71 | } 72 | 73 | int INIReader::ValueHandler(void* user, const char* section, const char* name, 74 | const char* value) 75 | { 76 | INIReader* reader = (INIReader*)user; 77 | string key = MakeKey(section, name); 78 | if (reader->_values[key].size() > 0) 79 | reader->_values[key] += "\n"; 80 | reader->_values[key] += value; 81 | return 1; 82 | } 83 | -------------------------------------------------------------------------------- /libs/inih/src/ini.c: -------------------------------------------------------------------------------- 1 | /* inih -- simple .INI file parser 2 | 3 | inih is released under the New BSD license (see LICENSE.txt). Go to the project 4 | home page for more info: 5 | 6 | https://github.com/benhoyt/inih 7 | 8 | */ 9 | 10 | #if defined(_MSC_VER) && !defined(_CRT_SECURE_NO_WARNINGS) 11 | #define _CRT_SECURE_NO_WARNINGS 12 | #endif 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include "ini.h" 19 | 20 | #if !INI_USE_STACK 21 | #include 22 | #endif 23 | 24 | #define MAX_SECTION 50 25 | #define MAX_NAME 50 26 | 27 | /* Used by ini_parse_string() to keep track of string parsing state. */ 28 | typedef struct { 29 | const char* ptr; 30 | size_t num_left; 31 | } ini_parse_string_ctx; 32 | 33 | /* Strip whitespace chars off end of given string, in place. Return s. */ 34 | static char* rstrip(char* s) 35 | { 36 | char* p = s + strlen(s); 37 | while (p > s && isspace((unsigned char)(*--p))) 38 | *p = '\0'; 39 | return s; 40 | } 41 | 42 | /* Return pointer to first non-whitespace char in given string. */ 43 | static char* lskip(const char* s) 44 | { 45 | while (*s && isspace((unsigned char)(*s))) 46 | s++; 47 | return (char*)s; 48 | } 49 | 50 | /* Return pointer to first char (of chars) or inline comment in given string, 51 | or pointer to null at end of string if neither found. Inline comment must 52 | be prefixed by a whitespace character to register as a comment. */ 53 | static char* find_chars_or_comment(const char* s, const char* chars) 54 | { 55 | #if INI_ALLOW_INLINE_COMMENTS 56 | int was_space = 0; 57 | while (*s && (!chars || !strchr(chars, *s)) && 58 | !(was_space && strchr(INI_INLINE_COMMENT_PREFIXES, *s))) { 59 | was_space = isspace((unsigned char)(*s)); 60 | s++; 61 | } 62 | #else 63 | while (*s && (!chars || !strchr(chars, *s))) { 64 | s++; 65 | } 66 | #endif 67 | return (char*)s; 68 | } 69 | 70 | /* Version of strncpy that ensures dest (size bytes) is null-terminated. */ 71 | static char* strncpy0(char* dest, const char* src, size_t size) 72 | { 73 | strncpy(dest, src, size); 74 | dest[size - 1] = '\0'; 75 | return dest; 76 | } 77 | 78 | /* See documentation in header file. */ 79 | int ini_parse_stream(ini_reader reader, void* stream, ini_handler handler, 80 | void* user) 81 | { 82 | /* Uses a fair bit of stack (use heap instead if you need to) */ 83 | #if INI_USE_STACK 84 | char line[INI_MAX_LINE]; 85 | #else 86 | char* line; 87 | #endif 88 | char section[MAX_SECTION] = ""; 89 | char prev_name[MAX_NAME] = ""; 90 | 91 | char* start; 92 | char* end; 93 | char* name; 94 | char* value; 95 | int lineno = 0; 96 | int error = 0; 97 | 98 | #if !INI_USE_STACK 99 | line = (char*)malloc(INI_MAX_LINE); 100 | if (!line) { 101 | return -2; 102 | } 103 | #endif 104 | 105 | #if INI_HANDLER_LINENO 106 | #define HANDLER(u, s, n, v) handler(u, s, n, v, lineno) 107 | #else 108 | #define HANDLER(u, s, n, v) handler(u, s, n, v) 109 | #endif 110 | 111 | /* Scan through stream line by line */ 112 | while (reader(line, INI_MAX_LINE, stream) != NULL) { 113 | lineno++; 114 | 115 | start = line; 116 | #if INI_ALLOW_BOM 117 | if (lineno == 1 && (unsigned char)start[0] == 0xEF && 118 | (unsigned char)start[1] == 0xBB && 119 | (unsigned char)start[2] == 0xBF) { 120 | start += 3; 121 | } 122 | #endif 123 | start = lskip(rstrip(start)); 124 | 125 | if (*start == ';' || *start == '#') { 126 | /* Per Python configparser, allow both ; and # comments at the 127 | start of a line */ 128 | } 129 | #if INI_ALLOW_MULTILINE 130 | else if (*prev_name && *start && start > line) { 131 | /* Non-blank line with leading whitespace, treat as continuation 132 | of previous name's value (as per Python configparser). */ 133 | if (!HANDLER(user, section, prev_name, start) && !error) 134 | error = lineno; 135 | } 136 | #endif 137 | else if (*start == '[') { 138 | /* A "[section]" line */ 139 | end = find_chars_or_comment(start + 1, "]"); 140 | if (*end == ']') { 141 | *end = '\0'; 142 | strncpy0(section, start + 1, sizeof(section)); 143 | *prev_name = '\0'; 144 | } 145 | else if (!error) { 146 | /* No ']' found on section line */ 147 | error = lineno; 148 | } 149 | } 150 | else if (*start) { 151 | /* Not a comment, must be a name[=:]value pair */ 152 | end = find_chars_or_comment(start, "=:"); 153 | if (*end == '=' || *end == ':') { 154 | *end = '\0'; 155 | name = rstrip(start); 156 | value = end + 1; 157 | #if INI_ALLOW_INLINE_COMMENTS 158 | end = find_chars_or_comment(value, NULL); 159 | if (*end) 160 | *end = '\0'; 161 | #endif 162 | value = lskip(value); 163 | rstrip(value); 164 | 165 | /* Valid name[=:]value pair found, call handler */ 166 | strncpy0(prev_name, name, sizeof(prev_name)); 167 | if (!HANDLER(user, section, name, value) && !error) 168 | error = lineno; 169 | } 170 | else if (!error) { 171 | /* No '=' or ':' found on name[=:]value line */ 172 | error = lineno; 173 | } 174 | } 175 | 176 | #if INI_STOP_ON_FIRST_ERROR 177 | if (error) 178 | break; 179 | #endif 180 | } 181 | 182 | #if !INI_USE_STACK 183 | free(line); 184 | #endif 185 | 186 | return error; 187 | } 188 | 189 | /* See documentation in header file. */ 190 | int ini_parse_file(FILE* file, ini_handler handler, void* user) 191 | { 192 | return ini_parse_stream((ini_reader)fgets, file, handler, user); 193 | } 194 | 195 | /* See documentation in header file. */ 196 | int ini_parse(const char* filename, ini_handler handler, void* user) 197 | { 198 | FILE* file; 199 | int error; 200 | 201 | file = fopen(filename, "r"); 202 | if (!file) 203 | return -1; 204 | error = ini_parse_file(file, handler, user); 205 | fclose(file); 206 | return error; 207 | } 208 | 209 | /* An ini_reader function to read the next line from a string buffer. This 210 | is the fgets() equivalent used by ini_parse_string(). */ 211 | static char* ini_reader_string(char* str, int num, void* stream) { 212 | ini_parse_string_ctx* ctx = (ini_parse_string_ctx*)stream; 213 | const char* ctx_ptr = ctx->ptr; 214 | size_t ctx_num_left = ctx->num_left; 215 | char* strp = str; 216 | char c; 217 | 218 | if (ctx_num_left == 0 || num < 2) 219 | return NULL; 220 | 221 | while (num > 1 && ctx_num_left != 0) { 222 | c = *ctx_ptr++; 223 | ctx_num_left--; 224 | *strp++ = c; 225 | if (c == '\n') 226 | break; 227 | num--; 228 | } 229 | 230 | *strp = '\0'; 231 | ctx->ptr = ctx_ptr; 232 | ctx->num_left = ctx_num_left; 233 | return str; 234 | } 235 | 236 | /* See documentation in header file. */ 237 | int ini_parse_string(const char* string, ini_handler handler, void* user) { 238 | ini_parse_string_ctx ctx; 239 | 240 | ctx.ptr = string; 241 | ctx.num_left = strlen(string); 242 | return ini_parse_stream((ini_reader)ini_reader_string, &ctx, handler, 243 | user); 244 | } 245 | -------------------------------------------------------------------------------- /libs/mavio/include/CircularBuffer.h: -------------------------------------------------------------------------------- 1 | /* 2 | CircularBuffer.h 3 | 4 | MAVIO MAVLink I/O library. 5 | 6 | Copyright (C) 2019 Envirover 7 | 8 | All rights reserved. 9 | 10 | This library is free software; you can redistribute it and/or 11 | modify it under the terms of the GNU Lesser General Public 12 | License as published by the Free Software Foundation; either 13 | version 2.1 of the License, or (at your option) any later version. 14 | 15 | This library is distributed in the hope that it will be useful, 16 | but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 18 | Lesser General Public License for more details. 19 | 20 | You should have received a copy of the GNU Lesser General Public 21 | License along with this library; if not, write to the Free Software 22 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 23 | */ 24 | 25 | #ifndef LIBS_MAVIO_INCLUDE_CIRCULARBUFFER_H_ 26 | #define LIBS_MAVIO_INCLUDE_CIRCULARBUFFER_H_ 27 | 28 | #include 29 | #include 30 | 31 | namespace mavio { 32 | 33 | /** 34 | * Thread-safe fixed-size queue. 35 | */ 36 | template 37 | class CircularBuffer { 38 | public: 39 | explicit CircularBuffer(size_t size) 40 | : buf_(std::unique_ptr(new T[size])), 41 | max_size_(size) {} 42 | 43 | void push(T item) { 44 | std::lock_guard lock(mutex_); 45 | 46 | buf_[head_] = item; 47 | 48 | if (full_) { 49 | tail_ = (tail_ + 1) % max_size_; 50 | } 51 | 52 | head_ = (head_ + 1) % max_size_; 53 | 54 | full_ = head_ == tail_; 55 | } 56 | 57 | bool pop(T& item) { 58 | std::lock_guard lock(mutex_); 59 | 60 | if (empty()) { 61 | return false; 62 | } 63 | 64 | // Read data and advance the tail (we now have a free space) 65 | item = buf_[tail_]; 66 | full_ = false; 67 | tail_ = (tail_ + 1) % max_size_; 68 | 69 | return true; 70 | } 71 | 72 | void reset() { 73 | std::lock_guard lock(mutex_); 74 | head_ = tail_; 75 | full_ = false; 76 | } 77 | 78 | bool empty() const { 79 | // if head and tail are equal, we are empty 80 | return (!full_ && (head_ == tail_)); 81 | } 82 | 83 | bool full() const { 84 | // If tail is ahead the head by 1, we are full 85 | return full_; 86 | } 87 | 88 | size_t capacity() const { return max_size_; } 89 | 90 | size_t size() const { 91 | size_t size = max_size_; 92 | 93 | if (!full_) { 94 | if (head_ >= tail_) { 95 | size = head_ - tail_; 96 | } else { 97 | size = max_size_ + head_ - tail_; 98 | } 99 | } 100 | 101 | return size; 102 | } 103 | 104 | private: 105 | std::mutex mutex_; 106 | std::unique_ptr buf_; 107 | size_t head_ = 0; 108 | size_t tail_ = 0; 109 | const size_t max_size_; 110 | bool full_ = 0; 111 | }; 112 | 113 | } // namespace mavio 114 | 115 | #endif // LIBS_MAVIO_INCLUDE_CIRCULARBUFFER_H_ 116 | -------------------------------------------------------------------------------- /libs/mavio/include/IridiumSBD.h: -------------------------------------------------------------------------------- 1 | /* 2 | IridiumSBD.h 3 | 4 | MAVIO MAVLink I/O library. 5 | 6 | Original work Copyright (C) 2013-4 Mikal Hart 7 | Modified work Copyright (C) 2017-9 Envirover 8 | 9 | All rights reserved. 10 | 11 | This library is free software; you can redistribute it and/or 12 | modify it under the terms of the GNU Lesser General Public 13 | License as published by the Free Software Foundation; either 14 | version 2.1 of the License, or (at your option) any later version. 15 | 16 | This library is distributed in the hope that it will be useful, 17 | but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 19 | Lesser General Public License for more details. 20 | 21 | You should have received a copy of the GNU Lesser General Public 22 | License along with this library; if not, write to the Free Software 23 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 24 | */ 25 | 26 | #ifndef LIBS_MAVIO_INCLUDE_IRIDIUMSBD_H_ 27 | #define LIBS_MAVIO_INCLUDE_IRIDIUMSBD_H_ 28 | 29 | #include "Serial.h" 30 | 31 | #include 32 | 33 | #define ISBD_SUCCESS 0 34 | #define ISBD_ALREADY_AWAKE 1 35 | #define ISBD_SERIAL_FAILURE 2 36 | #define ISBD_PROTOCOL_ERROR 3 37 | #define ISBD_CANCELLED 4 38 | #define ISBD_NO_MODEM_DETECTED 5 39 | #define ISBD_SBDIX_FATAL_ERROR 6 40 | #define ISBD_SENDRECEIVE_TIMEOUT 7 41 | #define ISBD_RX_OVERFLOW 8 42 | #define ISBD_REENTRANT 9 43 | #define ISBD_IS_ASLEEP 10 44 | #define ISBD_NO_SLEEP_PIN 11 45 | 46 | #define ISBD_MAX_MT_MGS_SIZE 270 47 | 48 | namespace mavio { 49 | 50 | /** 51 | * POSIX implementation for Iridium SBD transceiver communication. 52 | */ 53 | class IridiumSBD { 54 | public: 55 | IridiumSBD(Serial& serial); 56 | 57 | int begin(); 58 | 59 | int getTransceiverModel(char* buffer, size_t bufferSize); 60 | int getTransceiverSerialNumber(char* buffer, size_t bufferSize); 61 | int sendSBDText(const char* message); 62 | int sendSBDBinary(const uint8_t* txData, size_t txDataSize); 63 | int sendReceiveSBDText(const char* message, uint8_t* rxBuffer, 64 | size_t& rxBufferSize); 65 | int sendReceiveSBDBinary(const uint8_t* txData, size_t txDataSize, 66 | uint8_t* rxBuffer, size_t& rxBufferSize); 67 | int getSignalQuality(int& quality); 68 | int queryRingIndicationStatus(int& sri); 69 | 70 | // This command returns current state of the mobile originated and mobile 71 | // terminated buffers, and the SBD ring alert status. 72 | int getStatusExtended(uint16_t& moFlag, uint16_t& moMSN, uint16_t& mtFlag, 73 | uint16_t& mtMSN, uint16_t& raFlag, 74 | uint16_t& msgWaiting); 75 | 76 | int getWaitingMessageCount(); 77 | int sleep(); 78 | bool isAsleep(); 79 | 80 | void setPowerProfile(int profile); // 0 = direct connect (default), 1 = USB 81 | void adjustATTimeout(std::chrono::milliseconds ms); // default value = 20 seconds 82 | void adjustSendReceiveTimeout(std::chrono::milliseconds ms); // default value = 300 seconds 83 | void setMinimumSignalQuality( 84 | int quality); // a number between 1 and 5, default 85 | // ISBD_DEFAULT_CSQ_MINIMUM 86 | void useMSSTMWorkaround( 87 | bool useWorkAround); // true to use workaround from Iridium Alert 5/7 88 | 89 | private: 90 | // Internal utilities 91 | bool smartWait(std::chrono::milliseconds ms); 92 | bool waitForATResponse(char* response = NULL, int responseSize = 0, 93 | const char* prompt = NULL, 94 | const char* terminator = "OK\r\n"); 95 | 96 | int internalBegin(); 97 | int internalGetTransceiverModel(char* buffer, size_t bufferSize); 98 | int internalGetTransceiverSerialNumber(char* buffer, size_t bufferSize); 99 | int internalSendReceiveSBD(const char* txTxtMessage, const uint8_t* txData, 100 | size_t txDataSize, uint8_t* rxBuffer, 101 | size_t* prxBufferSize); 102 | int internalQueryRingIndicationStatus(int& sri); 103 | int internalGetStatusExtended(uint16_t& moFlag, uint16_t& moMSN, 104 | uint16_t& mtFlag, uint16_t& mtMSN, 105 | uint16_t& raFlag, uint16_t& msgWaiting); 106 | int internalGetSignalQuality(int& quality); 107 | int internalMSSTMWorkaround(bool& okToProceed); 108 | int internalSleep(); 109 | 110 | int doSBDIX(uint16_t& moCode, uint16_t& moMSN, uint16_t& mtCode, 111 | uint16_t& mtMSN, uint16_t& mtLen, uint16_t& mtRemaining); 112 | int doSBDRB(uint8_t* rxBuffer, size_t* prxBufferSize); // in/out 113 | int readUInt(uint16_t& u); 114 | void power(bool on); 115 | 116 | void send(const char* str); 117 | void send(uint16_t n); 118 | 119 | bool cancelled(); 120 | 121 | Serial& stream; // Communicating with the Iridium 122 | 123 | // Timings milliseconds 124 | std::chrono::milliseconds csqInterval; 125 | std::chrono::milliseconds sbdixInterval; 126 | std::chrono::milliseconds atTimeout; 127 | std::chrono::milliseconds sendReceiveTimeout; 128 | 129 | // State variables 130 | int remainingMessages; 131 | int sleepPin; 132 | bool asleep; 133 | bool reentrant; 134 | int minimumCSQ; 135 | bool useWorkaround; 136 | unsigned long lastPowerOnTime; 137 | }; 138 | 139 | } // namespace mavio 140 | 141 | #endif // LIBS_MAVIO_INCLUDE_IRIDIUMSBD_H_ -------------------------------------------------------------------------------- /libs/mavio/include/Logger.h: -------------------------------------------------------------------------------- 1 | /* 2 | Logger.h 3 | 4 | MAVIO MAVLink I/O library. 5 | 6 | (C) Copyright 2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | 23 | #ifndef LIBS_MAVIO_INCLUDE_LOGGER_H_ 24 | #define LIBS_MAVIO_INCLUDE_LOGGER_H_ 25 | 26 | // Log priorities 27 | #define LOG_EMERG 0 28 | #define LOG_ALERT 1 29 | #define LOG_CRIT 2 30 | #define LOG_ERR 3 31 | #define LOG_WARNING 4 32 | #define LOG_NOTICE 5 33 | #define LOG_INFO 6 34 | #define LOG_DEBUG 7 35 | 36 | // Log mask up to priority 37 | #define LOG_UPTO(pri) ((1 << ((pri) + 1)) - 1) 38 | 39 | namespace mavio { 40 | 41 | // Initializes global logger. 42 | // Mask determines which future log() calls shall be ignored. 43 | void openlog(const char* identity, int mask); 44 | 45 | // Closes logger 46 | void closelog(); 47 | 48 | // Logs message with given priority 49 | // Interface matches ::syslog() method. 50 | void log(int priority, const char* format, ...); 51 | 52 | } // namespace mavio 53 | 54 | #endif // LIBS_MAVIO_INCLUDE_LOGGER_H_ 55 | -------------------------------------------------------------------------------- /libs/mavio/include/MAVLinkAutopilot.h: -------------------------------------------------------------------------------- 1 | /* 2 | MAVLinkAutopilot.h 3 | 4 | MAVIO MAVLink I/O library. 5 | 6 | 7 | (C) Copyright 2019 Envirover. 8 | 9 | This library is free software; you can redistribute it and/or 10 | modify it under the terms of the GNU Lesser General Public 11 | License as published by the Free Software Foundation; either 12 | version 2.1 of the License, or (at your option) any later version. 13 | 14 | This library is distributed in the hope that it will be useful, 15 | but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 17 | Lesser General Public License for more details. 18 | 19 | You should have received a copy of the GNU Lesser General Public 20 | License along with this library; if not, write to the Free Software 21 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 22 | */ 23 | 24 | #ifndef LIBS_MAVIO_INCLUDE_MAVLINKAUTOPILOT_H_ 25 | #define LIBS_MAVIO_INCLUDE_MAVLINKAUTOPILOT_H_ 26 | 27 | #include "CircularBuffer.h" 28 | #include "MAVLinkChannel.h" 29 | #include "MAVLinkLib.h" 30 | #include "MAVLinkSerial.h" 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | namespace mavio { 37 | 38 | constexpr uint8_t gcs_system_id = 255; // GCS system Id 39 | constexpr uint8_t gcs_component_id = 1; // GCS component Id 40 | constexpr uint8_t ardupilot_component_id = 0; 41 | 42 | /** 43 | * Asynchronously sends and receivs MAVLink messages to autopilot over serial 44 | * interface. 45 | */ 46 | class MAVLinkAutopilot : public MAVLinkChannel { 47 | public: 48 | /** 49 | * Constructs MAVLinkSerial instance. 50 | */ 51 | MAVLinkAutopilot(); 52 | 53 | /** 54 | * Initializes connection to the serial device and starts send and receive 55 | * tasks. 56 | * 57 | * @param path serial device path 58 | * @param speed serial device speed 59 | * @param devices list of all serial devices 60 | * @return true if initialization succeeded 61 | */ 62 | bool init(const std::string& path, int speed, 63 | const std::vector& devices); 64 | 65 | /** 66 | * Stops send and receive tasks and closes the serial device. 67 | */ 68 | void close() override; 69 | 70 | /** 71 | * Returns the path of serial device set by init(...) call. 72 | */ 73 | inline std::string get_path() const { return serial.get_path(); } 74 | 75 | /** 76 | * Returns the autopilot system id. 77 | */ 78 | inline uint8_t get_system_id() const { return system_id; } 79 | 80 | /** 81 | * Send MAVLink message to autopilot. 82 | * 83 | * @param msg MAVLink message 84 | * @return always returns true 85 | */ 86 | bool send_message(const mavlink_message_t& msg) override; 87 | 88 | /** 89 | * Receive MAVLink message from autopilot. 90 | * 91 | * @param msg reference to MAVLink message object 92 | * @return true if MAVLink message was received 93 | */ 94 | bool receive_message(mavlink_message_t& msg) override; 95 | 96 | /** 97 | * Returns true if there is a message in the receive queue. 98 | */ 99 | bool message_available() override; 100 | 101 | /** 102 | * Returns time of the last successfully sent message. 103 | */ 104 | std::chrono::milliseconds last_send_time() override; 105 | 106 | /** 107 | * Returns time of the last successfully received message. 108 | */ 109 | std::chrono::milliseconds last_receive_time() override; 110 | 111 | private: 112 | /** 113 | * Connects to the serial device. 114 | */ 115 | bool connect(const std::string& path, int speed, 116 | const std::vector& devices); 117 | 118 | /* 119 | * Checks if MAVLink autopilot is available on the specified serial device. 120 | * 121 | * If an autopilot was detected, returns the autopilot's system id, 122 | * otherwise returns 0. 123 | */ 124 | uint8_t detect_autopilot(const std::string device); 125 | 126 | /** 127 | * Sends REQUEST_AUTOPILOT_CAPABILITIES message to the autopilot and 128 | * reads AUTOPILOT_VERSION message replied by the autopilot. 129 | * 130 | * Returns true if AUTOPILOT_VERSION message was received. 131 | */ 132 | bool request_autopilot_version( 133 | uint8_t& autopilot, uint8_t& mav_type, uint8_t& sys_id, 134 | mavlink_autopilot_version_t& autopilot_version); 135 | 136 | /** 137 | * Retrieves firmware version string from the specified AUTOPILOT_VERSION 138 | * message. 139 | * 140 | * Returns the firmware version string. 141 | */ 142 | static char* get_firmware_version( 143 | const mavlink_autopilot_version_t& autopilot_version, char* buff, 144 | size_t buff_size); 145 | 146 | /** 147 | * While running is true, retrieves messages from send_queue and sends 148 | * them to serial. 149 | */ 150 | void send_task(); 151 | 152 | /** 153 | * While running is true, receives messages from serial and pushes them to 154 | * receive_queue. 155 | */ 156 | void receive_task(); 157 | 158 | std::atomic running; 159 | std::thread send_thread; // Thread of send_task 160 | std::thread receive_thread; // Thread of receive_task 161 | MAVLinkSerial serial; // Serial interface 162 | // Queue that buffers messages sent to autopilot 163 | CircularBuffer send_queue; 164 | // Queue that buffers messages received from autopilot 165 | CircularBuffer receive_queue; 166 | uint8_t system_id; // Autopilot system Id 167 | std::chrono::milliseconds send_time; // Last send epoch time 168 | std::chrono::milliseconds receive_time; // Last receive epoch time 169 | }; 170 | 171 | } // namespace mavio 172 | 173 | #endif // LIBS_MAVIO_INCLUDE_MAVLINKAUTOPILOT_H_ 174 | -------------------------------------------------------------------------------- /libs/mavio/include/MAVLinkChannel.h: -------------------------------------------------------------------------------- 1 | /* 2 | MAVLinkChannel.h 3 | 4 | MAVIO MAVLink I/O library. 5 | 6 | (C) Copyright 2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | 23 | #ifndef LIBS_MAVIO_INCLUDE_MAVLINKCHANNEL_H_ 24 | #define LIBS_MAVIO_INCLUDE_MAVLINKCHANNEL_H_ 25 | 26 | #include "MAVLinkLib.h" 27 | 28 | #include 29 | #include 30 | 31 | namespace mavio { 32 | 33 | // Channel IDs 34 | constexpr uint8_t autopilot_channel_id = 0; 35 | constexpr uint8_t tcp_channel_id = 1; 36 | constexpr uint8_t isbd_channel_id = 2; 37 | constexpr uint8_t camera_handler_channel_id = 3; 38 | 39 | /* 40 | * Interface for send/receive channels of MAVLink messages. 41 | */ 42 | class MAVLinkChannel { 43 | public: 44 | explicit MAVLinkChannel(uint8_t channel_id) : channel_id(channel_id) {} 45 | 46 | virtual ~MAVLinkChannel() {} 47 | 48 | /** 49 | * Returns the channel ID. 50 | */ 51 | virtual uint8_t get_channel_id() const { return channel_id; } 52 | 53 | /** 54 | * Closes the connection if it was open. 55 | */ 56 | virtual void close() = 0; 57 | 58 | /** 59 | * Sends the specified MAVLink message to the socket. 60 | * 61 | * Returns true if the message was sent successfully. 62 | */ 63 | virtual bool send_message(const mavlink_message_t& msg) = 0; 64 | 65 | /** 66 | * Receives MAVLink message from the socket. 67 | * 68 | * Returns true if a message was received. 69 | */ 70 | virtual bool receive_message(mavlink_message_t& msg) = 0; 71 | 72 | /** 73 | * Checks if data is available in the socket input buffer. 74 | * 75 | * Returns true if data is available. 76 | */ 77 | virtual bool message_available() = 0; 78 | 79 | /** 80 | * Returns time of the last successfully sent message. 81 | */ 82 | virtual std::chrono::milliseconds last_send_time() = 0; 83 | 84 | /** 85 | * Returns time of the last successfully received message. 86 | */ 87 | virtual std::chrono::milliseconds last_receive_time() = 0; 88 | 89 | private: 90 | uint8_t channel_id; 91 | }; 92 | 93 | } // namespace mavio 94 | 95 | #endif // LIBS_MAVIO_INCLUDE_MAVLINKCHANNEL_H_ 96 | -------------------------------------------------------------------------------- /libs/mavio/include/MAVLinkISBD.h: -------------------------------------------------------------------------------- 1 | /* 2 | MAVLinkSBD.h 3 | 4 | MAVIO MAVLink I/O library. 5 | 6 | (C) Copyright 2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | 23 | #ifndef LIBS_MAVIO_INCLUDE_MAVLINKISBD_H_ 24 | #define LIBS_MAVIO_INCLUDE_MAVLINKISBD_H_ 25 | 26 | #include 27 | #include 28 | 29 | #include "IridiumSBD.h" 30 | #include "MAVLinkLib.h" 31 | 32 | namespace mavio { 33 | 34 | /** 35 | * MAVLinkSBD is used to send/receive MAVLink messages to/from an ISBD 36 | * transceiver. 37 | */ 38 | class MAVLinkISBD { 39 | public: 40 | MAVLinkISBD(); 41 | ~MAVLinkISBD(); 42 | 43 | /** 44 | * Initializes connection to ISBD transceiver on the specified serial 45 | * device. The method automatically detects 46 | * the serial device if the transceiver is available on any of the serial 47 | * devices. 48 | * 49 | * Returns true if connection was successful. 50 | */ 51 | bool init(std::string path, int speed, 52 | const std::vector& devices); 53 | 54 | /* 55 | * Closes the serial device used to connect to ISBD. 56 | */ 57 | void close(); 58 | 59 | /** 60 | * Checks if data is available in ISBD. 61 | * 62 | * Returns true if data is available. 63 | */ 64 | bool message_available(); 65 | 66 | /** 67 | * Retrieves ring alert flag. 68 | * 69 | * Returns true is the operation was successful. 70 | */ 71 | bool get_ring_alert_flag(uint16_t& raFlag); 72 | 73 | /** 74 | * Returns the number of mobile terminated messages left in the queue. 75 | */ 76 | int get_waiting_wessage_count(); 77 | 78 | /** 79 | * Sends MT message to ISBD and receives MO message from the in-bound 80 | * message queue if any. 81 | * 82 | * Returns true if the ISBD session succeeded. 83 | */ 84 | bool send_receive_message(const mavlink_message_t& mo_msg, 85 | mavlink_message_t& mt_msg, bool& received); 86 | 87 | /** 88 | * Returns true if ISBD transceiver detected at the specified serial device. 89 | */ 90 | bool detect_transceiver(std::string device); 91 | 92 | /** 93 | * Queries ISBD signal quality. 94 | * 95 | * Returns true if the operation succeeded. 96 | */ 97 | bool get_signal_quality(int& quality); 98 | 99 | private: 100 | 101 | Serial stream; 102 | IridiumSBD isbd; 103 | }; 104 | 105 | } // namespace mavio 106 | 107 | #endif // LIBS_MAVIO_INCLUDE_MAVLINKISBD_H_ 108 | -------------------------------------------------------------------------------- /libs/mavio/include/MAVLinkISBDChannel.h: -------------------------------------------------------------------------------- 1 | /* 2 | MAVLinkISBDChannel.h 3 | 4 | MAVIO MAVLink I/O library. 5 | 6 | (C) Copyright 2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | 23 | #ifndef LIBS_MAVIO_INCLUDE_MAVLINKISBDCHANNEL_H_ 24 | #define LIBS_MAVIO_INCLUDE_MAVLINKISBDCHANNEL_H_ 25 | 26 | #include "CircularBuffer.h" 27 | #include "MAVLinkChannel.h" 28 | #include "MAVLinkISBD.h" 29 | #include "MAVLinkLib.h" 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | namespace mavio { 37 | 38 | /** 39 | * MAVLinkISBDChannel asynchronously sends and receives MAVLink messages to/from 40 | * an ISBD transceiver. 41 | */ 42 | class MAVLinkISBDChannel : public MAVLinkChannel { 43 | public: 44 | MAVLinkISBDChannel(); 45 | ~MAVLinkISBDChannel(); 46 | 47 | /** 48 | * Initializes connection to ISBD transceiver on the specified serial 49 | * device. The method automatically detects 50 | * the serial device if the transceiver is available on any of the serial 51 | * devices. 52 | * 53 | * Returns true if connection was successful. 54 | */ 55 | bool init(std::string path, int speed, 56 | const std::vector& devices); 57 | 58 | /* 59 | * Closes the serial device used to connect to ISBD. 60 | */ 61 | void close(); 62 | 63 | /** 64 | * Sends the specified MAVLink message to ISBD. 65 | * 66 | * Returns true if the message was sent successfully. 67 | */ 68 | bool send_message(const mavlink_message_t& msg); 69 | 70 | /** 71 | * Receives MAVLink message from ISBD transceiver. 72 | * 73 | * Returns true if a message was received. 74 | */ 75 | bool receive_message(mavlink_message_t& msg); 76 | 77 | /** 78 | * Checks if data is available in ISBD transceiver. 79 | * 80 | * Returns true if data is available. 81 | */ 82 | bool message_available(); 83 | 84 | /** 85 | * Returns time of the last successfully sent message. 86 | */ 87 | std::chrono::milliseconds last_send_time(); 88 | 89 | /** 90 | * Returns time of the last successfully received message. 91 | */ 92 | std::chrono::milliseconds last_receive_time(); 93 | 94 | /** 95 | * Queries ISBD signal quality. 96 | * 97 | * Returns true if the operation succeeded. 98 | */ 99 | bool get_signal_quality(int& quality); 100 | 101 | private: 102 | /** 103 | * While running is true, executes send-receive ISBD sessions. 104 | */ 105 | void send_receive_task(); 106 | 107 | MAVLinkISBD isbd; 108 | std::atomic running; 109 | // Thread of send_receive_task 110 | std::thread send_receive_thread; 111 | // Queue that buffers messages to be sent to the socket 112 | CircularBuffer send_queue; 113 | // Queue that buffers messages received from the socket 114 | CircularBuffer receive_queue; 115 | std::chrono::milliseconds send_time; // Last send epoch time 116 | std::chrono::milliseconds receive_time; // Last receive epoch time 117 | std::atomic signal_quality; 118 | }; 119 | 120 | } // namespace mavio 121 | 122 | #endif // LIBS_MAVIO_INCLUDE_MAVLINKISBDCHANNEL_H_ 123 | -------------------------------------------------------------------------------- /libs/mavio/include/MAVLinkLib.h: -------------------------------------------------------------------------------- 1 | /* 2 | MAVLinkLib.h 3 | 4 | MAVIO MAVLink I/O library. 5 | 6 | (C) Copyright 2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | 23 | #ifndef LIBS_MAVIO_INCLUDE_MAVLINKLIB_H_ 24 | #define LIBS_MAVIO_INCLUDE_MAVLINKLIB_H_ 25 | 26 | // MAVLink profile used by RadioRoom 27 | 28 | #include "ardupilotmega/mavlink.h" 29 | 30 | #endif // LIBS_MAVIO_INCLUDE_MAVLINKLIB_H_ 31 | -------------------------------------------------------------------------------- /libs/mavio/include/MAVLinkLogger.h: -------------------------------------------------------------------------------- 1 | /* 2 | MAVLinkLogger.h 3 | 4 | MAVIO MAVLink I/O library. 5 | 6 | (C) Copyright 2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | 23 | #ifndef LIBS_MAVIO_INCLUDE_MAVLINKLOGGER_H_ 24 | #define LIBS_MAVIO_INCLUDE_MAVLINKLOGGER_H_ 25 | 26 | #include "Logger.h" 27 | #include "MAVLinkLib.h" 28 | 29 | namespace mavio { 30 | 31 | /** 32 | * Class MAVLinkLogger provides static methos for logging MAVLink messages to 33 | * syslog. 34 | */ 35 | class MAVLinkLogger { 36 | public: 37 | /** 38 | * Logs MAVLink message to syslog at the specified priority with the 39 | * specified text prefix. 40 | * 41 | * Depending on inportance and frequency of the message, 'priority' 42 | * parameter should be set to LOG_ERR, LOG_NOTICE, LOG_INFO, or LOG_DEBUG. 43 | * 44 | * Example prefixes: 45 | * "MAV >>" - message received form the autopilot. 46 | * "MAV <<" - message sent to the autopilot. 47 | * "SBD >>" - message received from SBD transceiver 48 | * "SBD <<" - message sent to the SBD transceiver 49 | */ 50 | static void log(int priority, const char* prefix, 51 | const mavlink_message_t& message); 52 | }; 53 | 54 | } // namespace mavio 55 | 56 | #endif // LIBS_MAVIO_INCLUDE_MAVLINKLOGGER_H_ 57 | -------------------------------------------------------------------------------- /libs/mavio/include/MAVLinkSerial.h: -------------------------------------------------------------------------------- 1 | /* 2 | MAVLinkSerial.h 3 | 4 | MAVIO MAVLink I/O library. 5 | 6 | (C) Copyright 2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | 23 | #ifndef LIBS_MAVIO_INCLUDE_MAVLINKSERIAL_H_ 24 | #define LIBS_MAVIO_INCLUDE_MAVLINKSERIAL_H_ 25 | 26 | #include 27 | #include "MAVLinkLib.h" 28 | #include "Serial.h" 29 | 30 | namespace mavio { 31 | 32 | /** 33 | * MAVLinkSerial is used to send and receive MAVLink messages to/from a serial 34 | * interface. 35 | */ 36 | class MAVLinkSerial { 37 | public: 38 | /** 39 | * Constructs MAVLinkSerial instance using the specified serial interface. 40 | */ 41 | MAVLinkSerial(); 42 | 43 | /** 44 | * Initialize connection to the serial device with the specified baud rate. 45 | * 46 | * Returns true if serial device connection succeeded. 47 | */ 48 | bool init(const std::string& path, int speed); 49 | 50 | /** 51 | * Closes connection to the serial device. 52 | */ 53 | void close(); 54 | 55 | /** 56 | * Returns the path of serial device set by init(...) call. 57 | */ 58 | inline std::string get_path() const { return serial.get_path(); } 59 | 60 | /** 61 | * Sends MAVLink message to the serial interface. 62 | * 63 | * Returns true on success. 64 | */ 65 | bool send_message(const mavlink_message_t& msg); 66 | 67 | /** 68 | * Receives MAVLink message from the serial interface. 69 | * 70 | * Returns true if MAVLink message was received. 71 | */ 72 | bool receive_message(mavlink_message_t& msg); 73 | 74 | private: 75 | Serial serial; 76 | }; 77 | 78 | } // namespace mavio 79 | 80 | #endif // LIBS_MAVIO_INCLUDE_MAVLINKSERIAL_H_ 81 | -------------------------------------------------------------------------------- /libs/mavio/include/MAVLinkTCP.h: -------------------------------------------------------------------------------- 1 | /* 2 | MAVLinkTCP.h 3 | 4 | MAVIO MAVLink I/O library. 5 | 6 | (C) Copyright 2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | 23 | #ifndef LIBS_MAVIO_INCLUDE_MAVLINKTCP_H_ 24 | #define LIBS_MAVIO_INCLUDE_MAVLINKTCP_H_ 25 | 26 | #include "MAVLinkLib.h" 27 | #include 28 | #include 29 | 30 | namespace mavio { 31 | 32 | /** 33 | * Sends/receives MAVLink messages to/from a TCP/IP socket. 34 | */ 35 | class MAVLinkTCP { 36 | public: 37 | /** 38 | * Constructs an instance of MAVLinkTcpClient. 39 | */ 40 | MAVLinkTCP(); 41 | 42 | /** 43 | * Closes connection and frees the resources. 44 | */ 45 | virtual ~MAVLinkTCP(); 46 | 47 | /** 48 | * Connects to the TCP/IP socket at the specified address and port. 49 | * 50 | * Returns true if the connection was successful. 51 | */ 52 | bool init(const std::string address, uint16_t port); 53 | 54 | /** 55 | * Closes the connection if it was open. 56 | */ 57 | void close(); 58 | 59 | /** 60 | * Sends the specified MAVLink message to the socket. 61 | * 62 | * Returns true if the message was sent successfully. 63 | */ 64 | bool send_message(const mavlink_message_t& msg); 65 | 66 | /** 67 | * Receives MAVLink message from the socket. 68 | * 69 | * Returns true if a message was received. 70 | */ 71 | bool receive_message(mavlink_message_t& msg); 72 | 73 | private: 74 | /** 75 | * Connects to the TCP/IP socket specified by the init() call. 76 | */ 77 | bool connect(); 78 | 79 | struct sockaddr_in serv_addr; // Socket address 80 | int socket_fd; // Socket file descriptor 81 | }; 82 | 83 | } // namespace mavio 84 | 85 | #endif // LIBS_MAVIO_INCLUDE_MAVLINKTCP_H_ 86 | -------------------------------------------------------------------------------- /libs/mavio/include/MAVLinkTCPChannel.h: -------------------------------------------------------------------------------- 1 | /* 2 | MAVLinkTCPChannel.h 3 | 4 | MAVIO MAVLink I/O library. 5 | 6 | (C) Copyright 2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | 23 | #ifndef LIBS_MAVIO_INCLUDE_MAVLINKTCPCHANNEL_H_ 24 | #define LIBS_MAVIO_INCLUDE_MAVLINKTCPCHANNEL_H_ 25 | 26 | #include "CircularBuffer.h" 27 | #include "MAVLinkChannel.h" 28 | #include "MAVLinkLib.h" 29 | #include "MAVLinkTCP.h" 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | namespace mavio { 37 | 38 | /** 39 | * Asyncronous sends/receives MAVLink messages to/from a TCP/IP socket. 40 | */ 41 | class MAVLinkTCPChannel : public MAVLinkChannel { 42 | public: 43 | /** 44 | * Constructs an instance of MAVLinkTcpClient. 45 | */ 46 | MAVLinkTCPChannel(); 47 | 48 | /** 49 | * Closes connection and frees the resources. 50 | */ 51 | virtual ~MAVLinkTCPChannel(); 52 | 53 | /** 54 | * Connects to the TCP/IP socket at the specified address and port. 55 | * 56 | * Returns true if the connection was successful. 57 | */ 58 | bool init(const std::string address, uint16_t port); 59 | 60 | /** 61 | * Closes the connection if it was open. 62 | */ 63 | void close(); 64 | 65 | /** 66 | * Sends the specified MAVLink message to the socket. 67 | * 68 | * Returns true if the message was sent successfully. 69 | */ 70 | bool send_message(const mavlink_message_t& msg); 71 | 72 | /** 73 | * Receives MAVLink message from the socket. 74 | * 75 | * Returns true if a message was received. 76 | */ 77 | bool receive_message(mavlink_message_t& msg); 78 | 79 | /** 80 | * Checks if data is available in the socket input buffer. 81 | * 82 | * Returns true if data is available. 83 | */ 84 | bool message_available(); 85 | 86 | /** 87 | * Returns time of the last successfully sent message. 88 | */ 89 | std::chrono::milliseconds last_send_time(); 90 | 91 | /** 92 | * Returns time of the last successfully received message. 93 | */ 94 | std::chrono::milliseconds last_receive_time(); 95 | 96 | private: 97 | /** 98 | * While running is true, retrieves messages from send_queue and sends them to 99 | * serial. 100 | */ 101 | void send_task(); 102 | 103 | /** 104 | * While running is true, receives messages from serial and pushes them to 105 | * receive_queue. 106 | */ 107 | void receive_task(); 108 | 109 | // send and receive threads are running while this flag is true 110 | std::atomic running; 111 | // Thread of send_task 112 | std::thread send_thread; 113 | // Thread of receive_task 114 | std::thread receive_thread; 115 | // MAVLink TCP socket connection 116 | MAVLinkTCP socket; 117 | // Queue that buffers messages to be sent to the socket 118 | CircularBuffer send_queue; 119 | // Queue that buffers messages received from the socket 120 | CircularBuffer receive_queue; 121 | std::chrono::milliseconds send_time; // Last send epoch time 122 | std::chrono::milliseconds receive_time; // Last receive epoch time 123 | }; 124 | 125 | } // namespace mavio 126 | 127 | #endif // LIBS_MAVIO_INCLUDE_MAVLINKTCPCHANNEL_H_ 128 | -------------------------------------------------------------------------------- /libs/mavio/include/Serial.h: -------------------------------------------------------------------------------- 1 | /* 2 | Serial.h 3 | 4 | MAVIO MAVLink I/O library. 5 | 6 | (C) Copyright 2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | 23 | #ifndef LIBS_MAVIO_INCLUDE_SERIAL_H_ 24 | #define LIBS_MAVIO_INCLUDE_SERIAL_H_ 25 | 26 | #include 27 | 28 | #include 29 | #include 30 | 31 | namespace mavio { 32 | 33 | /** 34 | * Provides access to serial devices. 35 | */ 36 | class Serial { 37 | public: 38 | /** 39 | * Default constructor. 40 | */ 41 | Serial(); 42 | 43 | /** 44 | * Destructor. 45 | */ 46 | virtual ~Serial(); 47 | 48 | /** 49 | * Returns the serial device path set by open(...) call. 50 | */ 51 | inline std::string get_path() const { return path; } 52 | 53 | /** 54 | * Opens serial device with the specified path and sets the baud rate. 55 | * 56 | * Returns 0 in case of success or -1 in case of failure. 57 | */ 58 | int open(const std::string& path, int baud_rate); 59 | 60 | /** 61 | * Closes the serial device. 62 | */ 63 | int close(); 64 | 65 | /* 66 | * Reads single byte from the serial device. 67 | * 68 | * Returns the character read, or -1 if none is available. 69 | */ 70 | int read(); 71 | 72 | /** 73 | * Reads up to size bytes from the serial device, storing the results in the 74 | * buffer. 75 | * 76 | * Returns the number of bytes read or -1 in case of error. 77 | */ 78 | int read(void* buffer, size_t size); 79 | 80 | /** 81 | * Writes single character to the serial device. 82 | * 83 | * Returns 1 in case of success or -1 in case of error. 84 | */ 85 | int write(int c); 86 | 87 | /** 88 | * Writes the specified number of bytes from the specified buffer to the 89 | * serial device. 90 | * 91 | * Returns the number of bytes written or -1 in case of error. 92 | */ 93 | int write(const void* buffer, size_t n); 94 | 95 | /** 96 | * Returns the last I/O error code (errno). 97 | */ 98 | int get_last_error(); 99 | 100 | /** 101 | * Retrieves the list of serial devices from '/dev/serial/by-path' folder. 102 | * If 'dev/serial/by-path' folder is not available, the method uses static 103 | * list of standard serial devices. 104 | * 105 | * Full paths of the devices are added to 'devices' vector. 106 | * 107 | * Returns the number of devices found or -1 in case of an error. 108 | */ 109 | static int get_serial_devices(std::vector& devices); 110 | 111 | private: 112 | int tty_fd; 113 | termios old_tio; 114 | std::string path; 115 | int last_error; 116 | }; 117 | 118 | } // namespace mavio 119 | 120 | #endif // LIBS_MAVIO_INCLUDE_SERIAL_H_ 121 | -------------------------------------------------------------------------------- /libs/mavio/src/Logger.cc: -------------------------------------------------------------------------------- 1 | /* 2 | Logger.h 3 | 4 | MAVIO MAVLink I/O library. 5 | 6 | (C) Copyright 2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | 23 | #include "Logger.h" 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | #include "timelib.h" 33 | 34 | namespace mavio { 35 | 36 | std::atomic_int log_mask(LOG_UPTO(LOG_INFO)); 37 | std::atomic_bool log_to_stdout(true); 38 | 39 | const char* priority2str(int priority) { 40 | switch (priority) { 41 | case LOG_ALERT: 42 | return "ALERT"; 43 | case LOG_CRIT: 44 | return "CRIT"; 45 | case LOG_DEBUG: 46 | return "DEBUG"; 47 | case LOG_EMERG: 48 | return "EMERG"; 49 | case LOG_ERR: 50 | return "ERR"; 51 | case LOG_INFO: 52 | return "INFO"; 53 | case LOG_NOTICE: 54 | return "NOTICE"; 55 | case LOG_WARNING: 56 | return "WARNING"; 57 | default: 58 | return "UNDEFINED"; 59 | } 60 | } 61 | 62 | void openlog(const char* identity, int mask) { 63 | #ifdef __CYGWIN__ 64 | printf("Opened log '%s'", identity); 65 | log_mask = mask; 66 | #else 67 | ::openlog(identity, LOG_CONS | LOG_NDELAY, LOG_USER); 68 | ::setlogmask(mask); 69 | log_to_stdout = false; 70 | #endif 71 | } 72 | 73 | void closelog() { 74 | #ifndef __CYGWIN__ 75 | ::closelog(); 76 | log_to_stdout = true; 77 | #endif 78 | } 79 | 80 | void vlog(int priority, const char* format, va_list args) { 81 | #ifdef __CYGWIN__ 82 | if (log_mask & priority) { 83 | char str[64]; 84 | timelib::timestamp(str, sizeof(str)); 85 | printf("%s %s ", str, priority2str(priority)); 86 | vprintf(format, args); 87 | printf("\n"); 88 | } 89 | #else 90 | if (log_to_stdout) { 91 | if (log_mask & priority) { 92 | char str[64]; 93 | timelib::timestamp(str, sizeof(str)); 94 | printf("%s %s ", str, priority2str(priority)); 95 | vprintf(format, args); 96 | printf("\n"); 97 | } 98 | } else { 99 | ::vsyslog(priority, format, args); 100 | } 101 | #endif 102 | } 103 | 104 | void log(int priority, const char* format, ...) { 105 | va_list args; 106 | va_start(args, format); 107 | mavio::vlog(priority, format, args); 108 | va_end(args); 109 | } 110 | 111 | } // namespace mavio 112 | -------------------------------------------------------------------------------- /libs/mavio/src/MAVLinkAutopilot.cc: -------------------------------------------------------------------------------- 1 | /* 2 | MAVLinkAutopilot.cc 3 | 4 | (C) Copyright 2019 Envirover. 5 | 6 | This library is free software; you can redistribute it and/or 7 | modify it under the terms of the GNU Lesser General Public 8 | License as published by the Free Software Foundation; either 9 | version 2.1 of the License, or (at your option) any later version. 10 | 11 | This library is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public 17 | License along with this library; if not, write to the Free Software 18 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 19 | */ 20 | 21 | #include "MAVLinkAutopilot.h" 22 | 23 | #include "timelib.h" 24 | 25 | #include "MAVLinkLogger.h" 26 | 27 | namespace mavio { 28 | 29 | using std::string; 30 | using std::vector; 31 | using timelib::sleep; 32 | using timelib::Stopwatch; 33 | 34 | constexpr int send_retries = 5; 35 | constexpr int receive_retries = 10; 36 | 37 | const std::chrono::milliseconds max_heartbeat_interval(2000); 38 | const std::chrono::milliseconds autopilot_send_interval(10); 39 | const std::chrono::milliseconds receive_retry_delay(10); 40 | 41 | constexpr size_t max_autopilot_queue_size = 1024; 42 | 43 | MAVLinkAutopilot::MAVLinkAutopilot() 44 | : MAVLinkChannel(autopilot_channel_id), 45 | running(false), 46 | send_thread(), 47 | receive_thread(), 48 | serial(), 49 | send_queue(max_autopilot_queue_size), 50 | receive_queue(max_autopilot_queue_size), 51 | system_id(0), 52 | send_time(0), 53 | receive_time(0) {} 54 | 55 | bool MAVLinkAutopilot::init(const string& path, int speed, 56 | const vector& devices) { 57 | bool ret = connect(path, speed, devices); 58 | 59 | if (!running) { 60 | running = true; 61 | 62 | std::thread send_th(&MAVLinkAutopilot::send_task, this); 63 | send_thread.swap(send_th); 64 | 65 | std::thread receive_th(&MAVLinkAutopilot::receive_task, this); 66 | receive_thread.swap(receive_th); 67 | } 68 | 69 | return ret; 70 | } 71 | 72 | void MAVLinkAutopilot::close() { 73 | if (running) { 74 | running = false; 75 | 76 | receive_thread.join(); 77 | send_thread.join(); 78 | } 79 | 80 | serial.close(); 81 | } 82 | 83 | bool MAVLinkAutopilot::request_autopilot_version( 84 | uint8_t& autopilot, uint8_t& mav_type, uint8_t& sys_id, 85 | mavlink_autopilot_version_t& autopilot_version) { 86 | mavlink_message_t msg, msg_command_long; 87 | autopilot = mav_type = sys_id = 0; 88 | memset(&autopilot_version, 0, sizeof(autopilot_version)); 89 | 90 | Stopwatch timer; 91 | 92 | while (timer.elapsed_time() < max_heartbeat_interval) { 93 | if (serial.receive_message(msg)) { 94 | if (msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) { 95 | autopilot = mavlink_msg_heartbeat_get_autopilot(&msg); 96 | mav_type = mavlink_msg_heartbeat_get_type(&msg); 97 | sys_id = msg.sysid; 98 | 99 | if (autopilot != MAV_AUTOPILOT_INVALID) // Filter out heartbeat 100 | // messages forwarded from GCS 101 | break; 102 | } 103 | } 104 | 105 | sleep(receive_retry_delay); 106 | } 107 | 108 | // Return false if heartbeat message was not received 109 | if (sys_id == 0) { 110 | mavio::log(LOG_DEBUG, "Heartbeat not received.\n"); 111 | return false; 112 | } 113 | 114 | for (int i = 0; i < send_retries; i++) { 115 | mavlink_msg_command_long_pack( 116 | gcs_system_id, gcs_component_id, &msg_command_long, sys_id, 117 | ardupilot_component_id, MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES, i, 1.0, 118 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); 119 | if (serial.send_message(msg_command_long)) { 120 | for (int j = 0; j < receive_retries; j++) { 121 | if (serial.receive_message(msg)) { 122 | // printf("**** msg.msgid = %d\n", msg.msgid); 123 | if (msg.msgid == MAVLINK_MSG_ID_AUTOPILOT_VERSION) { 124 | mavlink_msg_autopilot_version_decode(&msg, &autopilot_version); 125 | sys_id = msg.sysid; 126 | return true; 127 | } 128 | } 129 | } 130 | } else { 131 | mavio::log(LOG_DEBUG, "Failed to send message to autopilot.\n"); 132 | } 133 | 134 | sleep(receive_retry_delay); 135 | } 136 | 137 | return true; 138 | } 139 | 140 | char* MAVLinkAutopilot::get_firmware_version( 141 | const mavlink_autopilot_version_t& autopilot_version, char* buff, 142 | size_t buff_size) { 143 | strncpy(buff, "unknown", buff_size); 144 | 145 | if (autopilot_version.flight_sw_version != 0) { 146 | int majorVersion, minorVersion, patchVersion; 147 | FIRMWARE_VERSION_TYPE versionType; 148 | 149 | majorVersion = (autopilot_version.flight_sw_version >> (8 * 3)) & 0xFF; 150 | minorVersion = (autopilot_version.flight_sw_version >> (8 * 2)) & 0xFF; 151 | patchVersion = (autopilot_version.flight_sw_version >> (8 * 1)) & 0xFF; 152 | versionType = (FIRMWARE_VERSION_TYPE)( 153 | (autopilot_version.flight_sw_version >> (8 * 0)) & 0xFF); 154 | 155 | snprintf(buff, buff_size, "%d.%d.%d/%d ", majorVersion, minorVersion, 156 | patchVersion, versionType); 157 | } 158 | 159 | return buff; 160 | } 161 | 162 | bool MAVLinkAutopilot::send_message(const mavlink_message_t& msg) { 163 | send_queue.push(msg); 164 | return true; 165 | } 166 | 167 | bool MAVLinkAutopilot::receive_message(mavlink_message_t& msg) { 168 | return receive_queue.pop(msg); 169 | } 170 | 171 | bool MAVLinkAutopilot::message_available() { return !receive_queue.empty(); } 172 | 173 | std::chrono::milliseconds MAVLinkAutopilot::last_send_time() { 174 | return send_time; 175 | } 176 | 177 | std::chrono::milliseconds MAVLinkAutopilot::last_receive_time() { 178 | return receive_time; 179 | } 180 | 181 | bool MAVLinkAutopilot::connect(const string& path, int speed, 182 | const vector& devices) { 183 | mavio::log(LOG_NOTICE, "Connecting to autopilot (%s %d)...", path.data(), 184 | speed); 185 | 186 | system_id = 0; 187 | 188 | if (serial.init(path, speed)) { 189 | system_id = detect_autopilot(path); 190 | 191 | if (system_id) { 192 | return true; 193 | } 194 | 195 | serial.close(); 196 | } else { 197 | mavio::log(LOG_WARNING, "Failed to open serial device '%s'.", path.data()); 198 | } 199 | 200 | if (devices.size() > 0) { 201 | mavio::log(LOG_NOTICE, 202 | "Attempting to detect autopilot at the serial devices..."); 203 | 204 | for (const std::string device : devices) { 205 | if (device == path) 206 | continue; 207 | 208 | if (serial.init(device, speed)) { 209 | system_id = detect_autopilot(device); 210 | 211 | if (system_id) 212 | return true; 213 | 214 | mavio::log(LOG_NOTICE, "Autopilot not detected at serial device '%s'.", 215 | device.data()); 216 | 217 | serial.close(); 218 | } else { 219 | mavio::log(LOG_NOTICE, "Failed to open serial device '%s'.", 220 | device.data()); 221 | } 222 | } 223 | 224 | mavio::log(LOG_ERR, 225 | "Autopilot was not detected on any of the serial devices."); 226 | } 227 | 228 | return false; 229 | } 230 | 231 | uint8_t MAVLinkAutopilot::detect_autopilot(const string device) { 232 | mavlink_autopilot_version_t autopilot_version; 233 | uint8_t autopilot, mav_type, sys_id; 234 | 235 | mavio::log(LOG_NOTICE, "Detecting autopilot at serial device '%s'...", 236 | device.data()); 237 | 238 | if (!request_autopilot_version(autopilot, mav_type, sys_id, 239 | autopilot_version)) { 240 | mavio::log(LOG_DEBUG, "Autopilot not detected at serial device '%s'.", 241 | device.data()); 242 | return 0; 243 | } 244 | 245 | char buff[64]; 246 | get_firmware_version(autopilot_version, buff, sizeof(buff)); 247 | 248 | mavio::log(LOG_NOTICE, "Autopilot detected at serial device '%s'.", 249 | device.data()); 250 | mavio::log( 251 | LOG_NOTICE, 252 | "MAV type: %d, system id: %d, autopilot class: %d, firmware version: %s", 253 | mav_type, sys_id, autopilot, buff); 254 | 255 | return sys_id; 256 | } 257 | 258 | void MAVLinkAutopilot::send_task() { 259 | while (running) { 260 | mavlink_message_t msg; 261 | 262 | if (send_queue.pop(msg)) { 263 | if (serial.send_message(msg)) { 264 | send_time = timelib::time_since_epoch(); 265 | } 266 | } 267 | 268 | timelib::sleep(autopilot_send_interval); 269 | } 270 | } 271 | 272 | void MAVLinkAutopilot::receive_task() { 273 | while (running) { 274 | mavlink_message_t msg; 275 | 276 | if (serial.receive_message(msg)) { 277 | receive_time = timelib::time_since_epoch(); 278 | receive_queue.push(msg); 279 | } 280 | } 281 | } 282 | 283 | } // namespace mavio 284 | -------------------------------------------------------------------------------- /libs/mavio/src/MAVLinkISBD.cc: -------------------------------------------------------------------------------- 1 | /* 2 | MAVLinkSBD.cc 3 | 4 | MAVIO MAVLink I/O library. 5 | 6 | (C) Copyright 2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | 23 | #include "MAVLinkISBD.h" 24 | 25 | #include 26 | 27 | #include "MAVLinkLogger.h" 28 | 29 | namespace mavio { 30 | 31 | using std::string; 32 | using std::vector; 33 | 34 | // Custom serialization/deerialization helpers 35 | uint16_t message_to_send_buffer(uint8_t *buf, const mavlink_message_t *msg); 36 | bool parse_message(const uint8_t *buf, size_t buf_size, mavlink_message_t *msg); 37 | 38 | MAVLinkISBD::MAVLinkISBD() : stream(), isbd(stream) {} 39 | 40 | MAVLinkISBD::~MAVLinkISBD() {} 41 | 42 | bool MAVLinkISBD::get_ring_alert_flag(uint16_t& ra_flag) { 43 | uint16_t mo_flag = 0, mo_msn = 0, mt_flag = 0, mt_msn = 0, msg_waiting = 0; 44 | 45 | ra_flag = 0; 46 | 47 | int err = isbd.getStatusExtended(mo_flag, mo_msn, mt_flag, mt_msn, ra_flag, 48 | msg_waiting); 49 | 50 | if (err != ISBD_SUCCESS) { 51 | mavio::log(LOG_WARNING, "Failed to get ISBD status. Error = %d", err); 52 | } else if (ra_flag) { 53 | mavio::log(LOG_INFO, "Ring alert received."); 54 | } 55 | 56 | return err == ISBD_SUCCESS; 57 | } 58 | 59 | int MAVLinkISBD::get_waiting_wessage_count() { 60 | return isbd.getWaitingMessageCount(); 61 | } 62 | 63 | bool MAVLinkISBD::detect_transceiver(string device) { 64 | int ret = isbd.begin(); 65 | 66 | if (ret == ISBD_SUCCESS || ret == ISBD_ALREADY_AWAKE) { 67 | char model[256], imea[256]; 68 | imea[0] = model[0] = 0; 69 | ret = isbd.getTransceiverModel(model, sizeof(model)); 70 | if (ret == ISBD_SUCCESS) { 71 | ret = isbd.getTransceiverSerialNumber(imea, sizeof(imea)); 72 | if (ret == ISBD_SUCCESS) { 73 | mavio::log(LOG_NOTICE, "%s (IMEA %s) detected at serial device '%s'.", 74 | model, imea, device.data()); 75 | return true; 76 | } 77 | } 78 | } 79 | 80 | mavio::log( 81 | LOG_DEBUG, 82 | "ISBD transceiver not detected at serial device '%s'. Error code = %d.", 83 | device.data(), ret); 84 | return false; 85 | } 86 | 87 | bool MAVLinkISBD::get_signal_quality(int& quality) { 88 | return isbd.getSignalQuality(quality) == ISBD_SUCCESS; 89 | } 90 | 91 | bool MAVLinkISBD::init(string path, int speed, const vector& devices) { 92 | mavio::log(LOG_INFO, "Connecting to ISBD transceiver (%s %d)...", path.data(), 93 | speed); 94 | 95 | isbd.setPowerProfile(1); 96 | 97 | if (stream.open(path, speed) == 0) { 98 | if (detect_transceiver(path)) { 99 | return true; 100 | } 101 | 102 | stream.close(); 103 | } else { 104 | mavio::log(LOG_INFO, "Failed to open serial device '%s'.", path.data()); 105 | } 106 | 107 | if (devices.size() > 0) { 108 | mavio::log(LOG_INFO, 109 | "Attempting to detect ISBD transceiver at the available serial " 110 | "devices..."); 111 | 112 | for (size_t i = 0; i < devices.size(); i++) { 113 | if (devices[i] == path) continue; 114 | 115 | if (stream.open(devices[i].data(), speed) == 0) { 116 | if (detect_transceiver(devices[i])) { 117 | return true; 118 | } else { 119 | stream.close(); 120 | } 121 | } else { 122 | mavio::log(LOG_DEBUG, "Failed to open serial device '%s'.", 123 | devices[i].data()); 124 | } 125 | } 126 | } 127 | 128 | stream.open(path, speed); 129 | mavio::log(LOG_ERR, 130 | "ISBD transceiver was not detected on any of the serial devices."); 131 | 132 | return false; 133 | } 134 | 135 | void MAVLinkISBD::close() { 136 | stream.close(); 137 | mavio::log(LOG_DEBUG, "ISBD connection closed."); 138 | } 139 | 140 | /** 141 | * Checks if data is available in ISBD. 142 | * 143 | * Returns true if data is available. 144 | */ 145 | bool MAVLinkISBD::message_available() { 146 | if (isbd.getWaitingMessageCount() > 0) { 147 | return true; 148 | } 149 | 150 | uint16_t ra_flag = 0; 151 | 152 | get_ring_alert_flag(ra_flag); 153 | 154 | return ra_flag != 0; 155 | } 156 | 157 | bool MAVLinkISBD::send_receive_message(const mavlink_message_t& mo_msg, 158 | mavlink_message_t& mt_msg, 159 | bool& received) { 160 | uint8_t buf[ISBD_MAX_MT_MGS_SIZE]; 161 | size_t buf_size = sizeof(buf); 162 | uint16_t len = 0; 163 | 164 | if (mo_msg.len != 0 && mo_msg.msgid != 0) { 165 | len = message_to_send_buffer(buf, &mo_msg); 166 | } 167 | 168 | received = false; 169 | 170 | int ret = isbd.sendReceiveSBDBinary(buf, len, buf, buf_size); 171 | 172 | if (ret != ISBD_SUCCESS) { 173 | if (mo_msg.len != 0 && mo_msg.msgid != 0) { 174 | char prefix[32]; 175 | snprintf(prefix, sizeof(prefix), "SBD << FAILED(%d)", ret); 176 | MAVLinkLogger::log(LOG_WARNING, prefix, mo_msg); 177 | } else { 178 | mavio::log(LOG_WARNING, "SBD >> FAILED(%d)", 179 | ret); // Failed to receive MT message from ISBD 180 | } 181 | 182 | return false; 183 | } 184 | 185 | if (buf_size > 0) { 186 | if (parse_message(buf, buf_size, &mt_msg)) { 187 | MAVLinkLogger::log(LOG_INFO, "SBD >>", mt_msg); 188 | } else { 189 | mavio::log(LOG_WARNING, 190 | "Failed to parse MAVLink message received from ISBD."); 191 | } 192 | } 193 | 194 | MAVLinkLogger::log(LOG_INFO, "SBD <<", mo_msg); 195 | 196 | return true; 197 | } 198 | 199 | /** 200 | * Custom serialization of MAVLink messages. 201 | * 202 | * With standard mavlink_msg_to_send_buffer() function, HIGH_LATENCY2 takes 54 203 | * bytes, which requires 2 RockBLOCK credits. With the custom serialization 204 | * sending HIGH_LATENCY2 takes 50 bytes. 205 | * 206 | * For MAVLink 2.0 the serialization does not include compatibility, 207 | * incompatibility flags, and the checksum. 208 | */ 209 | uint16_t message_to_send_buffer(uint8_t* buf, const mavlink_message_t* msg) { 210 | uint8_t header_len; 211 | uint8_t length = msg->len; 212 | 213 | buf[0] = msg->magic; 214 | buf[1] = length; 215 | buf[2] = msg->seq; 216 | buf[3] = msg->sysid; 217 | buf[4] = msg->compid; 218 | 219 | if (msg->magic == MAVLINK_STX_MAVLINK1) { 220 | header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN; 221 | buf[5] = msg->msgid & 0xFF; 222 | memcpy(buf + 6, _MAV_PAYLOAD(msg), msg->len); 223 | uint8_t* ck = buf + header_len + 1 + (uint16_t)msg->len; 224 | ck[0] = (uint8_t)(msg->checksum & 0xFF); 225 | ck[1] = (uint8_t)(msg->checksum >> 8); 226 | return header_len + 1 + (uint16_t)length + 2; 227 | } else { // MAVLink 2.0 228 | length = _mav_trim_payload(_MAV_PAYLOAD(msg), length); 229 | header_len = MAVLINK_CORE_HEADER_LEN - 2; 230 | buf[5] = msg->msgid & 0xFF; 231 | buf[6] = (msg->msgid >> 8) & 0xFF; 232 | buf[7] = (msg->msgid >> 16) & 0xFF; 233 | memcpy(buf + 8, _MAV_PAYLOAD(msg), length); 234 | return header_len + 1 + (uint16_t)length; 235 | } 236 | } 237 | 238 | /* 239 | * Custom deserialization of MAVLink messages. 240 | */ 241 | bool parse_message(const uint8_t* buf, size_t buf_size, 242 | mavlink_message_t* msg) { 243 | if (buf == NULL || buf_size < 8 || msg == NULL) 244 | return false; 245 | 246 | const mavlink_msg_entry_t *msg_entry; 247 | 248 | msg->magic = buf[0]; 249 | msg->compat_flags = 0; 250 | msg->incompat_flags = 0; 251 | msg->len = buf[1]; 252 | msg->seq = buf[2]; 253 | msg->sysid = buf[3]; 254 | msg->compid = buf[4]; 255 | 256 | switch (msg->magic) { 257 | case MAVLINK_STX_MAVLINK1: 258 | 259 | msg->msgid = buf[5]; 260 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf + 6, msg->len); 261 | 262 | // Compute checksum 263 | crc_init(&(msg->checksum)); 264 | crc_accumulate_buffer(&(msg->checksum), (const char *)(buf + 1), 265 | msg->len + 5); 266 | 267 | msg_entry = mavlink_get_msg_entry(msg->msgid); 268 | 269 | if (msg_entry == NULL) { 270 | return false; 271 | } 272 | 273 | crc_accumulate(msg_entry->crc_extra, &(msg->checksum)); 274 | 275 | msg->ck[0] = (uint8_t)(msg->checksum & 0xFF); 276 | msg->ck[1] = (uint8_t)(msg->checksum >> 8); 277 | break; 278 | case MAVLINK_STX: 279 | msg->msgid = buf[5] + (buf[6] << 8) + (buf[7] << 16); 280 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf + 8, msg->len); 281 | 282 | // Compute checksum 283 | crc_init(&(msg->checksum)); 284 | crc_accumulate(buf[1], &(msg->checksum)); 285 | crc_accumulate(0, &(msg->checksum)); // Compat flags 286 | crc_accumulate(0, &(msg->checksum)); // Incompat flags 287 | crc_accumulate_buffer(&(msg->checksum), (const char *)(buf + 2), 288 | buf_size - 2); 289 | 290 | msg_entry = mavlink_get_msg_entry(msg->msgid); 291 | 292 | if (msg_entry == NULL) { 293 | return false; 294 | } 295 | 296 | crc_accumulate(msg_entry->crc_extra, &(msg->checksum)); 297 | break; 298 | default: 299 | return false; 300 | } 301 | 302 | return true; 303 | } 304 | 305 | } // namespace mavio 306 | -------------------------------------------------------------------------------- /libs/mavio/src/MAVLinkISBDChannel.cc: -------------------------------------------------------------------------------- 1 | /* 2 | MAVLinkISBDChannel.cc 3 | 4 | MAVIO MAVLink I/O library. 5 | 6 | (C) Copyright 2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | 23 | #include "MAVLinkISBDChannel.h" 24 | 25 | #include "timelib.h" 26 | 27 | namespace mavio { 28 | 29 | using std::string; 30 | using std::vector; 31 | using timelib::sleep; 32 | 33 | constexpr size_t max_isbd_channel_queue_size = 1024; 34 | 35 | const std::chrono::milliseconds isbd_channel_poll_interval(10); 36 | 37 | MAVLinkISBDChannel::MAVLinkISBDChannel() 38 | : MAVLinkChannel(isbd_channel_id), 39 | isbd(), 40 | running(false), 41 | send_receive_thread(), 42 | send_queue(max_isbd_channel_queue_size), 43 | receive_queue(max_isbd_channel_queue_size), 44 | send_time(0), 45 | receive_time(0), 46 | signal_quality(0) {} 47 | 48 | MAVLinkISBDChannel::~MAVLinkISBDChannel() {} 49 | 50 | bool MAVLinkISBDChannel::init(std::string path, int speed, 51 | const vector& devices) { 52 | bool ret = isbd.init(path, speed, devices); 53 | 54 | if (!running) { 55 | running = true; 56 | 57 | std::thread send_receive_th(&MAVLinkISBDChannel::send_receive_task, this); 58 | send_receive_thread.swap(send_receive_th); 59 | } 60 | 61 | return ret; 62 | } 63 | 64 | void MAVLinkISBDChannel::close() { 65 | if (running) { 66 | running = false; 67 | 68 | send_receive_thread.join(); 69 | } 70 | 71 | isbd.close(); 72 | } 73 | 74 | bool MAVLinkISBDChannel::send_message(const mavlink_message_t& msg) { 75 | if (msg.len == 0 && msg.msgid == 0) { 76 | return true; 77 | } 78 | 79 | send_queue.push(msg); 80 | 81 | return true; 82 | } 83 | 84 | bool MAVLinkISBDChannel::receive_message(mavlink_message_t& msg) { 85 | return receive_queue.pop(msg); 86 | } 87 | 88 | bool MAVLinkISBDChannel::message_available() { return !receive_queue.empty(); } 89 | 90 | std::chrono::milliseconds MAVLinkISBDChannel::last_send_time() { 91 | return send_time; 92 | } 93 | 94 | std::chrono::milliseconds MAVLinkISBDChannel::last_receive_time() { 95 | return receive_time; 96 | } 97 | 98 | bool MAVLinkISBDChannel::get_signal_quality(int& quality) { 99 | quality = signal_quality; 100 | return true; 101 | } 102 | 103 | /** 104 | * If there are messages in send_queue or ring alert flag of ISBD transceiver 105 | * is up, pop send_queue, run send-receive session, and push received messages 106 | * to receive_queue. 107 | */ 108 | void MAVLinkISBDChannel::send_receive_task() { 109 | while (running) { 110 | int quality = 0; 111 | if (isbd.get_signal_quality(quality)) { 112 | signal_quality = quality; 113 | } else { 114 | signal_quality = 0; 115 | } 116 | 117 | if (!send_queue.empty() || isbd.message_available()) { 118 | mavlink_message_t mo_msg, mt_msg; 119 | if (!send_queue.pop(mo_msg)) { 120 | mo_msg.len = 0; 121 | mo_msg.msgid = 0; 122 | } 123 | 124 | bool received = false; 125 | if (isbd.send_receive_message(mo_msg, mt_msg, received)) { 126 | send_time = timelib::time_since_epoch(); 127 | if (received) { 128 | receive_time = send_time; 129 | receive_queue.push(mt_msg); 130 | } 131 | } 132 | } 133 | 134 | sleep(isbd_channel_poll_interval); 135 | } 136 | } 137 | 138 | } // namespace mavio 139 | -------------------------------------------------------------------------------- /libs/mavio/src/MAVLinkSerial.cc: -------------------------------------------------------------------------------- 1 | /* 2 | MAVLinkSerial.cc 3 | 4 | MAVIO MAVLink I/O library. 5 | 6 | (C) Copyright 2017-2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | 23 | #include "MAVLinkSerial.h" 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include "MAVLinkLogger.h" 30 | 31 | namespace mavio { 32 | 33 | using std::string; 34 | 35 | MAVLinkSerial::MAVLinkSerial() : serial() {} 36 | 37 | bool MAVLinkSerial::init(const string& path, int speed) { 38 | if (serial.open(path, speed) == 0) { 39 | return true; 40 | } 41 | 42 | // mavio::log(LOG_WARNING, "Failed to open serial device '%s'.", path.data()); 43 | 44 | return false; 45 | } 46 | 47 | void MAVLinkSerial::close() { serial.close(); } 48 | 49 | bool MAVLinkSerial::send_message(const mavlink_message_t& msg) { 50 | if (msg.len == 0 && msg.msgid == 0) { 51 | return true; 52 | } 53 | 54 | uint8_t buf[MAVLINK_MAX_PACKET_LEN]; 55 | 56 | // Copy the message to send buffer 57 | uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); 58 | 59 | uint16_t n = serial.write(buf, len); 60 | 61 | if (n == len) { 62 | MAVLinkLogger::log(msg.msgid == MAVLINK_MSG_ID_HEARTBEAT ? 63 | LOG_DEBUG : LOG_INFO, "MAV <<", msg); 64 | } else { 65 | char prefix[32]; 66 | snprintf(prefix, sizeof(prefix), "MAV << FAILED(%d)", 67 | serial.get_last_error()); 68 | MAVLinkLogger::log(LOG_WARNING, prefix, msg); 69 | } 70 | 71 | return n == len; 72 | } 73 | 74 | bool MAVLinkSerial::receive_message(mavlink_message_t& msg) { 75 | mavlink_status_t mavlink_status; 76 | 77 | int c = serial.read(); 78 | 79 | while (c >= 0) { 80 | // Serial.println(c); 81 | 82 | if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &mavlink_status)) { 83 | MAVLinkLogger::log(LOG_DEBUG, "MAV >>", msg); 84 | return true; 85 | } 86 | 87 | c = serial.read(); 88 | } 89 | 90 | return false; 91 | } 92 | 93 | } // namespace mavio 94 | -------------------------------------------------------------------------------- /libs/mavio/src/MAVLinkTCP.cc: -------------------------------------------------------------------------------- 1 | /* 2 | MAVLinkTCP.cc 3 | 4 | MAVIO MAVLink I/O library. 5 | 6 | (C) Copyright 2020 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | 23 | #include "MAVLinkTCP.h" 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include "MAVLinkLogger.h" 37 | 38 | namespace mavio { 39 | 40 | // TCP keepalive options values 41 | constexpr int so_keepalive_value = 1; // enabled 42 | 43 | constexpr int so_read_timeout = 1; // seconds 44 | 45 | MAVLinkTCP::MAVLinkTCP() : socket_fd(0) {} 46 | 47 | MAVLinkTCP::~MAVLinkTCP() { close(); } 48 | 49 | bool MAVLinkTCP::init(const std::string address, uint16_t port) { 50 | if (address.empty()) { 51 | return false; 52 | } 53 | 54 | struct hostent* server = ::gethostbyname(address.c_str()); 55 | 56 | if (server == NULL) { 57 | mavio::log(LOG_ERR, "No such host '%s'.", address.c_str()); 58 | return false; 59 | } 60 | 61 | memset(&serv_addr, 0, sizeof(serv_addr)); 62 | 63 | serv_addr.sin_family = AF_INET; 64 | 65 | memcpy(&serv_addr.sin_addr.s_addr, server->h_addr, server->h_length); 66 | 67 | serv_addr.sin_port = htons(port); 68 | 69 | return connect(); 70 | } 71 | 72 | bool MAVLinkTCP::connect() { 73 | if (socket_fd > 0) { 74 | ::close(socket_fd); 75 | } 76 | 77 | socket_fd = ::socket(AF_INET, SOCK_STREAM, 0); 78 | 79 | if (socket_fd < 0) { 80 | mavio::log(LOG_ERR, "Socket creation failed. %s", strerror(errno)); 81 | socket_fd = 0; 82 | return false; 83 | } 84 | 85 | if (::setsockopt(socket_fd, SOL_SOCKET, SO_KEEPALIVE, &so_keepalive_value, 86 | sizeof(so_keepalive_value))) { 87 | mavio::log(LOG_ERR, "Failed to enable TCP socket keepalive. %s", 88 | strerror(errno)); 89 | return false; 90 | } 91 | 92 | struct timeval so_timeout; 93 | so_timeout.tv_sec = so_read_timeout; 94 | so_timeout.tv_usec = 0; 95 | if (::setsockopt(socket_fd, SOL_SOCKET, SO_RCVTIMEO, &so_timeout, 96 | sizeof(so_timeout))) { 97 | mavio::log(LOG_ERR, "Failed to set TCP socket read timeout. %s", 98 | strerror(errno)); 99 | return false; 100 | } 101 | 102 | char sin_addr_str[INET_ADDRSTRLEN]; 103 | ::inet_ntop(AF_INET, &(serv_addr.sin_addr), sin_addr_str, INET_ADDRSTRLEN); 104 | 105 | if (::connect(socket_fd, reinterpret_cast(&serv_addr), 106 | sizeof(serv_addr)) < 0) { 107 | mavio::log(LOG_ERR, "Connection to 'tcp://%s:%d' failed. %s", sin_addr_str, 108 | ntohs(serv_addr.sin_port), strerror(errno)); 109 | return false; 110 | } 111 | 112 | mavio::log(LOG_NOTICE, "Connected to 'tcp://%s:%d'.", sin_addr_str, 113 | ntohs(serv_addr.sin_port)); 114 | 115 | return true; 116 | } 117 | 118 | void MAVLinkTCP::close() { 119 | if (socket_fd != 0) { 120 | ::close(socket_fd); 121 | socket_fd = 0; 122 | } 123 | } 124 | 125 | bool MAVLinkTCP::send_message(const mavlink_message_t& msg) { 126 | if (socket_fd == 0) { 127 | return false; 128 | } 129 | 130 | if (msg.len == 0 && msg.msgid == 0) { 131 | return true; 132 | } 133 | 134 | uint8_t buf[MAVLINK_MAX_PACKET_LEN]; 135 | 136 | // Copy the message to send buffer 137 | uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); 138 | 139 | uint16_t n = ::send(socket_fd, buf, len, 0); 140 | 141 | if (n == len) { 142 | MAVLinkLogger::log(LOG_INFO, "TCP <<", msg); 143 | return true; 144 | } 145 | 146 | mavio::log(LOG_ERR, "TCP socket send failed. %s", strerror(errno)); 147 | 148 | MAVLinkLogger::log(LOG_WARNING, "TCP << FAILED", msg); 149 | 150 | // Re-connect to the socket. 151 | connect(); 152 | 153 | return false; 154 | } 155 | 156 | bool MAVLinkTCP::receive_message(mavlink_message_t& msg) { 157 | if (socket_fd == 0) { 158 | return false; 159 | } 160 | 161 | ssize_t rc; 162 | uint8_t chr = 0; 163 | 164 | while ((rc = ::recv(socket_fd, &chr, 1, MSG_WAITALL)) > 0) { 165 | mavlink_status_t mavlink_status; 166 | 167 | if (mavlink_parse_char(MAVLINK_COMM_0, chr, &msg, &mavlink_status)) { 168 | MAVLinkLogger::log(LOG_INFO, "TCP >>", msg); 169 | return true; 170 | } 171 | } 172 | 173 | if (rc == 0) { 174 | mavio::log(LOG_INFO, "TCP >> FAILED (The socket peer has performed " 175 | "an orderly shutdown)"); 176 | } else { 177 | mavio::log(LOG_DEBUG, "TCP >> FAILED (Failed to receive message from " 178 | "the socket. %s)", strerror(errno)); 179 | } 180 | 181 | return false; 182 | } 183 | 184 | } // namespace mavio 185 | -------------------------------------------------------------------------------- /libs/mavio/src/MAVLinkTCPChannel.cc: -------------------------------------------------------------------------------- 1 | /* 2 | MAVLinkTCPChannel.cc 3 | 4 | MAVIO MAVLink I/O library. 5 | 6 | (C) Copyright 2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | 23 | #include "MAVLinkTCPChannel.h" 24 | 25 | #include "timelib.h" 26 | 27 | namespace mavio { 28 | 29 | using timelib::sleep; 30 | 31 | constexpr size_t max_tcp_channnel_queue_size = 1024; 32 | 33 | const std::chrono::milliseconds tcp_channel_send_interval(10); 34 | const std::chrono::milliseconds tcp_channel_receive_interval(10); 35 | 36 | MAVLinkTCPChannel::MAVLinkTCPChannel() 37 | : MAVLinkChannel(tcp_channel_id), 38 | running(false), 39 | send_thread(), 40 | receive_thread(), 41 | socket(), 42 | send_queue(max_tcp_channnel_queue_size), 43 | receive_queue(max_tcp_channnel_queue_size), 44 | send_time(0), 45 | receive_time(0) {} 46 | 47 | MAVLinkTCPChannel::~MAVLinkTCPChannel() { close(); } 48 | 49 | bool MAVLinkTCPChannel::init(const std::string address, uint16_t port) { 50 | bool ret = socket.init(address, port); 51 | 52 | if (!running) { 53 | running = true; 54 | 55 | std::thread send_th(&MAVLinkTCPChannel::send_task, this); 56 | send_thread.swap(send_th); 57 | 58 | std::thread receive_th(&MAVLinkTCPChannel::receive_task, this); 59 | receive_thread.swap(receive_th); 60 | } 61 | 62 | return ret; 63 | } 64 | 65 | void MAVLinkTCPChannel::close() { 66 | if (running) { 67 | running = false; 68 | 69 | receive_thread.join(); 70 | send_thread.join(); 71 | } 72 | 73 | socket.close(); 74 | } 75 | 76 | bool MAVLinkTCPChannel::send_message(const mavlink_message_t& msg) { 77 | send_queue.push(msg); 78 | 79 | return true; 80 | } 81 | 82 | bool MAVLinkTCPChannel::receive_message(mavlink_message_t& msg) { 83 | return receive_queue.pop(msg); 84 | } 85 | 86 | bool MAVLinkTCPChannel::message_available() { return !receive_queue.empty(); } 87 | 88 | std::chrono::milliseconds MAVLinkTCPChannel::last_send_time() { 89 | return send_time; 90 | } 91 | 92 | std::chrono::milliseconds MAVLinkTCPChannel::last_receive_time() { 93 | return receive_time; 94 | } 95 | 96 | void MAVLinkTCPChannel::send_task() { 97 | while (running) { 98 | mavlink_message_t msg; 99 | 100 | if (send_queue.pop(msg)) { 101 | if (socket.send_message(msg)) { 102 | send_time = timelib::time_since_epoch(); 103 | } 104 | } 105 | 106 | sleep(tcp_channel_send_interval); 107 | } 108 | } 109 | 110 | void MAVLinkTCPChannel::receive_task() { 111 | while (running) { 112 | mavlink_message_t msg; 113 | 114 | if (socket.receive_message(msg)) { 115 | receive_time = timelib::time_since_epoch(); 116 | receive_queue.push(msg); 117 | } 118 | 119 | sleep(tcp_channel_receive_interval); 120 | } 121 | } 122 | 123 | } // namespace mavio 124 | -------------------------------------------------------------------------------- /libs/mavio/src/Serial.cc: -------------------------------------------------------------------------------- 1 | /* 2 | Serial.cc 3 | 4 | MAVIO MAVLink I/O library. 5 | 6 | (C) Copyright 2017-2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | #include "Serial.h" 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | #include "Logger.h" 40 | 41 | namespace mavio { 42 | 43 | using std::string; 44 | using std::vector; 45 | 46 | constexpr suseconds_t serial_read_timeout = 250000; // microseconds 47 | 48 | constexpr char serial_by_id_dir[] = "/dev/serial/by-id/"; 49 | constexpr char serial_by_path_dir[] = "/dev/serial/by-path/"; 50 | constexpr char standard_serials[] = ""; // "/dev/ttyS0,/dev/ttyAMA0"; 51 | 52 | speed_t baud_rate_to_speed_t(int baud_rate) { 53 | switch (baud_rate) { 54 | case 0: 55 | return B0; 56 | case 50: 57 | return B50; 58 | case 75: 59 | return B75; 60 | case 110: 61 | return B110; 62 | case 134: 63 | return B134; 64 | case 150: 65 | return B150; 66 | case 200: 67 | return B200; 68 | case 300: 69 | return B300; 70 | case 600: 71 | return B600; 72 | case 1200: 73 | return B1200; 74 | case 1800: 75 | return B1800; 76 | case 2400: 77 | return B2400; 78 | case 4800: 79 | return B4800; 80 | case 9600: 81 | return B9600; 82 | case 19200: 83 | return B19200; 84 | case 38400: 85 | return B38400; 86 | case 57600: 87 | return B57600; 88 | case 115200: 89 | return B115200; 90 | // case 128000: 91 | // return B128000; 92 | case 230400: 93 | return B230400; 94 | // case 256000: 95 | // return B256000; 96 | case 460800: 97 | return B460800; 98 | case 500000: 99 | return B500000; 100 | case 576000: 101 | return B576000; 102 | case 921600: 103 | return B921600; 104 | break; 105 | case 1000000: 106 | return B1000000; 107 | case 1152000: 108 | return B1152000; 109 | case 1500000: 110 | return B1500000; 111 | case 2000000: 112 | return B2000000; 113 | case 2500000: 114 | return B2500000; 115 | case 3000000: 116 | return B3000000; 117 | } 118 | 119 | return B57600; 120 | } 121 | 122 | Serial::Serial() : tty_fd(-1), path(), last_error(0) {} 123 | 124 | Serial::~Serial() {} 125 | 126 | int Serial::open(const string& path, int baud_rate) { 127 | last_error = 0; 128 | this->path = path; 129 | 130 | tty_fd = ::open(path.data(), O_RDWR | O_NOCTTY /*| O_NONBLOCK*/); 131 | 132 | if (tty_fd < 0) { 133 | last_error = errno; 134 | mavio::log(LOG_ERR, "Failed to open file '%s' (errno = %d).", path.data(), 135 | errno); 136 | return -1; 137 | } 138 | 139 | struct termios tio; 140 | 141 | memset(&tio, 0, sizeof(tio)); 142 | tio.c_iflag = 0; 143 | tio.c_oflag = 0; 144 | tio.c_cflag = CS8 | CREAD | CLOCAL; 145 | tio.c_lflag = 0; 146 | tio.c_cc[VMIN] = 1; 147 | tio.c_cc[VTIME] = 5; 148 | 149 | speed_t speed = baud_rate_to_speed_t(baud_rate); 150 | cfsetospeed(&tio, speed); 151 | cfsetispeed(&tio, speed); 152 | 153 | tcgetattr(tty_fd, &old_tio); 154 | 155 | tcsetattr(tty_fd, TCSANOW, &tio); 156 | 157 | return 0; 158 | } 159 | 160 | int Serial::close() { 161 | if (tty_fd >= 0) { 162 | tcsetattr(tty_fd, TCSANOW, &old_tio); 163 | return ::close(tty_fd); 164 | } 165 | 166 | return 0; 167 | } 168 | 169 | int Serial::read() { 170 | unsigned char c; 171 | 172 | int ret = read(&c, 1); 173 | 174 | if (ret <= 0) { 175 | return -1; 176 | } 177 | 178 | return c; 179 | } 180 | 181 | int Serial::read(void* buffer, size_t size) { 182 | last_error = 0; 183 | 184 | if (tty_fd < 0) { 185 | return -1; 186 | } 187 | 188 | fd_set set; 189 | struct timeval timeout; 190 | 191 | FD_ZERO(&set); /* clear the set */ 192 | FD_SET(tty_fd, &set); /* add our file descriptor to the set */ 193 | 194 | timeout.tv_sec = 0; 195 | timeout.tv_usec = serial_read_timeout; 196 | 197 | int rv = ::select(tty_fd + 1, &set, NULL, NULL, &timeout); 198 | 199 | if (rv < 0) { 200 | last_error = errno; 201 | return -1; /* an error accured */ 202 | } 203 | 204 | if (rv == 0) { 205 | return 0; /* a timeout occured */ 206 | } 207 | 208 | int ret = ::read(tty_fd, buffer, size); 209 | last_error = errno; 210 | return ret; 211 | } 212 | 213 | int Serial::write(int c) { return write(&c, 1); } 214 | 215 | int Serial::write(const void* buffer, size_t n) { 216 | last_error = 0; 217 | 218 | if (tty_fd < 0) { 219 | last_error = -1; 220 | return -1; 221 | } 222 | 223 | int ret = ::write(tty_fd, buffer, n); 224 | 225 | if (ret < 0) { 226 | last_error = errno; 227 | return ret; 228 | } 229 | 230 | if (::tcflush(tty_fd, TCIOFLUSH) < 0) { 231 | last_error = errno; 232 | return -2; 233 | } 234 | 235 | return ret; 236 | } 237 | 238 | int Serial::get_last_error() { 239 | return last_error; 240 | } 241 | 242 | int Serial::get_serial_devices(vector& devices) { 243 | DIR* dp; 244 | struct dirent* dirp; 245 | 246 | if ((dp = opendir(serial_by_path_dir)) != NULL) { 247 | while ((dirp = readdir(dp)) != NULL) { 248 | if (string(dirp->d_name) == "." || string(dirp->d_name) == "..") continue; 249 | 250 | string device = string(serial_by_path_dir) + string(dirp->d_name); 251 | #ifdef __CYGWIN__ 252 | devices.push_back(string(device.data())); 253 | #else 254 | char real_path[PATH_MAX]; 255 | realpath(device.data(), real_path); 256 | devices.push_back(string(real_path)); 257 | #endif 258 | } 259 | 260 | closedir(dp); 261 | } 262 | 263 | // Add the default list of serial devices 264 | 265 | string s(standard_serials); 266 | size_t pos = 0; 267 | while ((pos = s.find(",")) != string::npos) { 268 | string device = s.substr(0, pos); 269 | devices.push_back(device); 270 | s.erase(0, pos + 1); 271 | } 272 | 273 | devices.push_back(s); 274 | 275 | return devices.size(); 276 | } 277 | 278 | } // namespace mavio 279 | -------------------------------------------------------------------------------- /libs/timelib/include/timelib.h: -------------------------------------------------------------------------------- 1 | /* 2 | timelib.h 3 | 4 | TimeLib library provides operations for getting current time, measuring 5 | duration, getting timestamps, and sleeping. 6 | 7 | (C) Copyright 2019 Envirover. 8 | 9 | This library is free software; you can redistribute it and/or 10 | modify it under the terms of the GNU Lesser General Public 11 | License as published by the Free Software Foundation; either 12 | version 2.1 of the License, or (at your option) any later version. 13 | 14 | This library is distributed in the hope that it will be useful, 15 | but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 17 | Lesser General Public License for more details. 18 | 19 | You should have received a copy of the GNU Lesser General Public 20 | License along with this library; if not, write to the Free Software 21 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 22 | */ 23 | 24 | #ifndef LIBS_TIMELIB_INCLUDE_TIMELIB_H_ 25 | #define LIBS_TIMELIB_INCLUDE_TIMELIB_H_ 26 | 27 | #include 28 | 29 | namespace timelib { 30 | 31 | // Stopwatch class is used to measure elapsed time. 32 | class Stopwatch { 33 | public: 34 | Stopwatch() : start_time(std::chrono::high_resolution_clock::now()) {} 35 | 36 | // Resets stopwatch. 37 | void reset() { reset(std::chrono::high_resolution_clock::now()); } 38 | 39 | // Set start time. 40 | void reset(std::chrono::high_resolution_clock::time_point t) { 41 | start_time = t; 42 | } 43 | 44 | // Returns number of milliseconds elapsed from the start of the stopwatch. 45 | std::chrono::milliseconds elapsed_time() const { 46 | return std::chrono::duration_cast( 47 | std::chrono::high_resolution_clock::now() - start_time); 48 | } 49 | 50 | private: 51 | std::chrono::high_resolution_clock::time_point start_time; 52 | }; 53 | 54 | // Returns number of milliseconds since epoch from the system clock. 55 | inline std::chrono::milliseconds time_since_epoch() { 56 | return std::chrono::duration_cast( 57 | std::chrono::high_resolution_clock::now().time_since_epoch()); 58 | } 59 | 60 | // Sleeps for the specified time in milliseconds. 61 | inline void sleep(const std::chrono::milliseconds& ms) { 62 | struct timespec t; 63 | t.tv_sec = ms.count() / 1000; 64 | t.tv_nsec = (ms.count() % 1000) * 1000000L; 65 | ::nanosleep(&t, nullptr); 66 | } 67 | 68 | // Writes current timestamp into the specified char buffer. 69 | inline void timestamp(char* str, size_t n) { 70 | std::chrono::time_point now = 71 | std::chrono::high_resolution_clock::now(); 72 | int64_t millis = std::chrono::duration_cast( 73 | now.time_since_epoch()) 74 | .count(); 75 | std::time_t time_now_t = std::chrono::high_resolution_clock::to_time_t(now); 76 | std::tm* t = std::localtime(&time_now_t); 77 | 78 | snprintf(str, n, "%04d-%02d-%02d %02d:%02d:%02d.%03d", t->tm_year + 1900, 79 | t->tm_mon + 1, t->tm_mday, t->tm_hour, t->tm_min, t->tm_sec, 80 | static_cast(millis % 1000)); 81 | } 82 | 83 | // Convert seconds to milliseconds 84 | inline constexpr std::chrono::milliseconds sec2ms(double seconds) { 85 | return std::chrono::milliseconds(static_cast(seconds * 1000)); 86 | } 87 | 88 | } // namespace timelib 89 | 90 | #endif // LIBS_TIMELIB_INCLUDE_TIMELIB_H_ 91 | -------------------------------------------------------------------------------- /pack/common/etc/logrotate.d/radioroom_lr.conf: -------------------------------------------------------------------------------- 1 | /var/log/radioroom.log 2 | { 3 | rotate 1 4 | daily 5 | size 1M 6 | missingok 7 | notifempty 8 | delaycompress 9 | compress 10 | postrotate 11 | invoke-rc.d rsyslog rotate > /dev/null 12 | endscript 13 | su root syslog 14 | } 15 | -------------------------------------------------------------------------------- /pack/common/etc/rsyslog.d/radioroom_log.conf: -------------------------------------------------------------------------------- 1 | $template radioroom_brief,"%timereported:::date-rfc3339% %syslogpriority-text% %msg%\n" 2 | :syslogtag, isequal, "radioroom:" /var/log/radioroom.log;radioroom_brief 3 | & ~ -------------------------------------------------------------------------------- /pack/common/etc/systemd/system/radioroom.service: -------------------------------------------------------------------------------- 1 | [Unit] 2 | Description=UV Radio Room Service 3 | Documentation=http://envirover.com/docs/radioroom.html 4 | 5 | [Service] 6 | Type=simple 7 | ExecStart=/usr/sbin/radioroom 8 | StandardOutput=null 9 | Restart=on-failure 10 | RestartSec=30 11 | 12 | [Install] 13 | WantedBy=multi-user.target 14 | Alias=radioroom.service -------------------------------------------------------------------------------- /pack/jetson/etc/radioroom.conf: -------------------------------------------------------------------------------- 1 | [autopilot] 2 | 3 | # Serial device path of the autopilot. If autopilot is connected using USB, 4 | # It is recommended to use device paths that do not change after reboots, 5 | # such as USB device links listed at /dev/serial/by-path/ or /dev/serial/by-id/. 6 | serial=/dev/ttyUSB0 7 | 8 | # Use the serial baud rate of the autopilot's telemetry port. 9 | serial_speed=57600 10 | 11 | [radioroom] 12 | 13 | # Setting auto_detect_serials to true enables automatic detection of autopilot 14 | # and ISBD transceivers on serial interfaces available in the system. 15 | auto_detect_serials=true 16 | 17 | [tcp] 18 | 19 | # Setting enabled to true enables TCP comm channel. 20 | enabled=false 21 | 22 | # Host and port of the TCP service. 23 | host=uvhub 24 | port=5060 25 | 26 | # Default reporting period of TCP comm channel in seconds. 27 | report_period=1 28 | 29 | [isbd] 30 | 31 | # Setting enabled to true enables ISBD comm channel. 32 | enabled=true 33 | 34 | # Serial device path of the ISBD transceiver. If autopilot is connected using USB, 35 | # It is recommended to use device paths that do not change after reboots, 36 | # such as USB device links listed at /dev/serial/by-path/ or /dev/serial/by-id/. 37 | serial=/dev/ttyUSB1 38 | 39 | ## Use the serial baud rate of the ISBD transceiver. 40 | serial_speed=19200 41 | 42 | # Default reporting period of ISBD comm channel in seconds. The report period can be changed at runtime 43 | # by setting HL_REPORT_PERIOD on-board parameter. 44 | report_period=60 45 | 46 | [camera_handler] 47 | 48 | # Command executed on MAV_CMD_VIDEO_START_CAPTURE command 49 | on_video_start_capture=systemctl start rtmpstream.service 50 | 51 | # Command executed on MAV_CMD_VIDEO_STOP_CAPTURE command 52 | on_video_stop_capture=systemctl stop rtmpstream.service 53 | 54 | # Command executed on MAV_CMD_IMAGE_START_CAPTURE command 55 | on_image_start_capture= 56 | 57 | # Command executed on MAV_CMD_IMAGE_STOP_CAPTURE command 58 | on_image_stop_capture= 59 | 60 | # Command executed on MAV_CMD_DO_DIGICAM_CONTROL command 61 | on_do_digicam_control=gst-launch-1.0 nvarguscamerasrc num-buffers=1 ! 'video/x-raw(memory:NVMM), width=(int)640, height=(int)480, format=(string)NV12, framerate=(fraction)30/1' ! nvjpegenc ! filesink location=/data/image-`date +"%Y-%m-%d-%H-%M-%S"`.jpg 62 | -------------------------------------------------------------------------------- /pack/jetson/etc/systemd/system/rtmpstream.service: -------------------------------------------------------------------------------- 1 | [Unit] 2 | Description=Captures video from MIPI CSI camera and streams it to RTMP server 3 | Documentation=http://envirover.com/docs/radioroom.html 4 | 5 | [Service] 6 | Type=simple 7 | ExecStart=/usr/bin/gst-launch-1.0 nvarguscamerasrc ! 'video/x-raw(memory:NVMM), width=(int)720, height=(int)480, format=(string)NV12, framerate=(fraction)30/1' ! nvv4l2h264enc control-rate=1 bitrate=1500000 ! 'video/x-h264, stream-format=(string)byte-stream' ! h264parse ! flvmux ! rtmpsink location='rtmp://videosrv/live/stream live=1' 8 | StandardOutput=file:/var/log/rtmpstream.log 9 | Restart=on-failure 10 | RestartSec=30 11 | 12 | [Install] 13 | WantedBy=multi-user.target 14 | Alias=rtmpstream.service -------------------------------------------------------------------------------- /pack/raspbian/etc/radioroom.conf: -------------------------------------------------------------------------------- 1 | [autopilot] 2 | 3 | # Serial device path of the autopilot. If autopilot is connected using USB, 4 | # It is recommended to use device paths that do not change after reboots, 5 | # such as USB device links listed at /dev/serial/by-path/ or /dev/serial/by-id/. 6 | serial=/dev/ttyUSB0 7 | 8 | # Use the serial baud rate of the autopilot's telemetry port. 9 | serial_speed=57600 10 | 11 | [radioroom] 12 | 13 | # Setting auto_detect_serials to true enables automatic detection of autopilot 14 | # and ISBD transceivers on serial interfaces available in the system. 15 | auto_detect_serials=true 16 | 17 | [tcp] 18 | 19 | # Setting enabled to true enables TCP comm channel. 20 | enabled=false 21 | 22 | # Host and port of the TCP service. 23 | host=uvhub 24 | port=5060 25 | 26 | # Default reporting period of TCP comm channel in seconds. 27 | report_period=1 28 | 29 | [isbd] 30 | 31 | # Setting enabled to true enables ISBD comm channel. 32 | enabled=true 33 | 34 | # Serial device path of the ISBD transceiver. If autopilot is connected using USB, 35 | # It is recommended to use device paths that do not change after reboots, 36 | # such as USB device links listed at /dev/serial/by-path/ or /dev/serial/by-id/. 37 | serial=/dev/ttyUSB1 38 | 39 | ## Use the serial baud rate of the ISBD transceiver. 40 | serial_speed=19200 41 | 42 | # Default reporting period of ISBD comm channel in seconds. The report period can be changed at runtime 43 | # by setting HL_REPORT_PERIOD on-board parameter. 44 | report_period=60 45 | 46 | [camera_handler] 47 | 48 | # Command executed on MAV_CMD_VIDEO_START_CAPTURE command 49 | on_video_start_capture=systemctl start rtmpstream.service 50 | 51 | # Command executed on MAV_CMD_VIDEO_STOP_CAPTURE command 52 | on_video_stop_capture=systemctl stop rtmpstream.service 53 | 54 | # Command executed on MAV_CMD_IMAGE_START_CAPTURE command 55 | on_image_start_capture= 56 | 57 | # Command executed on MAV_CMD_IMAGE_STOP_CAPTURE command 58 | on_image_stop_capture= 59 | 60 | # Command executed on MAV_CMD_DO_DIGICAM_CONTROL command 61 | on_do_digicam_control=raspistill -w 640 -h 480 -o /data/image-`date +"%Y-%m-%d-%H-%M-%S"`.jpg 62 | -------------------------------------------------------------------------------- /pack/raspbian/etc/sytetmd/system/rtmpstream.service: -------------------------------------------------------------------------------- 1 | [Unit] 2 | Description=Captures video from Raspberry Pi camera and streams it to RTMP server 3 | Documentation=http://envirover.com/docs/radioroom.html 4 | 5 | [Service] 6 | Type=simple 7 | ExecStart=/usr/bin/raspivid --nopreview -w 640 -h 480 --mode 5 --bitrate 1500000 --intra 1 --awb auto --brightness 55 --saturation 10 --sharpness 50 --contrast 15 --drc medium -fl --timeout 0 --output - | gst-launch-1.0 -v fdsrc ! h264parse ! flvmux ! rtmpsink location='rtmp://uvhub/live/stream live=1' 8 | StandardOutput=file:/var/log/rtmpstream.log 9 | Restart=on-failure 10 | RestartSec=30 11 | 12 | [Install] 13 | WantedBy=multi-user.target 14 | Alias=rtmpstream.service -------------------------------------------------------------------------------- /src/CameraHandler.cc: -------------------------------------------------------------------------------- 1 | /* 2 | CameraHandler.cc 3 | 4 | Telemetry for MAVLink autopilots. 5 | 6 | (C) Copyright 2020 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | 23 | #include "CameraHandler.h" 24 | 25 | #include 26 | 27 | #include "Config.h" 28 | #include "MAVLinkLogger.h" 29 | #include "timelib.h" 30 | 31 | namespace radioroom { 32 | 33 | constexpr size_t max_handler_queue_size = 1; 34 | constexpr size_t max_receive_queue_size = 1024; 35 | 36 | const std::chrono::milliseconds executor_sleep_interval(10); 37 | 38 | std::string replace(std::string str, const std::string& from, 39 | const std::string& to) { 40 | size_t start_pos = 0; 41 | while ((start_pos = str.find(from, start_pos)) != std::string::npos) { 42 | str.replace(start_pos, from.length(), to); 43 | start_pos += 44 | to.length(); // Handles case where 'to' is a substring of 'from' 45 | } 46 | return str; 47 | } 48 | 49 | CameraHandler::CmdExecutor::CmdExecutor( 50 | const std::string name, mavio::CircularBuffer& ack_queue) 51 | : name(name), 52 | command(), 53 | out_queue(ack_queue), 54 | in_queue(max_handler_queue_size), 55 | handler_thread(), 56 | running(false), 57 | executing(false) {} 58 | 59 | bool CameraHandler::CmdExecutor::init(const std::string cmd) { 60 | if (!running && !cmd.empty()) { 61 | command = cmd; 62 | 63 | running = true; 64 | 65 | handler_thread = std::thread{&CmdExecutor::task, this}; 66 | 67 | mavio::log(LOG_INFO, "Handler '%s' initialized.", name.c_str()); 68 | 69 | return true; 70 | } 71 | 72 | return false; 73 | } 74 | 75 | void CameraHandler::CmdExecutor::close() { 76 | if (running) { 77 | running = false; 78 | 79 | handler_thread.join(); 80 | 81 | mavio::log(LOG_INFO, "Handler '%s' closed.", name.c_str()); 82 | } 83 | } 84 | 85 | bool CameraHandler::CmdExecutor::submit(const mavlink_message_t& msg) { 86 | // Dismiss message if the executor is not initialized. 87 | if (running) { 88 | in_queue.push(msg); 89 | return true; 90 | } 91 | 92 | return false; 93 | } 94 | 95 | void CameraHandler::CmdExecutor::task() { 96 | while (running) { 97 | mavlink_message_t msg; 98 | 99 | if (in_queue.pop(msg)) { 100 | executing = true; 101 | execute_cmd(msg); 102 | executing = false; 103 | } 104 | 105 | timelib::sleep(executor_sleep_interval); 106 | } 107 | } 108 | 109 | void CameraHandler::CmdExecutor::replace_parameters( 110 | const mavlink_message_t& msg, std::string& cmd) const { 111 | if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) { 112 | cmd = replace(cmd, "{{param1}}", 113 | std::to_string(mavlink_msg_command_long_get_param1(&msg))); 114 | cmd = replace(cmd, "{{param2}}", 115 | std::to_string(mavlink_msg_command_long_get_param2(&msg))); 116 | cmd = replace(cmd, "{{param3}}", 117 | std::to_string(mavlink_msg_command_long_get_param3(&msg))); 118 | cmd = replace(cmd, "{{param4}}", 119 | std::to_string(mavlink_msg_command_long_get_param4(&msg))); 120 | cmd = replace(cmd, "{{param5}}", 121 | std::to_string(mavlink_msg_command_long_get_param5(&msg))); 122 | cmd = replace(cmd, "{{param6}}", 123 | std::to_string(mavlink_msg_command_long_get_param6(&msg))); 124 | cmd = replace(cmd, "{{param7}}", 125 | std::to_string(mavlink_msg_command_long_get_param7(&msg))); 126 | } else if (msg.msgid == MAVLINK_MSG_ID_COMMAND_INT) { 127 | cmd = replace(cmd, "{{param1}}", 128 | std::to_string(mavlink_msg_command_int_get_param1(&msg))); 129 | cmd = replace(cmd, "{{param2}}", 130 | std::to_string(mavlink_msg_command_int_get_param2(&msg))); 131 | cmd = replace(cmd, "{{param3}}", 132 | std::to_string(mavlink_msg_command_int_get_param3(&msg))); 133 | cmd = replace(cmd, "{{param4}}", 134 | std::to_string(mavlink_msg_command_int_get_param4(&msg))); 135 | } 136 | } 137 | 138 | void CameraHandler::CmdExecutor::execute_cmd(const mavlink_message_t& msg) { 139 | mavio::log(LOG_INFO, "Handler '%s' execution started.", name.c_str()); 140 | 141 | std::string cmd = command; 142 | 143 | replace_parameters(msg, cmd); 144 | 145 | int ret = ::system(cmd.c_str()); 146 | 147 | mavlink_command_ack_t command_ack; 148 | 149 | if (ret < 0) { 150 | mavio::log(LOG_ERR, "Handler '%s' execution failed. %s", name.c_str(), 151 | strerror(errno)); 152 | command_ack.result = MAV_RESULT_FAILED; 153 | } else { 154 | mavio::log(LOG_INFO, "Handler '%s' execution completed.", name.c_str()); 155 | command_ack.result = MAV_RESULT_ACCEPTED; 156 | } 157 | 158 | mavlink_message_t ack_msg; 159 | if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) { 160 | command_ack.command = mavlink_msg_command_long_get_command(&msg); 161 | mavlink_msg_command_ack_encode( 162 | mavlink_msg_command_long_get_target_system(&msg), 163 | mavlink_msg_command_long_get_target_component(&msg), &ack_msg, 164 | &command_ack); 165 | } else if (msg.msgid == MAVLINK_MSG_ID_COMMAND_INT) { 166 | command_ack.command = mavlink_msg_command_int_get_command(&msg); 167 | mavlink_msg_command_ack_encode( 168 | mavlink_msg_command_int_get_target_system(&msg), 169 | mavlink_msg_command_int_get_target_component(&msg), &ack_msg, 170 | &command_ack); 171 | } 172 | 173 | if (command_ack.command != MAV_CMD_COMPONENT_ARM_DISARM) { 174 | out_queue.push(ack_msg); 175 | } 176 | } 177 | 178 | CameraHandler::CameraHandler() 179 | : MAVLinkChannel(mavio::camera_handler_channel_id), 180 | running(false), 181 | on_video_start_capture_executor("on_video_start_capture", receive_queue), 182 | on_video_stop_capture_executor("on_video_stop_capture", receive_queue), 183 | on_image_start_capture_executor("on_image_start_capture", receive_queue), 184 | on_image_stop_capture_executor("on_image_stop_capture", receive_queue), 185 | on_do_digicam_control_executor("on_do_digicam_control", receive_queue), 186 | receive_queue(max_handler_queue_size) {} 187 | 188 | bool CameraHandler::init() { 189 | if (!running) { 190 | running = true; 191 | 192 | on_video_start_capture_executor.init(config.get_on_video_start_capture()); 193 | on_video_stop_capture_executor.init(config.get_on_video_stop_capture()); 194 | on_image_start_capture_executor.init(config.get_on_image_start_capture()); 195 | on_image_stop_capture_executor.init(config.get_on_image_stop_capture()); 196 | on_do_digicam_control_executor.init(config.get_on_do_digicam_control()); 197 | } 198 | 199 | return true; 200 | } 201 | 202 | void CameraHandler::close() { 203 | if (running) { 204 | running = false; 205 | 206 | on_video_start_capture_executor.close(); 207 | on_video_start_capture_executor.close(); 208 | on_image_start_capture_executor.close(); 209 | on_image_stop_capture_executor.close(); 210 | on_do_digicam_control_executor.close(); 211 | } 212 | } 213 | 214 | bool CameraHandler::send_message(const mavlink_message_t& msg) { 215 | uint16_t command; 216 | 217 | if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) { 218 | command = mavlink_msg_command_long_get_command(&msg); 219 | } else if (msg.msgid == MAVLINK_MSG_ID_COMMAND_INT) { 220 | command = mavlink_msg_command_int_get_command(&msg); 221 | } else { 222 | return false; 223 | } 224 | 225 | switch (command) { 226 | case MAV_CMD_VIDEO_START_CAPTURE: 227 | return on_video_start_capture_executor.submit(msg); 228 | case MAV_CMD_VIDEO_STOP_CAPTURE: 229 | return on_video_stop_capture_executor.submit(msg); 230 | case MAV_CMD_IMAGE_START_CAPTURE: 231 | return on_image_start_capture_executor.submit(msg); 232 | case MAV_CMD_IMAGE_STOP_CAPTURE: 233 | return on_image_stop_capture_executor.submit(msg); 234 | case MAV_CMD_DO_DIGICAM_CONTROL: 235 | return on_do_digicam_control_executor.submit(msg); 236 | } 237 | 238 | return false; 239 | } 240 | 241 | bool CameraHandler::receive_message(mavlink_message_t& msg) { 242 | return receive_queue.pop(msg); 243 | } 244 | 245 | bool CameraHandler::message_available() { return !receive_queue.empty(); } 246 | 247 | std::chrono::milliseconds CameraHandler::last_send_time() { 248 | return std::chrono::milliseconds(0); 249 | } 250 | 251 | std::chrono::milliseconds CameraHandler::last_receive_time() { 252 | return std::chrono::milliseconds(0); 253 | } 254 | 255 | } // namespace radioroom 256 | -------------------------------------------------------------------------------- /src/CameraHandler.h: -------------------------------------------------------------------------------- 1 | /* 2 | CameraHandler.h 3 | 4 | Telemetry for MAVLink autopilots. 5 | 6 | (C) Copyright 2020 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | 23 | #ifndef SRC_CAMERAHANDLER_H_ 24 | #define SRC_CAMERAHANDLER_H_ 25 | 26 | #include "CircularBuffer.h" 27 | #include "MAVLinkChannel.h" 28 | #include "MAVLinkLib.h" 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | namespace radioroom { 36 | 37 | /** 38 | * Executes system commands on MAVLink camera protocol messages such as 39 | * photo and video capture commands. 40 | */ 41 | class CameraHandler : public mavio::MAVLinkChannel { 42 | /** 43 | * Executes system command in a dedicated thread on MAVLink command messages. 44 | */ 45 | class CmdExecutor { 46 | public: 47 | CmdExecutor(const std::string name, 48 | mavio::CircularBuffer& ack_queue); 49 | 50 | /** 51 | * Initializes CmdExecutor instance if cmd string is not empty. 52 | */ 53 | bool init(const std::string cmd); 54 | 55 | /** 56 | * Stops the thread. 57 | */ 58 | void close(); 59 | 60 | /** 61 | * Submits MAVLink command message for execution. 62 | * 63 | * returns true if the command will be handled by the executor. 64 | */ 65 | bool submit(const mavlink_message_t& msg); 66 | 67 | private: 68 | void task(); 69 | 70 | void replace_parameters(const mavlink_message_t& msg, 71 | std::string& cmd) const; 72 | 73 | void execute_cmd(const mavlink_message_t& msg); 74 | 75 | std::string name; 76 | std::string command; 77 | mavio::CircularBuffer& out_queue; 78 | mavio::CircularBuffer in_queue; 79 | std::thread handler_thread; 80 | std::atomic running; 81 | std::atomic executing; 82 | }; 83 | 84 | public: 85 | /** 86 | * Constructs CameraHandler instance. 87 | */ 88 | CameraHandler(); 89 | 90 | /** 91 | * Initializes CameraHandler instance. 92 | */ 93 | bool init(); 94 | 95 | /** 96 | * Coloses CameraHandler instance. 97 | */ 98 | void close() override; 99 | 100 | /** 101 | * Send MAVLink message to CameraHandler. 102 | * 103 | * @param msg MAVLink message 104 | * @return returns true if command will be handled by the handler. 105 | */ 106 | bool send_message(const mavlink_message_t& msg) override; 107 | 108 | /** 109 | * Receive MAVLink message from CameraHandler. 110 | * 111 | * @param msg reference to MAVLink message object 112 | * @return true if MAVLink message was received. 113 | */ 114 | bool receive_message(mavlink_message_t& msg) override; 115 | 116 | /** 117 | * Returns true if there is a message in the receive queue. 118 | */ 119 | bool message_available() override; 120 | 121 | /** 122 | * Returns time of the last successfully sent message. 123 | */ 124 | std::chrono::milliseconds last_send_time() override; 125 | 126 | /** 127 | * Returns time of the last successfully received message. 128 | */ 129 | std::chrono::milliseconds last_receive_time() override; 130 | 131 | private: 132 | std::atomic running; 133 | 134 | // Command executors 135 | CmdExecutor on_video_start_capture_executor; 136 | CmdExecutor on_video_stop_capture_executor; 137 | CmdExecutor on_image_start_capture_executor; 138 | CmdExecutor on_image_stop_capture_executor; 139 | CmdExecutor on_do_digicam_control_executor; 140 | 141 | // Queue that buffers messages received from the handlers 142 | mavio::CircularBuffer receive_queue; 143 | 144 | mavlink_message_t arm_disarm_cmd; 145 | }; 146 | 147 | } // namespace radioroom 148 | 149 | #endif // SRC_CAMERAHANDLER_H_ 150 | -------------------------------------------------------------------------------- /src/Config.cc: -------------------------------------------------------------------------------- 1 | /* 2 | Config.cc 3 | 4 | This file is a part of RadioRoom project. 5 | 6 | (C) Copyright 2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | 23 | #include "Config.h" 24 | #include "INIReader.h" 25 | 26 | namespace radioroom { 27 | 28 | Config config; 29 | 30 | constexpr char default_autopilot_serial[] = "/dev/ttyusb0"; 31 | constexpr int autopilot_serial_baud_rate = 57600; 32 | 33 | constexpr bool default_isbd_enabled = true; 34 | constexpr char default_isbd_serial[] = "/dev/ttyusb1"; 35 | constexpr int isbd_serial_baud_rate = 19200; 36 | 37 | constexpr bool default_tcp_enabled = false; 38 | constexpr char default_tcp_host[] = ""; 39 | constexpr int default_tcp_port = 5060; 40 | 41 | constexpr double default_isbd_report_period = 300.0; // 5 minutes 42 | constexpr double default_tcp_report_period = 60.0; // 1 minute 43 | 44 | // radioroom.conf properties 45 | constexpr char autopilot_config_section[] = "autopilot"; 46 | constexpr char autopilot_serial_property[] = "serial"; 47 | constexpr char autopilot_serial_speed_property[] = "serial_speed"; 48 | 49 | constexpr char radioroom_config_section[] = "radioroom"; 50 | constexpr char auto_detect_serials_property[] = "auto_detect_serials"; 51 | constexpr char report_period_property[] = "report_period"; 52 | 53 | constexpr char isbd_config_section[] = "isbd"; 54 | constexpr char isbd_enabled_property[] = "enabled"; 55 | constexpr char isbd_serial_property[] = "serial"; 56 | constexpr char isbd_serial_speed_property[] = "serial_speed"; 57 | 58 | constexpr char tcp_config_section[] = "tcp"; 59 | constexpr char tcp_enabled_property[] = "enabled"; 60 | constexpr char tcp_host_property[] = "host"; 61 | constexpr char tcp_port_property[] = "port"; 62 | 63 | constexpr char camera_handler_config_section[] = "camera_handler"; 64 | constexpr char on_video_start_capture_property[] = "on_video_start_capture"; 65 | constexpr char on_video_stop_capture_property[] = "on_video_stop_capture"; 66 | constexpr char on_image_start_capture_property[] = "on_image_start_capture"; 67 | constexpr char on_image_stop_capture_property[] = "on_image_stop_capture"; 68 | constexpr char on_do_digicam_control_property[] = "on_do_digicam_control"; 69 | 70 | Config::Config() 71 | : autopilot_serial(default_autopilot_serial), 72 | autopilot_serial_speed(autopilot_serial_baud_rate), 73 | auto_detect_serials(true), 74 | debug_mode(false), 75 | isbd_enabled(default_isbd_enabled), 76 | isbd_serial(default_isbd_serial), 77 | isbd_serial_speed(isbd_serial_baud_rate), 78 | isbd_report_period(default_isbd_report_period), 79 | tcp_enabled(default_tcp_enabled), 80 | tcp_host(default_tcp_host), 81 | tcp_port(default_tcp_port), 82 | tcp_report_period(default_tcp_report_period) {} 83 | 84 | int Config::init(const std::string& config_file) { 85 | INIReader conf(config_file); 86 | 87 | int ret = conf.ParseError(); 88 | 89 | if (ret < 0) { 90 | return ret; 91 | } 92 | 93 | /* [autopilot] config section */ 94 | 95 | set_autopilot_serial(conf.Get(autopilot_config_section, 96 | autopilot_serial_property, 97 | default_autopilot_serial)); 98 | 99 | set_autopilot_serial_speed(conf.GetInteger(autopilot_config_section, 100 | autopilot_serial_speed_property, 101 | autopilot_serial_baud_rate)); 102 | 103 | /* [radioroom] config section */ 104 | 105 | set_auto_detect_serials(conf.GetBoolean(radioroom_config_section, 106 | auto_detect_serials_property, true)); 107 | 108 | /* [isbd] config section */ 109 | 110 | set_isbd_enabled(conf.GetBoolean(isbd_config_section, isbd_enabled_property, 111 | default_isbd_enabled)); 112 | 113 | set_isbd_serial( 114 | conf.Get(isbd_config_section, isbd_serial_property, default_isbd_serial)); 115 | 116 | set_isbd_serial_speed(conf.GetInteger( 117 | isbd_config_section, isbd_serial_speed_property, isbd_serial_baud_rate)); 118 | 119 | set_isbd_report_period(conf.GetReal( 120 | isbd_config_section, report_period_property, default_isbd_report_period)); 121 | 122 | /* [tcp] config section */ 123 | 124 | set_tcp_enabled(conf.GetBoolean(tcp_config_section, tcp_enabled_property, 125 | default_tcp_enabled)); 126 | 127 | set_tcp_host( 128 | conf.Get(tcp_config_section, tcp_host_property, default_tcp_host)); 129 | 130 | set_tcp_port( 131 | conf.GetInteger(tcp_config_section, tcp_port_property, default_tcp_port)); 132 | 133 | set_tcp_report_period(conf.GetReal(tcp_config_section, report_period_property, 134 | default_tcp_report_period)); 135 | 136 | set_on_video_start_capture(conf.Get(camera_handler_config_section, 137 | on_video_start_capture_property, "")); 138 | 139 | set_on_video_stop_capture(conf.Get(camera_handler_config_section, 140 | on_video_stop_capture_property, "")); 141 | 142 | set_on_image_start_capture(conf.Get(camera_handler_config_section, 143 | on_image_start_capture_property, "")); 144 | 145 | set_on_image_stop_capture(conf.Get(camera_handler_config_section, 146 | on_image_stop_capture_property, "")); 147 | 148 | set_on_do_digicam_control(conf.Get(camera_handler_config_section, 149 | on_do_digicam_control_property, "")); 150 | 151 | return 0; 152 | } 153 | 154 | bool Config::get_debug_mode() const { return debug_mode; } 155 | 156 | void Config::set_debug_mode(bool debug) { debug_mode = debug; } 157 | 158 | std::string Config::get_autopilot_serial() const { return autopilot_serial; } 159 | 160 | void Config::set_autopilot_serial(const std::string& path) { 161 | autopilot_serial = path; 162 | } 163 | 164 | int Config::get_autopilot_serial_speed() const { 165 | return autopilot_serial_speed; 166 | } 167 | 168 | void Config::set_autopilot_serial_speed(int speed) { 169 | autopilot_serial_speed = speed; 170 | } 171 | 172 | bool Config::get_isbd_enabled() const { return isbd_enabled; } 173 | 174 | void Config::set_isbd_enabled(bool enabled) { isbd_enabled = enabled; } 175 | 176 | std::string Config::get_isbd_serial() const { return isbd_serial; } 177 | 178 | void Config::set_isbd_serial(const std::string& path) { isbd_serial = path; } 179 | 180 | int Config::get_isbd_serial_speed() const { return isbd_serial_speed; } 181 | 182 | void Config::set_isbd_serial_speed(int speed) { isbd_serial_speed = speed; } 183 | 184 | bool Config::get_auto_detect_serials() const { return auto_detect_serials; } 185 | 186 | void Config::set_auto_detect_serials(bool a) { auto_detect_serials = a; } 187 | 188 | double Config::get_isbd_report_period() const { return isbd_report_period; } 189 | 190 | void Config::set_isbd_report_period(double period) { 191 | isbd_report_period = period; 192 | } 193 | 194 | bool Config::get_tcp_enabled() const { return tcp_enabled; } 195 | 196 | void Config::set_tcp_enabled(bool enabled) { tcp_enabled = enabled; } 197 | 198 | std::string Config::get_tcp_host() const { return tcp_host; } 199 | 200 | void Config::set_tcp_host(const std::string& host) { tcp_host = host; } 201 | 202 | int Config::get_tcp_port() const { return tcp_port; } 203 | 204 | void Config::set_tcp_port(int port) { tcp_port = port; } 205 | 206 | double Config::get_tcp_report_period() const { return tcp_report_period; } 207 | 208 | void Config::set_tcp_report_period(double period) { 209 | tcp_report_period = period; 210 | } 211 | 212 | std::string Config::get_on_video_start_capture() const { 213 | return on_video_start_capture; 214 | } 215 | 216 | void Config::set_on_video_start_capture(const std::string cmd) { 217 | on_video_start_capture = cmd; 218 | } 219 | 220 | std::string Config::get_on_video_stop_capture() const { 221 | return on_video_stop_capture; 222 | } 223 | 224 | void Config::set_on_video_stop_capture(const std::string cmd) { 225 | on_video_stop_capture = cmd; 226 | } 227 | 228 | std::string Config::get_on_image_start_capture() const { 229 | return on_image_start_capture; 230 | } 231 | 232 | void Config::set_on_image_start_capture(const std::string cmd) { 233 | on_image_start_capture = cmd; 234 | } 235 | 236 | std::string Config::get_on_image_stop_capture() const { 237 | return on_image_stop_capture; 238 | } 239 | 240 | void Config::set_on_image_stop_capture(const std::string cmd) { 241 | on_image_stop_capture = cmd; 242 | } 243 | 244 | std::string Config::get_on_do_digicam_control() const { 245 | return on_do_digicam_control; 246 | } 247 | 248 | void Config::set_on_do_digicam_control(const std::string cmd) { 249 | on_do_digicam_control = cmd; 250 | } 251 | 252 | } // namespace radioroom 253 | -------------------------------------------------------------------------------- /src/Config.h: -------------------------------------------------------------------------------- 1 | /* 2 | Config.h 3 | 4 | This file is a part of RadioRoom project. 5 | 6 | (C) Copyright 2017-2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | #ifndef SRC_CONFIG_H_ 23 | #define SRC_CONFIG_H_ 24 | 25 | #include 26 | 27 | namespace radioroom { 28 | 29 | constexpr char default_config_file[] = "/etc/radioroom.conf"; 30 | 31 | /** 32 | * RadioRoom configuration properties. 33 | */ 34 | class Config { 35 | public: 36 | Config(); 37 | 38 | /* 39 | * Loads configuration from the specified config file. 40 | * 41 | * Returns 0 in case of success and exit code in case of invalid 42 | * configuration. 43 | */ 44 | int init(const std::string& config_file); 45 | 46 | /* Autopilot configuration properties */ 47 | 48 | std::string get_autopilot_serial() const; 49 | void set_autopilot_serial(const std::string& path); 50 | 51 | int get_autopilot_serial_speed() const; 52 | void set_autopilot_serial_speed(int speed); 53 | 54 | /* RadioRoom configuration properties */ 55 | 56 | bool get_auto_detect_serials() const; 57 | void set_auto_detect_serials(bool a); 58 | 59 | bool get_debug_mode() const; 60 | void set_debug_mode(bool debug); 61 | 62 | /* ISBD comm link configuration properties */ 63 | 64 | bool get_isbd_enabled() const; 65 | void set_isbd_enabled(bool enabled); 66 | 67 | std::string get_isbd_serial() const; 68 | void set_isbd_serial(const std::string& path); 69 | 70 | int get_isbd_serial_speed() const; 71 | void set_isbd_serial_speed(int speed); 72 | 73 | double get_isbd_report_period() const; 74 | void set_isbd_report_period(double period); 75 | 76 | /* TCP/IP comm link configuration properties */ 77 | 78 | bool get_tcp_enabled() const; 79 | void set_tcp_enabled(bool enabled); 80 | 81 | std::string get_tcp_host() const; 82 | void set_tcp_host(const std::string& host); 83 | 84 | int get_tcp_port() const; 85 | void set_tcp_port(int port); 86 | 87 | double get_tcp_report_period() const; 88 | void set_tcp_report_period(double period); 89 | 90 | std::string get_on_video_start_capture() const; 91 | void set_on_video_start_capture(const std::string cmd); 92 | 93 | std::string get_on_video_stop_capture() const; 94 | void set_on_video_stop_capture(const std::string cmd); 95 | 96 | std::string get_on_image_start_capture() const; 97 | void set_on_image_start_capture(const std::string cmd); 98 | 99 | std::string get_on_image_stop_capture() const; 100 | void set_on_image_stop_capture(const std::string cmd); 101 | 102 | std::string get_on_do_digicam_control() const; 103 | void set_on_do_digicam_control(const std::string cmd); 104 | 105 | private: 106 | std::string autopilot_serial; 107 | int autopilot_serial_speed; 108 | 109 | bool auto_detect_serials; 110 | bool debug_mode; 111 | 112 | bool isbd_enabled; 113 | std::string isbd_serial; 114 | int isbd_serial_speed; 115 | double isbd_report_period; 116 | 117 | bool tcp_enabled; 118 | std::string tcp_host; 119 | int tcp_port; 120 | double tcp_report_period; 121 | 122 | std::string on_video_start_capture; 123 | std::string on_video_stop_capture; 124 | std::string on_image_start_capture; 125 | std::string on_image_stop_capture; 126 | std::string on_do_digicam_control; 127 | }; 128 | 129 | extern Config config; 130 | 131 | } // namespace radioroom 132 | 133 | #endif // SRC_CONFIG_H_ 134 | -------------------------------------------------------------------------------- /src/MAVLinkHandler.h: -------------------------------------------------------------------------------- 1 | /* 2 | MAVLinkHandler.h 3 | 4 | Telemetry for MAVLink autopilots. 5 | 6 | (C) Copyright 2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | 23 | #ifndef SRC_MAVLINKHANDLER_H_ 24 | #define SRC_MAVLINKHANDLER_H_ 25 | 26 | #include "MAVLinkAutopilot.h" 27 | #include "CameraHandler.h" 28 | #include "MAVLinkISBDChannel.h" 29 | #include "MAVLinkTCPChannel.h" 30 | #include "MAVReport.h" 31 | #include "timelib.h" 32 | 33 | namespace radioroom { 34 | 35 | constexpr size_t max_mission_count = 1024; 36 | 37 | /** 38 | * Telemetry for MAVLink autopilots. 39 | */ 40 | class MAVLinkHandler { 41 | public: 42 | /** 43 | * Default constructor. 44 | */ 45 | MAVLinkHandler(); 46 | 47 | /** 48 | * Initializes enabled comm channels and autopilot connections. 49 | * 50 | * Returns true if autopilot and enabled comm link connections were configured 51 | * successfully. 52 | */ 53 | bool init(); 54 | 55 | /* 56 | * Closes all opened connections. 57 | */ 58 | void close(); 59 | 60 | /** 61 | * Single turn of the main message pump that timers, routes and processes 62 | * messages received from autopilot and comm channels. 63 | * 64 | * The pump must run in a tight loop started after init(). 65 | */ 66 | void loop(); 67 | 68 | private: 69 | /* 70 | * Returns channel that successfully sent or received message last. 71 | */ 72 | mavio::MAVLinkChannel& active_channel(); 73 | 74 | /* 75 | * Hanlde mobile-originated message received from autopilot. 76 | */ 77 | void handle_mo_message(const mavlink_message_t& msg, 78 | mavio::MAVLinkChannel& channel); 79 | 80 | /* 81 | * Handle mobile-terminated message received from a comm channel. 82 | */ 83 | void handle_mt_message(const mavlink_message_t& msg, 84 | mavio::MAVLinkChannel& channel); 85 | 86 | /** 87 | * Sends report message to one of the comm channels if the channel report 88 | * period has elapsed. 89 | * 90 | * returns true if report was sent. 91 | */ 92 | bool send_report(); 93 | 94 | /* 95 | * Sends heartbeat message to autopilot if hearbeat period has elapsed and 96 | * the comm channels are not at faulted state (one of the channels successfuly 97 | * sent a message during it's report period). 98 | * 99 | * This allows autopilots to handle lost link gracefully if heartbeats are not 100 | * received. 101 | */ 102 | bool send_heartbeat(); 103 | 104 | /* 105 | * Requests autopilot data streams required to compose report message. 106 | */ 107 | void request_data_streams(); 108 | 109 | /* 110 | * Sets send retry timer in milliseconds for the specified message. 111 | */ 112 | void set_retry_send_timer(const mavlink_message_t& msg, 113 | const std::chrono::milliseconds& timeout, 114 | int retries); 115 | 116 | /* 117 | * Cancels send retry timer for the specified message id. 118 | */ 119 | void cancel_retry_send_timer(int msgid); 120 | 121 | /* 122 | * Retries sending message specifif in set_retry_send_timer() call if 123 | * the retry timeout elapses and the retry counter iz nonzero. 124 | * 125 | * Decrements the retries counter. 126 | */ 127 | void check_retry_send_timer(); 128 | 129 | mavio::MAVLinkAutopilot autopilot; 130 | CameraHandler camera_handler; 131 | mavio::MAVLinkISBDChannel isbd_channel; 132 | mavio::MAVLinkTCPChannel tcp_channel; 133 | timelib::Stopwatch heartbeat_timer; 134 | timelib::Stopwatch primary_report_timer; 135 | timelib::Stopwatch secondary_report_timer; 136 | MAVReport report; 137 | mavlink_message_t mission_count_msg; 138 | mavlink_message_t missions[max_mission_count]; 139 | size_t missions_received; 140 | std::string param_set_param_id; 141 | uint16_t wp_num; 142 | 143 | timelib::Stopwatch retry_timer; 144 | mavlink_message_t retry_msg; 145 | std::chrono::milliseconds retry_timeout; 146 | int retry_count; 147 | }; 148 | 149 | } // namespace radioroom 150 | 151 | #endif // SRC_MAVLINKHANDLER_H_ 152 | -------------------------------------------------------------------------------- /src/MAVReport.cc: -------------------------------------------------------------------------------- 1 | /* 2 | MAVReport.cc 3 | 4 | Telemetry for MAVLink autopilots. 5 | 6 | (C) Copyright 2021 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | 23 | #include "MAVReport.h" 24 | 25 | namespace radioroom { 26 | 27 | constexpr uint16_t mavlink_msg_mask_high_latency = 0x00FF; 28 | 29 | // Convert radians to degrees 30 | inline int16_t rad_to_degrees(float rad) { 31 | return (int16_t)(rad * 180.0 / 3.14159265358979323846 + 360.0) % 360; 32 | } 33 | 34 | MAVReport::MAVReport() : sysid(1), compid(0), mask(0) {} 35 | 36 | /** 37 | * Integrates data from the specified MAVLink message into the HIGH_LATENCY2 38 | * message. 39 | */ 40 | bool MAVReport::update(const mavlink_message_t& msg) { 41 | switch (msg.msgid) { 42 | case MAVLINK_MSG_ID_HEARTBEAT: // 0 43 | report.type = mavlink_msg_heartbeat_get_type(&msg); 44 | report.autopilot = mavlink_msg_heartbeat_get_autopilot(&msg); 45 | report.custom_mode = get_custom_mode( 46 | mavlink_msg_heartbeat_get_base_mode(&msg), 47 | mavlink_msg_heartbeat_get_custom_mode(&msg)); 48 | sysid = msg.sysid; 49 | compid = msg.compid; 50 | mask |= mavlink_msg_mask_heartbeat; 51 | return true; 52 | case MAVLINK_MSG_ID_SYS_STATUS: // 1 53 | report.battery = mavlink_msg_sys_status_get_battery_remaining(&msg); 54 | report.custom0 = 55 | mavlink_msg_sys_status_get_voltage_battery(&msg) / 1000; 56 | report.failure_flags = get_failure_flags( 57 | mavlink_msg_sys_status_get_onboard_control_sensors_health(&msg)); 58 | mask |= mavlink_msg_mask_sys_status; 59 | return true; 60 | case MAVLINK_MSG_ID_GPS_RAW_INT: // 24 61 | report.groundspeed = mavlink_msg_gps_raw_int_get_vel(&msg) / 100 * 5; 62 | report.timestamp = 63 | (uint32_t)(mavlink_msg_gps_raw_int_get_time_usec(&msg) / 1000); 64 | report.latitude = mavlink_msg_gps_raw_int_get_lat(&msg); 65 | report.longitude = mavlink_msg_gps_raw_int_get_lon(&msg); 66 | report.altitude = mavlink_msg_gps_raw_int_get_alt(&msg) / 1000; 67 | // report.eph = mavlink_msg_gps_raw_int_get_eph(&msg); 68 | // report.epv = mavlink_msg_gps_raw_int_get_epv(&msg); 69 | mask |= mavlink_msg_mask_gps_raw_int; 70 | return true; 71 | case MAVLINK_MSG_ID_SCALED_PRESSURE: // 29 72 | report.temperature_air = 73 | mavlink_msg_scaled_pressure_get_temperature(&msg) / 100; 74 | mask |= mavlink_msg_mask_scaled_pressure; 75 | return true; 76 | case MAVLINK_MSG_ID_ATTITUDE: // 30 77 | report.heading = rad_to_degrees(mavlink_msg_attitude_get_yaw(&msg)) / 2; 78 | // Use eph and epv fields to report roll and pitch. 79 | report.eph = rad_to_degrees(mavlink_msg_attitude_get_roll(&msg)) / 2; 80 | report.epv = rad_to_degrees(mavlink_msg_attitude_get_pitch(&msg)) / 2; 81 | mask |= mavlink_msg_mask_attitude; 82 | return true; 83 | case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: // 33 84 | // report.timestamp = 85 | // mavlink_msg_global_position_int_get_time_boot_ms(&msg); 86 | // report.latitude = mavlink_msg_global_position_int_get_lat(&msg); 87 | // report.longitude = mavlink_msg_global_position_int_get_lon(&msg); 88 | // report.altitude = mavlink_msg_global_position_int_get_alt(&msg) / 1000; 89 | report.target_altitude = 90 | mavlink_msg_global_position_int_get_relative_alt(&msg) / 1000; 91 | mask |= mavlink_msg_mask_global_position_int; 92 | return true; 93 | case MAVLINK_MSG_ID_MISSION_CURRENT: // 42 94 | report.wp_num = mavlink_msg_mission_current_get_seq(&msg); 95 | mask |= mavlink_msg_mask_mission_current; 96 | return true; 97 | case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: // 62 98 | report.target_distance = 99 | mavlink_msg_nav_controller_output_get_wp_dist(&msg); 100 | report.target_heading = 101 | mavlink_msg_nav_controller_output_get_target_bearing(&msg) / 2; 102 | mask |= mavlink_msg_mask_nav_controller_output; 103 | return true; 104 | case MAVLINK_MSG_ID_VFR_HUD: // 74 105 | report.airspeed = mavlink_msg_vfr_hud_get_airspeed(&msg) * 5; 106 | report.groundspeed = mavlink_msg_vfr_hud_get_groundspeed(&msg) * 5; 107 | report.climb_rate = mavlink_msg_vfr_hud_get_climb(&msg) * 10; 108 | report.throttle = mavlink_msg_vfr_hud_get_throttle(&msg); 109 | mask |= mavlink_msg_mask_vfr_hud; 110 | return true; 111 | case MAVLINK_MSG_ID_BATTERY2: // 147 112 | report.custom1 = mavlink_msg_battery2_get_voltage(&msg) / 1000; 113 | mask |= mavlink_msg_mask_battery2; 114 | return true; 115 | case MAVLINK_MSG_ID_WIND: // 168 116 | report.wind_heading = mavlink_msg_wind_get_direction(&msg) / 2; 117 | report.windspeed = mavlink_msg_wind_get_speed(&msg) * 5; 118 | mask |= mavlink_msg_mask_wind; 119 | return true; 120 | } 121 | 122 | return false; 123 | } 124 | 125 | void MAVReport::get_message(uint8_t channel_id, mavlink_message_t& msg, 126 | uint16_t& msg_mask) { 127 | report.custom2 = channel_id; 128 | msg_mask = mask; 129 | mavlink_msg_high_latency2_encode(sysid, compid, &msg, &report); 130 | } 131 | 132 | uint16_t MAVReport::get_custom_mode(uint8_t base_mode, 133 | uint32_t custom_mode) const { 134 | return base_mode | ((custom_mode & 0xFF) << 8); 135 | } 136 | 137 | uint16_t MAVReport::get_failure_flags(uint32_t sensors_health) const { 138 | uint16_t failure_flags = 0; 139 | 140 | if (sensors_health & MAV_SYS_STATUS_SENSOR_GPS) { 141 | failure_flags |= HL_FAILURE_FLAG_GPS; 142 | } 143 | 144 | if (sensors_health & MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE) { 145 | failure_flags |= HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE; 146 | } 147 | 148 | if (sensors_health & MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE) { 149 | failure_flags |= HL_FAILURE_FLAG_ABSOLUTE_PRESSURE; 150 | } 151 | 152 | if (sensors_health & MAV_SYS_STATUS_SENSOR_3D_ACCEL) { 153 | failure_flags |= HL_FAILURE_FLAG_3D_ACCEL; 154 | } 155 | 156 | if (sensors_health & MAV_SYS_STATUS_SENSOR_3D_GYRO) { 157 | failure_flags |= HL_FAILURE_FLAG_3D_GYRO; 158 | } 159 | 160 | if (sensors_health & MAV_SYS_STATUS_SENSOR_3D_MAG) { 161 | failure_flags |= HL_FAILURE_FLAG_3D_MAG; 162 | } 163 | 164 | if (sensors_health & MAV_SYS_STATUS_TERRAIN) { 165 | failure_flags |= HL_FAILURE_FLAG_TERRAIN; 166 | } 167 | 168 | if (sensors_health & MAV_SYS_STATUS_SENSOR_BATTERY) { 169 | failure_flags |= HL_FAILURE_FLAG_BATTERY; 170 | } 171 | 172 | if (sensors_health & MAV_SYS_STATUS_SENSOR_RC_RECEIVER) { 173 | failure_flags |= HL_FAILURE_FLAG_RC_RECEIVER; 174 | } 175 | 176 | if (sensors_health & MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS) { 177 | failure_flags |= HL_FAILURE_FLAG_ENGINE; 178 | } 179 | 180 | if (sensors_health & MAV_SYS_STATUS_GEOFENCE) { 181 | failure_flags |= HL_FAILURE_FLAG_GEOFENCE; 182 | } 183 | 184 | return failure_flags; 185 | } 186 | 187 | } // namespace radioroom 188 | 189 | -------------------------------------------------------------------------------- /src/MAVReport.h: -------------------------------------------------------------------------------- 1 | /* 2 | MAVReport.h 3 | 4 | Telemetry for MAVLink autopilots. 5 | 6 | (C) Copyright 2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | #ifndef SRC_MAVREPORT_H_ 23 | #define SRC_MAVREPORT_H_ 24 | 25 | #include "MAVLinkLib.h" 26 | 27 | namespace radioroom { 28 | 29 | // Masks of MAVLink messages used to compose single HIGH_LATENCY2 message 30 | constexpr uint16_t mavlink_msg_mask_heartbeat = 0x0001; 31 | constexpr uint16_t mavlink_msg_mask_sys_status = 0x0002; 32 | constexpr uint16_t mavlink_msg_mask_gps_raw_int = 0x0004; 33 | constexpr uint16_t mavlink_msg_mask_attitude = 0x0008; 34 | constexpr uint16_t mavlink_msg_mask_global_position_int = 0x0010; 35 | constexpr uint16_t mavlink_msg_mask_mission_current = 0x0020; 36 | constexpr uint16_t mavlink_msg_mask_nav_controller_output = 0x0040; 37 | constexpr uint16_t mavlink_msg_mask_vfr_hud = 0x0080; 38 | constexpr uint16_t mavlink_msg_mask_battery2 = 0x0100; // optional 39 | constexpr uint16_t mavlink_msg_mask_wind = 0x0200; // optional 40 | constexpr uint16_t mavlink_msg_mask_scaled_pressure = 0x0400; // optional 41 | 42 | // Class used to update and retrieve vehicle state report. 43 | class MAVReport { 44 | public: 45 | MAVReport(); 46 | 47 | // Integrates the specified message into report message of HIGH_LATENCY2 type. 48 | // 49 | // Returns true if the message was integrated. 50 | bool update(const mavlink_message_t& msg); 51 | 52 | // Retrieves HIGH_LATENCY2 report message. 53 | // Set custom2 HIGH_LATENCY2.field to the channel_id value. 54 | // msg_mask identifies MAVLink messages used to compose HIGH_LATENCY2 message. 55 | void get_message(uint8_t channel_id, mavlink_message_t& msg, 56 | uint16_t& msg_mask); 57 | 58 | private: 59 | // Constructs 2 byte HIGH_LATENCY2.custom_mode from HEARTBEAT.base_mode and 60 | // HEARTBEAT.custom_mode. 61 | uint16_t get_custom_mode(uint8_t base_mode, uint32_t custom_mode) const; 62 | 63 | // Constructs HIGH_LATENCY2.failure_flags bitmap from 64 | // SYS_STATUS.onboard_control_sensors_health 65 | uint16_t get_failure_flags(uint32_t sensors_health) const; 66 | 67 | mavlink_high_latency2_t report; 68 | uint8_t sysid; 69 | uint8_t compid; 70 | uint16_t mask; 71 | }; 72 | 73 | } // namespace radioroom 74 | 75 | #endif // SRC_MAVREPORT_H_ 76 | -------------------------------------------------------------------------------- /src/build.h.in: -------------------------------------------------------------------------------- 1 | #ifndef VERSION_H_ 2 | #define VERSION_H_ 3 | 4 | #define RADIO_ROOM_VERSION "UV Radio Room @PROJECT_VERSION@" 5 | #define BUILD_NUM "@BUILD_NUM@" 6 | 7 | #endif /* VERSION_H_ */ 8 | -------------------------------------------------------------------------------- /src/radioroom.cc: -------------------------------------------------------------------------------- 1 | /* 2 | radioroom.cpp 3 | 4 | Telemetry for MAVLink autopilots. 5 | 6 | (C) Copyright 2018-2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | #include "Config.h" 31 | #include "MAVLinkHandler.h" 32 | #include "MAVLinkLogger.h" 33 | #include "build.h" 34 | #include "timelib.h" 35 | 36 | using mavio::log; 37 | using radioroom::MAVLinkHandler; 38 | using std::cout; 39 | using std::endl; 40 | 41 | constexpr char log_identity[] = "radioroom"; 42 | 43 | const std::chrono::milliseconds msg_handler_loop_period(10); 44 | 45 | std::atomic running(false); 46 | 47 | void print_help() { 48 | cout << "Usage: radioroom [options]" << endl; 49 | cout << "options:" << endl; 50 | cout << " -c " << endl; 51 | cout << " Alternative configuration file instead of " 52 | "/etc/radioroom.conf." 53 | << endl; 54 | cout << endl; 55 | cout << " -h Print this help and exit." << endl; 56 | cout << endl; 57 | cout << " -v Verbose logging." << endl; 58 | cout << endl; 59 | cout << " -V Print version and exit." << endl; 60 | } 61 | 62 | void print_version() { 63 | cout << RADIO_ROOM_VERSION << "." << BUILD_NUM << endl; 64 | cout << "MAVLink wire protocol version " << MAVLINK_WIRE_PROTOCOL_VERSION 65 | << endl; 66 | } 67 | 68 | void handle_signal(int sig) { 69 | if (sig == SIGTERM) { 70 | running = false; 71 | 72 | /* Reset signal handling to default behavior */ 73 | signal(SIGTERM, SIG_DFL); 74 | } 75 | } 76 | 77 | int main(int argc, char** argv) { 78 | MAVLinkHandler msg_handler; 79 | std::string config_file = radioroom::default_config_file; 80 | 81 | int c; 82 | while ((c = getopt(argc, argv, "c:hvV")) != -1) { 83 | switch (c) { 84 | case 'c': 85 | config_file = optarg; 86 | break; 87 | case 'h': 88 | print_help(); 89 | return EXIT_SUCCESS; 90 | case 'v': 91 | radioroom::config.set_debug_mode(true); 92 | break; 93 | case 'V': 94 | print_version(); 95 | return EXIT_SUCCESS; 96 | case '?': 97 | if (optopt == 'c') { 98 | cout << "Option -c requires an argument." << endl; 99 | } else if (isprint(optopt)) { 100 | cout << "Unknown option '-" << std::string(1, optopt) << "'." << endl; 101 | } else { 102 | cout << "Unknown option character '" << std::string(1, optopt) << "'." 103 | << endl; 104 | } 105 | return EXIT_FAILURE; 106 | } 107 | } 108 | 109 | mavio::openlog(log_identity, radioroom::config.get_debug_mode() 110 | ? LOG_UPTO(LOG_DEBUG) 111 | : LOG_UPTO(LOG_INFO)); 112 | 113 | log(LOG_INFO, "Starting %s.%s...", RADIO_ROOM_VERSION, BUILD_NUM); 114 | 115 | if (radioroom::config.init(config_file) < 0) { 116 | log(LOG_ERR, "Can't load configuration file '%s'", config_file.data()); 117 | } 118 | 119 | if (msg_handler.init()) { 120 | log(LOG_NOTICE, "%s.%s started.", RADIO_ROOM_VERSION, BUILD_NUM); 121 | } else { 122 | log(LOG_CRIT, "%s.%s initialization failed.", RADIO_ROOM_VERSION, 123 | BUILD_NUM); 124 | return EXIT_FAILURE; 125 | } 126 | 127 | signal(SIGTERM, handle_signal); 128 | 129 | running = true; 130 | 131 | while (running) { 132 | msg_handler.loop(); 133 | timelib::sleep(msg_handler_loop_period); 134 | } 135 | 136 | log(LOG_INFO, "Stopping %s.%s...", RADIO_ROOM_VERSION, BUILD_NUM); 137 | 138 | msg_handler.close(); 139 | 140 | log(LOG_NOTICE, "%s.%s stopped.", RADIO_ROOM_VERSION, BUILD_NUM); 141 | 142 | mavio::closelog(); 143 | 144 | return EXIT_SUCCESS; 145 | } 146 | -------------------------------------------------------------------------------- /tests/CircularBufferTest.cc: -------------------------------------------------------------------------------- 1 | /* 2 | CircularBufferTest.h 3 | 4 | MAVIO MAVLink I/O library 5 | 6 | (C) Copyright 2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | #include 23 | #include 24 | 25 | #include "CircularBuffer.h" 26 | #include "MAVLinkLib.h" 27 | 28 | using std::cout; 29 | using std::endl; 30 | 31 | constexpr size_t max_queue_size = 10; 32 | 33 | void get_test_message(int i, mavlink_message_t* msg) { 34 | mavlink_heartbeat_t heartbeat; 35 | heartbeat.autopilot = MAV_AUTOPILOT_GENERIC; 36 | heartbeat.base_mode = 1; 37 | heartbeat.custom_mode = 0; 38 | heartbeat.mavlink_version = MAVLINK_VERSION; 39 | heartbeat.system_status = 0; 40 | heartbeat.type = MAV_TYPE_GENERIC; 41 | 42 | mavlink_msg_heartbeat_encode(255, i, msg, &heartbeat); 43 | } 44 | 45 | int main() { 46 | cout << "CircularBuffer test started." << endl; 47 | 48 | mavio::CircularBuffer queue(max_queue_size); 49 | 50 | mavlink_message_t msg; 51 | get_test_message(0, &msg); 52 | queue.push(msg); 53 | 54 | queue.reset(); 55 | 56 | assert(queue.size() == 0); 57 | 58 | cout << "Queue size:" << static_cast(queue.size()) 59 | << ", Queue capacity:" << queue.capacity() << endl; 60 | 61 | assert(queue.size() == 0); 62 | assert(queue.capacity() == max_queue_size); 63 | 64 | cout << "Pushing messages..." << endl; 65 | 66 | for (uint8_t i = 0; i < 100; ++i) { 67 | get_test_message(i, &msg); 68 | queue.push(msg); 69 | 70 | cout << "i : " << static_cast(i) << ", full : " << queue.full() << endl; 71 | } 72 | 73 | assert(queue.size() == 10); 74 | 75 | cout << "Retrieving messages..." << endl; 76 | 77 | while (!queue.empty()) { 78 | queue.pop(msg); 79 | cout << "Message retrieved: " << static_cast(msg.compid) 80 | << ", Queue size: " << static_cast(queue.size()) << endl; 81 | } 82 | 83 | assert(queue.size() == 0); 84 | 85 | cout << "CircularBuffer test completed." << endl; 86 | 87 | return 0; 88 | } 89 | -------------------------------------------------------------------------------- /tests/CustomSerializationTest.cc: -------------------------------------------------------------------------------- 1 | /* 2 | CustomSerializationTest.h 3 | 4 | MAVIO MAVLink I/O library 5 | 6 | (C) Copyright 2021 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | #include "MAVLinkLib.h" 23 | 24 | #include 25 | #include 26 | 27 | using std::cout; 28 | using std::endl; 29 | 30 | namespace mavio { 31 | // Custom serialization/deerialization helpers 32 | uint16_t message_to_send_buffer(uint8_t *buf, const mavlink_message_t *msg); 33 | bool parse_message(const uint8_t *buf, size_t buf_size, mavlink_message_t *msg); 34 | } // namespace mavio 35 | 36 | int main() { 37 | cout << "Custom serialization test started." << endl; 38 | 39 | // Test MAVLink 2.0 serialization using HIGH_LATENCY2 message 40 | mavlink_high_latency2_t high_latency2; 41 | memset(&high_latency2, 0, sizeof(high_latency2)); 42 | high_latency2.timestamp = 1; 43 | high_latency2.airspeed = 1; 44 | high_latency2.custom2 = 1; 45 | 46 | uint8_t buf[MAVLINK_MAX_PACKET_LEN]; 47 | mavlink_message_t msg; 48 | memset(&msg, 0, sizeof(mavlink_message_t)); 49 | mavlink_msg_high_latency2_encode(1, 0, &msg, &high_latency2); 50 | 51 | uint8_t len = mavlink_msg_to_send_buffer(buf, &msg); 52 | 53 | cout << "Original message length: " << (long)len << endl; 54 | 55 | len = mavio::message_to_send_buffer(buf, &msg); 56 | 57 | cout << "Custom message length: " << (long)len << endl; 58 | 59 | mavlink_message_t parsed_msg; 60 | memset(&parsed_msg, 0, sizeof(mavlink_message_t)); 61 | if (mavio::parse_message(buf, len, &parsed_msg)) { 62 | assert(msg.checksum == parsed_msg.checksum); 63 | assert(memcmp(&msg.payload64, &parsed_msg.payload64, msg.len) == 0); 64 | 65 | mavlink_high_latency2_t high_latency2_parsed; 66 | memset(&high_latency2_parsed, 0, sizeof(high_latency2_parsed)); 67 | mavlink_msg_high_latency2_decode(&parsed_msg, &high_latency2_parsed); 68 | assert(memcmp(&high_latency2_parsed, &high_latency2, 69 | sizeof(high_latency2)) == 0); 70 | } else { 71 | cout << "Failed to parse MAVLink message." << endl; 72 | return 1; 73 | } 74 | 75 | // Test MAVLink 1.0 Serialization using HEARTBEAT message. 76 | mavlink_heartbeat_t heartbeat; 77 | heartbeat.custom_mode = 1; 78 | heartbeat.type = 0; 79 | heartbeat.autopilot = 0; 80 | heartbeat.base_mode = 0; 81 | heartbeat.system_status = 0; 82 | heartbeat.mavlink_version = 1; 83 | 84 | mavlink_message_t tmp_msg; 85 | mavlink_msg_heartbeat_encode(1, 0, &tmp_msg, &heartbeat); 86 | 87 | memset(&msg, 0, sizeof(mavlink_message_t)); 88 | msg.checksum = 52057; 89 | msg.magic = MAVLINK_STX_MAVLINK1; 90 | msg.len = MAVLINK_MSG_ID_HEARTBEAT_LEN; 91 | msg.sysid = 1; 92 | msg.compid = 0; 93 | msg.msgid = MAVLINK_MSG_ID_HEARTBEAT; 94 | memcpy(_MAV_PAYLOAD_NON_CONST(&msg), _MAV_PAYLOAD(&tmp_msg), msg.len); 95 | 96 | 97 | len = mavio::message_to_send_buffer(buf, &msg); 98 | memset(&parsed_msg, 0, sizeof(mavlink_message_t)); 99 | 100 | if (mavio::parse_message(buf, len, &parsed_msg)) { 101 | assert(msg.checksum == parsed_msg.checksum); 102 | assert(memcmp(&msg.payload64, &parsed_msg.payload64, msg.len) == 0); 103 | } else { 104 | cout << "Failed to parse MAVLink message." << endl; 105 | return 1; 106 | } 107 | 108 | cout << "Custom serialization test succeeded." << endl; 109 | return 0; 110 | } 111 | -------------------------------------------------------------------------------- /tests/MAVLinkAutopilotTest.cc: -------------------------------------------------------------------------------- 1 | /* 2 | MAVLinkAutopilotTest.cc 3 | 4 | MAVIO MAVLink I/O library 5 | 6 | (C) Copyright 2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | 23 | #include 24 | #include 25 | 26 | #include "Logger.h" 27 | #include "MAVLinkAutopilot.h" 28 | #include "timelib.h" 29 | 30 | using std::cout; 31 | using std::endl; 32 | 33 | using mavio::MAVLinkAutopilot; 34 | using mavio::Serial; 35 | using timelib::sec2ms; 36 | using timelib::sleep; 37 | using timelib::Stopwatch; 38 | 39 | void request_data_streams(MAVLinkAutopilot& autopilot, uint16_t rate); 40 | 41 | constexpr char default_autopilot_serial[] = "/dev/ttyusb0"; 42 | constexpr int autopilot_serial_baud_rate = 57600; 43 | 44 | constexpr uint16_t DATA_STREAM_RATE = 2; // Hz 45 | 46 | const std::chrono::milliseconds autopilot_send_interval(10); 47 | 48 | int main(int argc, char** argv) { 49 | cout << "MAVLinkAutopilot class test." << endl; 50 | cout << "Usage: aptest " << endl; 51 | 52 | for (int i = 0; i < argc; i++) { 53 | cout << argv[i] << endl; 54 | } 55 | 56 | mavio::openlog("aptest", LOG_UPTO(LOG_DEBUG)); 57 | 58 | MAVLinkAutopilot autopilot; 59 | 60 | std::string path = default_autopilot_serial; 61 | int speed = autopilot_serial_baud_rate; 62 | std::vector devices; 63 | Serial::get_serial_devices(devices); 64 | 65 | cout << "Serial devices:" << endl; 66 | 67 | for (std::string dev : devices) { 68 | cout << dev << endl; 69 | } 70 | 71 | if (argc > 1) { 72 | path = argv[1]; 73 | } 74 | 75 | if (!autopilot.init(path, speed, devices)) { 76 | cout << "MAVLinkAutopilot.init() failed." << endl; 77 | return 1; 78 | } 79 | 80 | cout << "MAVLinkAutopilot.get_system_id() returned " 81 | << static_cast(autopilot.get_system_id()) << endl; 82 | 83 | request_data_streams(autopilot, DATA_STREAM_RATE); 84 | 85 | Stopwatch start_time; 86 | Stopwatch heartbeat_time; 87 | 88 | int count = 0; 89 | const std::chrono::milliseconds period = sec2ms(10.0); 90 | const std::chrono::milliseconds heartbeat_interval = sec2ms(1.0); 91 | 92 | while (start_time.elapsed_time() <= period) { 93 | mavlink_message_t msg; 94 | if (autopilot.receive_message(msg)) { 95 | cout << "Message reseived. msgid = " << static_cast(msg.msgid) 96 | << endl; 97 | count++; 98 | } 99 | 100 | if (heartbeat_time.elapsed_time() >= heartbeat_interval) { 101 | heartbeat_time.reset(); 102 | 103 | mavlink_message_t mt_msg; 104 | mavlink_msg_heartbeat_pack(mavio::gcs_system_id, mavio::gcs_component_id, 105 | &mt_msg, MAV_TYPE_GCS, MAV_AUTOPILOT_INVALID, 106 | 0, 0, 0); 107 | autopilot.send_message(mt_msg); 108 | } 109 | 110 | sleep(autopilot_send_interval); 111 | } 112 | 113 | cout << count << " messages received in " << period.count() << " ms." << endl; 114 | 115 | autopilot.close(); 116 | 117 | cout << "MAVLinkAutopilot class test completed." << endl; 118 | 119 | mavio::closelog(); 120 | 121 | return 0; 122 | } 123 | 124 | void request_data_streams(MAVLinkAutopilot& autopilot, uint16_t rate) { 125 | mavlink_message_t mt_msg; 126 | 127 | /* 128 | * Send a heartbeat first 129 | */ 130 | mavlink_msg_heartbeat_pack(mavio::gcs_system_id, mavio::gcs_component_id, 131 | &mt_msg, MAV_TYPE_GCS, MAV_AUTOPILOT_INVALID, 0, 0, 132 | 0); 133 | autopilot.send_message(mt_msg); 134 | 135 | /* 136 | * Request data streams from the autopilot. 137 | */ 138 | uint8_t req_stream_ids[] = { 139 | MAV_DATA_STREAM_EXTENDED_STATUS, // SYS_STATUS, NAV_CONTROLLER_OUTPUT, 140 | // GPS_RAW, MISSION_CURRENT 141 | MAV_DATA_STREAM_POSITION, // GLOBAL_POSITION_INT 142 | // MAV_DATA_STREAM_RAW_CONTROLLER, 143 | MAV_DATA_STREAM_RAW_SENSORS, // 144 | MAV_DATA_STREAM_EXTRA1, // ATTITUDE 145 | MAV_DATA_STREAM_EXTRA2, // VFR_HUD 146 | MAV_DATA_STREAM_EXTRA3 // MSG_BATTERY2 147 | }; 148 | 149 | constexpr size_t n = sizeof(req_stream_ids) / sizeof(req_stream_ids[0]); 150 | 151 | for (size_t i = 0; i < n; ++i) { 152 | mavlink_msg_request_data_stream_pack( 153 | mavio::gcs_system_id, mavio::gcs_component_id, &mt_msg, 154 | autopilot.get_system_id(), mavio::ardupilot_component_id, 155 | req_stream_ids[i], rate, 1); 156 | autopilot.send_message(mt_msg); 157 | 158 | sleep(autopilot_send_interval); 159 | } 160 | } 161 | -------------------------------------------------------------------------------- /tests/MAVLinkISBDChannelTest.cc: -------------------------------------------------------------------------------- 1 | /* 2 | MAVLinkISBDChannelTest.cc 3 | 4 | MAVIO MAVLink I/O library 5 | 6 | (C) Copyright 2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | #include 23 | 24 | #include "MAVLinkISBDChannel.h" 25 | #include "Serial.h" 26 | #include "timelib.h" 27 | #include "Logger.h" 28 | 29 | using std::cout; 30 | using std::endl; 31 | 32 | using mavio::MAVLinkISBDChannel; 33 | using mavio::Serial; 34 | 35 | constexpr int isbd_serial_baud_rate = 19200; 36 | constexpr int max_test_time = 600; // seconds 37 | 38 | /** 39 | * Tests sending message to ISBD transceiver. 40 | * 41 | * Returns 0 if the test was successful. 42 | */ 43 | int test_send(MAVLinkISBDChannel& isbd_channel) { 44 | mavlink_high_latency_t report; 45 | memset(&report, 0, sizeof(mavlink_high_latency_t)); 46 | 47 | mavlink_message_t msg; 48 | uint8_t sysid = 1; 49 | uint8_t compid = 0; 50 | mavlink_msg_high_latency_encode(sysid, compid, &msg, &report); 51 | 52 | std::chrono::milliseconds send_time = isbd_channel.last_send_time(); 53 | 54 | isbd_channel.send_message(msg); 55 | 56 | for (int i = 0; i < max_test_time; i++) { 57 | int quality = 0; 58 | isbd_channel.get_signal_quality(quality); 59 | mavio::log(LOG_INFO, "Signal quality: %d", quality); 60 | 61 | if (isbd_channel.last_send_time() != send_time) { 62 | mavio::log(LOG_INFO, "Message sent."); 63 | return 0; 64 | } 65 | 66 | timelib::sleep(timelib::sec2ms(1.0)); 67 | } 68 | 69 | mavio::log(LOG_INFO, "Message was not sent."); 70 | return 1; 71 | } 72 | 73 | /** 74 | * Tests receiving message from ISBD transceiver. 75 | * A message must be sent to the transceiver before the test. 76 | * 77 | * Returns 0 if the test was successful. 78 | */ 79 | int test_receive(MAVLinkISBDChannel& isbd_channel) { 80 | mavlink_message_t msg; 81 | 82 | for (int i = 0; i < max_test_time; i++) { 83 | int quality = 0; 84 | isbd_channel.get_signal_quality(quality); 85 | mavio::log(LOG_INFO, "Signal quality: %d", quality); 86 | 87 | if (isbd_channel.message_available()) { 88 | if (isbd_channel.receive_message(msg)) { 89 | mavio::log(LOG_INFO, "Message received. msgid = %d", msg.msgid); 90 | return 0; 91 | } 92 | } 93 | 94 | timelib::sleep(timelib::sec2ms(1.0)); 95 | } 96 | 97 | return 1; 98 | } 99 | 100 | int main(int argc, char** argv) { 101 | cout << "MAVLinkISBDChannel class test." << endl; 102 | 103 | cout << "Usage: isbdtest " << endl; 104 | 105 | std::string path = "/dev/ttyUSB0"; 106 | if (argc > 1) { 107 | path = argv[1]; 108 | } 109 | 110 | std::string test = "send"; 111 | if (argc > 2) { 112 | test = argv[2]; 113 | } 114 | 115 | std::vector devices; 116 | Serial::get_serial_devices(devices); 117 | 118 | cout << "Device: " << path << endl; 119 | cout << "Test: " << test << endl; 120 | 121 | MAVLinkISBDChannel isbd_channel; 122 | 123 | int ret = 0; 124 | 125 | if (isbd_channel.init(path, isbd_serial_baud_rate, devices)) { 126 | cout << "ISBD channel init() succeeded." << endl; 127 | 128 | if (test == "send") { 129 | ret = test_send(isbd_channel); 130 | } else if (test == "receive") { 131 | ret = test_receive(isbd_channel); 132 | } else { 133 | cout << "Invalid test." << endl; 134 | ret = 1; 135 | } 136 | 137 | isbd_channel.close(); 138 | } else { 139 | cout << "ISBD channel init() failed." << endl; 140 | ret = 1; 141 | } 142 | 143 | return ret; 144 | } 145 | -------------------------------------------------------------------------------- /tests/MAVLinkTCPChannelTest.cc: -------------------------------------------------------------------------------- 1 | /* 2 | MAVLinkTCPChannelTest.cc 3 | 4 | MAVIO MAVLink I/O library 5 | 6 | (C) Copyright 2019 Envirover. 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | #include 23 | 24 | #include "MAVLinkTCPChannel.h" 25 | 26 | using std::cout; 27 | using std::endl; 28 | 29 | using mavio::MAVLinkTCPChannel; 30 | 31 | int main(int argc, char** argv) { 32 | cout << "MAVLinkTCPChannel class test." << endl; 33 | 34 | for (int i = 0; i < argc; i++) { 35 | cout << argv[i] << endl; 36 | } 37 | 38 | if (argc < 3) { 39 | cout << "Usage: tcptest " << endl; 40 | return 1; 41 | } 42 | 43 | std::string host = argv[1]; 44 | uint16_t port = atoi(argv[2]); 45 | 46 | cout << "Testing TCP channel " << host << ":" << port << "..." << endl; 47 | 48 | MAVLinkTCPChannel tcp_channel; 49 | 50 | if (tcp_channel.init(host, port)) { 51 | cout << "TCP channel init() succeeded." << endl; 52 | tcp_channel.close(); 53 | } else { 54 | cout << "TCP channel init() failed." << endl; 55 | return 1; 56 | } 57 | 58 | return 0; 59 | } 60 | -------------------------------------------------------------------------------- /tests/TimeLibTest.cc: -------------------------------------------------------------------------------- 1 | /* 2 | TimeLibTest.cc 3 | 4 | TimeLib library test. 5 | 6 | Copyright (C) 2019 Envirover 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public 19 | License along with this library; if not, write to the Free Software 20 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | */ 22 | 23 | #include 24 | 25 | #include "timelib.h" 26 | 27 | using std::cout; 28 | using std::endl; 29 | 30 | using timelib::sec2ms; 31 | using timelib::sleep; 32 | using timelib::Stopwatch; 33 | using timelib::time_since_epoch; 34 | using timelib::timestamp; 35 | 36 | int main() { 37 | cout << "TimeLib test started." << endl; 38 | std::chrono::milliseconds start_time = time_since_epoch(); 39 | cout << "Time since epoch = " << start_time.count() << " ms" << endl; 40 | 41 | std::chrono::milliseconds interval = sec2ms(1.0); 42 | 43 | while (interval.count() > 0) { 44 | Stopwatch timer; 45 | sleep(interval); 46 | std::chrono::milliseconds elapsed_time = timer.elapsed_time(); 47 | char str[32]; 48 | timestamp(str, sizeof(str)); 49 | cout << str << " "; 50 | cout << "sleep time = " << interval.count() << " ms, " 51 | << "elapsed time = " << elapsed_time.count() << " ms, " 52 | << "difference = " << (elapsed_time - interval).count() << " ms" 53 | << endl; 54 | interval /= 2; 55 | } 56 | 57 | std::chrono::milliseconds end_time = time_since_epoch(); 58 | cout << "Total test time = " << (end_time - start_time).count() << " ms" 59 | << endl; 60 | cout << "TimeLib test completed." << endl; 61 | 62 | return 0; 63 | } 64 | -------------------------------------------------------------------------------- /toolchain-arm-linux.cmake: -------------------------------------------------------------------------------- 1 | set(CMAKE_SYSTEM_NAME raspbian) 2 | 3 | set(CMAKE_C_COMPILER arm-linux-gnueabihf-gcc) 4 | set(CMAKE_CXX_COMPILER arm-linux-gnueabihf-g++) 5 | 6 | set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) 7 | set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) 8 | set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) 9 | set(CMAKE_FIND_ROOT_PATH_MODE_PACKAGE ONLY) 10 | 11 | set(CPACK_DEBIAN_PACKAGE_ARCHITECTURE "armhf") -------------------------------------------------------------------------------- /toolchain-arm-windows.cmake: -------------------------------------------------------------------------------- 1 | set(CMAKE_SYSTEM_NAME raspbian) 2 | 3 | set(CMAKE_C_COMPILER arm-linux-gnueabihf-gcc.exe) 4 | set(CMAKE_CXX_COMPILER arm-linux-gnueabihf-g++.exe) 5 | 6 | set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) 7 | set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) 8 | set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) 9 | set(CMAKE_FIND_ROOT_PATH_MODE_PACKAGE ONLY) -------------------------------------------------------------------------------- /toolchain-jetson.cmake: -------------------------------------------------------------------------------- 1 | set(CMAKE_SYSTEM_NAME jetson) 2 | 3 | set(CMAKE_C_COMPILER aarch64-linux-gnu-gcc) 4 | set(CMAKE_CXX_COMPILER aarch64-linux-gnu-g++) 5 | 6 | set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) 7 | set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) 8 | set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) 9 | set(CMAKE_FIND_ROOT_PATH_MODE_PACKAGE ONLY) 10 | 11 | set(CPACK_DEBIAN_PACKAGE_ARCHITECTURE "arm64") -------------------------------------------------------------------------------- /toolchain-x64-windows.cmake: -------------------------------------------------------------------------------- 1 | #set(CMAKE_C_COMPILER gcc.exe) 2 | #set(CMAKE_CXX_COMPILER g++.exe) 3 | #set(CMAKE_GENERATOR "Unix Makefiles") 4 | 5 | #set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) 6 | #set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) 7 | #set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) 8 | #set(CMAKE_FIND_ROOT_PATH_MODE_PACKAGE ONLY) 9 | 10 | add_compile_options(-g) --------------------------------------------------------------------------------