├── .github └── ISSUE_TEMPLATE ├── .gitignore ├── .travis.yml ├── .ycm_extra_conf.py ├── CONTRIBUTING.md ├── LICENSE-BSD.txt ├── LICENSE-GPLv3.txt ├── LICENSE-LGPLv3.txt ├── LICENSE.md ├── README.md ├── dependencies.rosinstall ├── libmavconn ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── cmake │ ├── Modules │ │ ├── CheckGeographicLibDatasets.cmake │ │ ├── EnableCXX11.cmake │ │ ├── FindGeographicLib.cmake │ │ └── MavrosMavlink.cmake │ ├── libmavconn-extras.cmake.develspace.in │ └── libmavconn-extras.cmake.installspace.in ├── include │ └── mavconn │ │ ├── console_bridge_compat.h │ │ ├── interface.h │ │ ├── mavlink_dialect.h.em │ │ ├── msgbuffer.h │ │ ├── serial.h │ │ ├── tcp.h │ │ ├── thread_utils.h │ │ └── udp.h ├── mavlink.context.py.in ├── package.xml ├── rosdoc.yaml ├── src │ ├── interface.cpp │ ├── mavlink_helpers.cpp.em │ ├── serial.cpp │ ├── tcp.cpp │ └── udp.cpp └── test │ ├── test_hang.cpp │ └── test_mavconn.cpp ├── mavros ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── mavros │ │ ├── frame_tf.h │ │ ├── mavlink_diag.h │ │ ├── mavros.h │ │ ├── mavros_plugin.h │ │ ├── mavros_uas.h │ │ ├── px4_custom_mode.h │ │ ├── setpoint_mixin.h │ │ └── utils.h ├── launch │ ├── apm.launch │ ├── apm2.launch │ ├── apm_config.yaml │ ├── apm_pluginlists.yaml │ ├── event_launcher.yaml │ ├── mavlink_bridge.launch │ ├── node.launch │ ├── px4.launch │ ├── px4_config.yaml │ └── px4_pluginlists.yaml ├── mavros_plugins.xml ├── package.xml ├── rosdoc.yaml ├── scripts │ ├── checkid │ ├── event_launcher │ ├── install_geographiclib_datasets.sh │ ├── mavcmd │ ├── mavftp │ ├── mavparam │ ├── mavsafety │ ├── mavsetp │ ├── mavsys │ └── mavwp ├── setup.py ├── src │ ├── gcs_bridge.cpp │ ├── lib │ │ ├── enum_sensor_orientation.cpp │ │ ├── enum_to_string.cpp │ │ ├── ftf_frame_conversions.cpp │ │ ├── ftf_quaternion_utils.cpp │ │ ├── mavlink_diag.cpp │ │ ├── mavros.cpp │ │ ├── rosconsole_bridge.cpp │ │ ├── uas_data.cpp │ │ ├── uas_stringify.cpp │ │ └── uas_timesync.cpp │ ├── mavros │ │ ├── __init__.py │ │ ├── command.py │ │ ├── event_launcher.py │ │ ├── ftp.py │ │ ├── mavlink.py │ │ ├── mission.py │ │ ├── nuttx_crc32.py │ │ ├── param.py │ │ ├── setpoint.py │ │ └── utils.py │ ├── mavros_node.cpp │ └── plugins │ │ ├── 3dr_radio.cpp │ │ ├── actuator_control.cpp │ │ ├── altitude.cpp │ │ ├── command.cpp │ │ ├── dummy.cpp │ │ ├── ftp.cpp │ │ ├── global_position.cpp │ │ ├── hil.cpp │ │ ├── home_position.cpp │ │ ├── imu.cpp │ │ ├── local_position.cpp │ │ ├── manual_control.cpp │ │ ├── param.cpp │ │ ├── rc_io.cpp │ │ ├── safety_area.cpp │ │ ├── setpoint_accel.cpp │ │ ├── setpoint_attitude.cpp │ │ ├── setpoint_position.cpp │ │ ├── setpoint_raw.cpp │ │ ├── setpoint_trajectory.cpp │ │ ├── setpoint_velocity.cpp │ │ ├── sys_status.cpp │ │ ├── sys_time.cpp │ │ ├── vfr_hud.cpp │ │ ├── waypoint.cpp │ │ └── wind_estimation.cpp ├── test │ ├── test_frame_conversions.cpp │ ├── test_quaternion_utils.cpp │ └── test_sensor_orientation.cpp └── tools │ ├── cogall.sh │ └── uncrustify-cpp.cfg ├── mavros_cog.py ├── mavros_extras ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── launch │ ├── f710_joy.yaml │ ├── px4_image.launch │ ├── px4flow.launch │ ├── px4flow_config.yaml │ ├── px4flow_pluginlists.yaml │ ├── servo_state_publisher.yaml │ └── teleop.launch ├── mavros_plugins.xml ├── package.xml ├── rosdoc.yaml ├── scripts │ ├── mavftpfuse │ └── mavteleop └── src │ ├── plugins │ ├── adsb.cpp │ ├── cam_imu_sync.cpp │ ├── companion_process_status.cpp │ ├── debug_value.cpp │ ├── distance_sensor.cpp │ ├── fake_gps.cpp │ ├── gps_rtk.cpp │ ├── landing_target.cpp │ ├── log_transfer.cpp │ ├── mocap_pose_estimate.cpp │ ├── mount_control.cpp │ ├── obstacle_distance.cpp │ ├── odom.cpp │ ├── onboard_computer_status.cpp │ ├── px4flow.cpp │ ├── rangefinder.cpp │ ├── trajectory.cpp │ ├── vibration.cpp │ ├── vision_pose_estimate.cpp │ ├── vision_speed_estimate.cpp │ └── wheel_odometry.cpp │ ├── servo_state_publisher.cpp │ └── visualization.cpp ├── mavros_msgs ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── mavros_msgs │ │ └── mavlink_convert.h ├── msg │ ├── ADSBVehicle.msg │ ├── ActuatorControl.msg │ ├── AllocationMatrix.msg │ ├── Altitude.msg │ ├── AttitudeTarget.msg │ ├── AttitudeThrustTarget.msg │ ├── BatteryStatus.msg │ ├── CamIMUStamp.msg │ ├── CommandCode.msg │ ├── CompanionProcessStatus.msg │ ├── DebugValue.msg │ ├── DynamixelStatus.msg │ ├── ExtendedState.msg │ ├── FileEntry.msg │ ├── GlobalPositionTarget.msg │ ├── HilActuatorControls.msg │ ├── HilControls.msg │ ├── HilGPS.msg │ ├── HilSensor.msg │ ├── HilStateQuaternion.msg │ ├── HomePosition.msg │ ├── LandingTarget.msg │ ├── LogData.msg │ ├── LogEntry.msg │ ├── ManualControl.msg │ ├── Mavlink.msg │ ├── MountControl.msg │ ├── OnboardComputerStatus.msg │ ├── OpticalFlowRad.msg │ ├── OverrideRCIn.msg │ ├── Param.msg │ ├── ParamValue.msg │ ├── PositionTarget.msg │ ├── RCIn.msg │ ├── RCOut.msg │ ├── RTCM.msg │ ├── RadioStatus.msg │ ├── State.msg │ ├── StatusText.msg │ ├── Thrust.msg │ ├── TiltAngleTarget.msg │ ├── TiltrotorActuatorCommands.msg │ ├── TimesyncStatus.msg │ ├── Trajectory.msg │ ├── VFR_HUD.msg │ ├── VehicleInfo.msg │ ├── Vibration.msg │ ├── Waypoint.msg │ ├── WaypointList.msg │ ├── WaypointReached.msg │ ├── WheelOdomStamped.msg │ └── WrenchTarget.msg ├── package.xml └── srv │ ├── CommandBool.srv │ ├── CommandHome.srv │ ├── CommandInt.srv │ ├── CommandLong.srv │ ├── CommandTOL.srv │ ├── CommandTriggerControl.srv │ ├── CommandTriggerInterval.srv │ ├── CommandVtolTransition.srv │ ├── FileChecksum.srv │ ├── FileClose.srv │ ├── FileList.srv │ ├── FileMakeDir.srv │ ├── FileOpen.srv │ ├── FileRead.srv │ ├── FileRemove.srv │ ├── FileRemoveDir.srv │ ├── FileRename.srv │ ├── FileTruncate.srv │ ├── FileWrite.srv │ ├── LogRequestData.srv │ ├── LogRequestEnd.srv │ ├── LogRequestList.srv │ ├── MessageInterval.srv │ ├── MountConfigure.srv │ ├── ParamGet.srv │ ├── ParamPull.srv │ ├── ParamPush.srv │ ├── ParamSet.srv │ ├── SetMavFrame.srv │ ├── SetMode.srv │ ├── StreamRate.srv │ ├── VehicleInfoGet.srv │ ├── WaypointClear.srv │ ├── WaypointPull.srv │ ├── WaypointPush.srv │ └── WaypointSetCurrent.srv └── test_mavros ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include └── test_mavros │ ├── sitl_test │ ├── sitl_test.h │ └── test_setup.h │ ├── tests │ └── offboard_control.h │ └── utils │ └── pid_controller.h ├── launch ├── apm │ ├── apm_imu_test.launch │ ├── apm_imu_test.rviz │ ├── apm_local_position_test.launch │ ├── schematic_plane.rviz │ ├── schematic_plane.urdf.xacro │ └── schematic_plane_rviz.launch ├── base_node.launch └── px4 │ └── iris_empty_world_offboard_ctl.launch ├── package.xml ├── scripts └── setpoint_position_demo └── sitl_test ├── lib ├── pid_controller.cpp └── sitl_test.cpp └── sitl_test_node.cpp /.github/ISSUE_TEMPLATE: -------------------------------------------------------------------------------- 1 | This is only bug and feature tracker, please use it 2 | to report bugs or request features. 3 | 4 | ---- 5 | 6 | ### Issue details 7 | 8 | _Please describe the problem, or desired feature_ 9 | 10 | 11 | ### MAVROS version and platform 12 | 13 | Mavros: ?0.18.4? 14 | ROS: ?Kinetic? 15 | Ubuntu: ?16.04? 16 | 17 | 18 | ### Autopilot type and version 19 | 20 | [ ] ArduPilot 21 | [ ] PX4 22 | 23 | Version: ?3.7.1? 24 | 25 | 26 | ### Node logs 27 | 28 | ``` 29 | copy output of mavros_node. Usually console where you run roslaunch 30 | ``` 31 | 32 | ### Diagnostics 33 | 34 | ``` 35 | place here result of: 36 | rostopic echo -n1 /diagnostics 37 | ``` 38 | 39 | ### Check ID 40 | 41 | ``` 42 | rosrun mavros checkid 43 | ``` 44 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.swp 2 | *~ 3 | *.pyc 4 | core 5 | 6 | # generated 7 | libmavconn/include/mavconn/mavlink_dialect.h 8 | libmavconn/src/mavlink_helpers.cpp 9 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # Based on https://github.com/felixduvallet/ros-travis-integration 2 | # 3 | # vim:set ts=2 sw=2 et: 4 | 5 | language: generic 6 | services: 7 | - docker 8 | 9 | cache: 10 | directories: 11 | - $HOME/.ccache 12 | 13 | env: 14 | global: 15 | - CCACHE_DIR=$HOME/.ccache 16 | - DOCKER_RUN_OPTS="-e CI=true" 17 | matrix: 18 | - ROS_DISTRO="kinetic" 19 | - ROS_DISTRO="melodic" 20 | - ROS_DISTRO="kinetic" NOT_TEST_BUILD=true 21 | - ROS_DISTRO="kinetic" UPSTREAM_WORKSPACE=file $ROSINSTALL_FILENAME='dependencies.rosinstall' 22 | 23 | install: 24 | - git clone --depth=1 https://github.com/ros-industrial/industrial_ci.git .industrial_ci 25 | script: 26 | - .industrial_ci/travis.sh 27 | 28 | notifications: 29 | webhooks: 30 | urls: 31 | - https://webhooks.gitter.im/e/b856f174ff9736ad23ec 32 | on_success: change 33 | on_failure: always 34 | on_start: false 35 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | Contributing 2 | ============ 3 | 4 | 5 | 1. Fork the repo: 6 | ![fork](http://s24.postimg.org/pfvt9sdv9/Fork_mavros.png) 7 | 2. Clone the repo (`git clone https://github.com/mavlink/mavros.git`); 8 | 3. Create a remote connection to your repo (`git remote add git@github.com:/mavros.git`); 9 | 4. Create a feature/dev branch (`git checkout -b `); 10 | 5. Add the changes; 11 | 6. Apply the changes by committing (`git commit -m ""` or `git commit -a` and then write message; if adding new files: `git add `); 12 | 7. Check code style `uncrustify -c ${ROS_WORKSPACE}/src/mavros/mavros/tools/uncrustify-cpp.cfg --replace --no-backup `; 13 | 8. Fix small code style errors and typos; 14 | 9. Commit with description like "uncrustify" or "code style fix". Please avoid changes in program logic (separate commit are better than mix of style and bug fix); 15 | 10. Run tests: 16 | - with `catkin_make`, issue `catkin_make tests` and then `catkin_make run_tests`; 17 | - with `catkin tools`, issue `catkin run_tests`; 18 | 11. If everything goes as planned, push the changes (`git push -u `) and issue a pull request. 19 | 20 | 21 | cog.py generators 22 | ----------------- 23 | 24 | In many places we need to copy some data from MAVLink, and in many places we have regular patterns of code (e.g. copy message fields). 25 | To avoid manual copy-paste work (and errors!) we use [cog.py][cog] generator/preprocessor. 26 | Generator program written in comment blocks on Python (that allow import pymavlink), output will be inserted between markers. 27 | As an example you may look at `utils::to_string()` implementation for some enums: [lib/enum_to_string.cpp][ets]. 28 | 29 | To install it : 30 | 31 | pip install --user cogapp pymavlink 32 | 33 | Then fill the behaviour you when between the `[[[cog:]]]` `[[[end]]]` balise 34 | and invoke cog like this: 35 | 36 | cog.py -cr your_file.h/cpp 37 | 38 | Your file will be updated by cog. 39 | 40 | ./mavros/tools/cogall.sh 41 | 42 | This script will regenerate all files with generators. 43 | 44 | 45 | [cog]: https://nedbatchelder.com/code/cog/ 46 | [ets]: https://github.com/mavlink/mavros/blob/master/mavros/src/lib/enum_to_string.cpp 47 | -------------------------------------------------------------------------------- /LICENSE-BSD.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2013-2015 Vladimir Ermakov. All rights reserved. 2 | 3 | Redistribution and use in source and binary forms, with or without 4 | modification, are permitted provided that the following conditions 5 | are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | 2. Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in 11 | the documentation and/or other materials provided with the 12 | distribution. 13 | 3. Neither the name PX4 nor the names of its contributors may be 14 | used to endorse or promote products derived from this software 15 | without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 20 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 21 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 22 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 23 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 24 | OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 25 | AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 26 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 27 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | POSSIBILITY OF SUCH DAMAGE. 29 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | Mavros license variants 2 | ======================= 3 | 4 | Mavros is available triple-licensed as [BSD][bsd], [GPLv3][gpl] and [LGPLv3][lgpl]. 5 | Its use is compatible with any of these licenses. 6 | However, contributions to the upstream repository must be made 7 | available under all three licenses and therefore have 8 | to be compatible to BSD, GPLv3 and LGPLv3. 9 | 10 | 11 | [lgpl]: https://www.gnu.org/licenses/lgpl.html 12 | [gpl]: https://www.gnu.org/licenses/gpl.html 13 | [bsd]: https://github.com/mavlink/mavros/blob/master/LICENSE-BSD.txt 14 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | MAVROS 2 | ====== 3 | [![GitHub release (latest by date)](https://img.shields.io/github/v/release/mavlink/mavros)](https://github.com/mavlink/mavros/releases) [![Gitter](https://badges.gitter.im/Join%20Chat.svg)](https://gitter.im/mavlink/mavros?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge&utm_content=badge) 4 | 5 | MAVLink extendable communication node for ROS. 6 | 7 | - Since 2014-08-11 this repository contains several packages. 8 | - Since 2014-11-02 hydro support separated from master to hydro-devel branch. 9 | - Since 2015-03-04 all packages also dual licensed under terms of BSD license. 10 | - Since 2015-08-10 all messages moved to mavros\_msgs package 11 | - Since 2016-02-05 (v0.17) frame conversion changed again 12 | - Since 2016-06-22 (pre v0.18) Indigo and Jade separated from master to indigo-devel branch. 13 | - Since 2016-06-23 (0.18.0) support MAVLink 2.0 without signing. 14 | - Since 2017-08-23 (0.20.0) [GeographicLib][geolib] and it's datasets are required. Used to convert AMSL (FCU) and WGS84 (ROS) altitudes. 15 | - Since 2018-05-11 (0.25.0) support building master for Indigo and Jade stopped. Mainly because update of console-bridge package. 16 | - Since 2018-05-14 (0.25.1) support for Indigo returned. We use compatibility layer for console-bridge. 17 | - Since 2019-01-03 (0.28.0) support for Indigo by master not guaranteed. Consider update to more recent distro. 18 | 19 | 20 | mavros package 21 | -------------- 22 | 23 | It is the main package, please see its [README][mrrm]. 24 | Here you may read [installation instructions][inst]. 25 | 26 | 27 | mavros\_extras package 28 | ---------------------- 29 | 30 | This package contains some extra nodes and plugins for mavros, please see its [README][exrm]. 31 | 32 | 33 | libmavconn package 34 | ------------------ 35 | 36 | This package contain mavconn library, see its [README][libmc]. 37 | LibMAVConn may be used outside of ROS environment. 38 | 39 | 40 | test\_mavros package 41 | -------------------- 42 | 43 | This package contain hand-tests and [manual page][test] for APM and PX4 SITL. 44 | Please see [README][test] first! 45 | 46 | 47 | mavros\_msgs package 48 | -------------------- 49 | 50 | This package contains messages and services used in MAVROS. 51 | 52 | 53 | Support forums and chats 54 | ------------------------ 55 | 56 | Please ask your questions not related to bugs/feature or requests on: 57 | 58 | - [MAVROS discussion in Gitter IM](https://gitter.im/mavlink/mavros) 59 | - [PX4 Discuss Forum](https://discuss.px4.io/) 60 | - [PX4 Slack](https://slack.px4.io/) 61 | - [Ardupilot Discuss Forum](https://discuss.ardupilot.org/) 62 | - [ArduPilot/VisionProjects in Gitter IM](https://gitter.im/ArduPilot/ardupilot/VisionProjects) 63 | 64 | We'd like to keep the project bug tracker as free as possible, so please contact via the above methods. You can also PM us via Gitter and the PX4 Slack. 65 | 66 | 67 | CI Statuses 68 | ----------- 69 | 70 | - ROS Kinetic: [![Build Status](http://build.ros.org/buildStatus/icon?job=Kdev__mavros__ubuntu_xenial_amd64)](http://build.ros.org/job/Kdev__mavros__ubuntu_xenial_amd64/) 71 | - ROS Melodic: [![Build Status](http://build.ros.org/buildStatus/icon?job=Mdev__mavros__ubuntu_bionic_amd64)](http://build.ros.org/job/Mdev__mavros__ubuntu_bionic_amd64/) 72 | - Travis master: [![Travis Status](https://travis-ci.org/mavlink/mavros.svg?branch=master)](https://travis-ci.org/mavlink/mavros) 73 | 74 | 75 | [mrrm]: https://github.com/mavlink/mavros/blob/master/mavros/README.md 76 | [exrm]: https://github.com/mavlink/mavros/blob/master/mavros_extras/README.md 77 | [libmc]: https://github.com/mavlink/mavros/blob/master/libmavconn/README.md 78 | [test]: https://github.com/mavlink/mavros/blob/master/test_mavros/README.md 79 | [inst]: https://github.com/mavlink/mavros/blob/master/mavros/README.md#installation 80 | [geolib]: https://geographiclib.sourceforge.io/ 81 | -------------------------------------------------------------------------------- /dependencies.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: mavlink 3 | uri: https://github.com/mavlink/mavlink-gbp-release.git 4 | version: release/kinetic/mavlink 5 | -------------------------------------------------------------------------------- /libmavconn/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(libmavconn) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | find_package(console_bridge REQUIRED) 11 | if(console_bridge_VERSION GREATER 0.3.0) 12 | add_definitions(-DDEFINE_LOGGING_MACROS) 13 | endif () 14 | 15 | find_package(Boost REQUIRED COMPONENTS system) 16 | 17 | # add package modules path, not needed in dependend packages 18 | list(INSERT CMAKE_MODULE_PATH 0 "${CMAKE_CURRENT_SOURCE_DIR}/cmake/Modules") 19 | include(EnableCXX11) 20 | include(MavrosMavlink) 21 | 22 | # Fixed in mavlink 2016.7.7 23 | #-> enum values out of int range 24 | #list(APPEND IGNORE_DIALECTS "autoquad") 25 | 26 | ################################### 27 | ## catkin specific configuration ## 28 | ################################### 29 | catkin_package( 30 | INCLUDE_DIRS include 31 | LIBRARIES mavconn 32 | DEPENDS Boost console_bridge mavlink 33 | CFG_EXTRAS libmavconn-extras.cmake 34 | ) 35 | 36 | ########### 37 | ## Build ## 38 | ########### 39 | 40 | include_directories( 41 | include 42 | ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/include 43 | ${Boost_INCLUDE_DIRS} 44 | ${mavlink_INCLUDE_DIRS} 45 | ${console_bridge_INCLUDE_DIRS} 46 | ) 47 | 48 | ## Declare a cpp library 49 | add_library(mavconn 50 | ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/src/mavlink_helpers.cpp 51 | src/interface.cpp 52 | src/serial.cpp 53 | src/tcp.cpp 54 | src/udp.cpp 55 | ) 56 | target_link_libraries(mavconn 57 | ${Boost_LIBRARIES} 58 | ${console_bridge_LIBRARIES} 59 | ) 60 | 61 | # Use catkin-supplied em_expand macros to generate source files 62 | em_expand(${CMAKE_CURRENT_SOURCE_DIR}/mavlink.context.py.in 63 | ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/mavlink.context.py 64 | ${CMAKE_CURRENT_SOURCE_DIR}/include/mavconn/mavlink_dialect.h.em 65 | ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/include/mavconn/mavlink_dialect.h) 66 | 67 | em_expand(${CMAKE_CURRENT_SOURCE_DIR}/mavlink.context.py.in 68 | ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/mavlink.context.py 69 | ${CMAKE_CURRENT_SOURCE_DIR}/src/mavlink_helpers.cpp.em 70 | ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/src/mavlink_helpers.cpp) 71 | 72 | if(DEFINED ENV{CI}) 73 | message(STATUS "CI environment detected. Assume that it is on RO fs") 74 | else() 75 | message(STATUS "Copy mavlink_dialect.h to source tree") 76 | file( 77 | COPY ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/include/mavconn/mavlink_dialect.h 78 | DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/include/mavconn/ 79 | ) 80 | endif() 81 | 82 | ############# 83 | ## Install ## 84 | ############# 85 | 86 | # all install targets should use catkin DESTINATION variables 87 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 88 | 89 | ## Mark executables and/or libraries for installation 90 | install(TARGETS mavconn 91 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 92 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 93 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 94 | ) 95 | 96 | ## Mark cpp header files for installation 97 | install(DIRECTORY include/mavconn/ 98 | DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}/mavconn 99 | FILES_MATCHING PATTERN "*.h" 100 | ) 101 | 102 | install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/include/mavconn/ 103 | DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}/mavconn 104 | FILES_MATCHING PATTERN "*.h" 105 | ) 106 | 107 | ## Install cmake files (thanks to cmake_modules package) 108 | install(DIRECTORY cmake/Modules 109 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/cmake 110 | ) 111 | 112 | ############# 113 | ## Testing ## 114 | ############# 115 | 116 | if(CATKIN_ENABLE_TESTING) 117 | catkin_add_gtest(mavconn-test test/test_mavconn.cpp) 118 | target_link_libraries(mavconn-test mavconn pthread) 119 | endif() 120 | 121 | # vim: ts=2 sw=2 et: 122 | -------------------------------------------------------------------------------- /libmavconn/README.md: -------------------------------------------------------------------------------- 1 | MAVCONN library 2 | =============== 3 | 4 | It is mavlink connection and communication library used in [MAVROS][mr]. 5 | Since 2014-11-02 it adopted to use outside from ROS environment 6 | by splitting to individual package and removing dependencies to rosconsole. 7 | 8 | 9 | Connection URL 10 | -------------- 11 | 12 | Connection defined by URL. 13 | Just pass one of that URL to `MAVConnInterface::open_url()` and get connection object. 14 | 15 | Supported schemas: 16 | 17 | - Serial: `/path/to/serial/device[:baudrate]` 18 | - Serial: `serial:///path/to/serial/device[:baudrate][?ids=sysid,compid]` 19 | - Serial with hardware flow control: `serial-hwfc:///path/to/serial/device[:baudrate][?ids=sysid,compid]` 20 | - UDP: `udp://[bind_host][:port]@[remote_host][:port][/?ids=sysid,compid]` 21 | - UDP broadcast until GCS discovery: `udp-b://[bind_host][:port]@[:port][/?ids=sysid,compid]` 22 | - UDP broadcast (permanent): `udp-pb://[bind_host][:port]@[:port][/?ids=sysid,compid]` 23 | - TCP client: `tcp://[server_host][:port][/?ids=sysid,compid]` 24 | - TCP server: `tcp-l://[bind_port][:port][/?ids=sysid,compid]` 25 | 26 | Note: ids from URL overrides ids given by system\_id & component\_id parameters. 27 | 28 | 29 | Dependencies 30 | ------------ 31 | 32 | Same as for mavros: 33 | 34 | - Linux host 35 | - Boost >= 1.46 (used Boost.ASIO) 36 | - console-bridge library 37 | - compiller with C++11 support 38 | 39 | 40 | License 41 | ------- 42 | 43 | Licensed under terms of [*LGPLv3*][lgpllic], [*BSD*][bsdlic], or [*GPLv3*][gpllic]. 44 | 45 | 46 | [mr]: https://github.com/mavlink/mavros 47 | [lgpllic]: https://www.gnu.org/licenses/lgpl.html 48 | [gpllic]: https://www.gnu.org/licenses/gpl.html 49 | [bsdlic]: https://github.com/mavlink/mavros/blob/master/LICENSE-BSD.txt 50 | -------------------------------------------------------------------------------- /libmavconn/cmake/Modules/CheckGeographicLibDatasets.cmake: -------------------------------------------------------------------------------- 1 | ## 2 | # This module verifies the installation of the GeographicLib datasets and warns 3 | # if it doesn't detect them. 4 | ## 5 | 6 | find_path(GEOGRAPHICLIB_GEOID_PATH NAMES geoids PATH_SUFFIXES share/GeographicLib share/geographiclib) 7 | find_path(GEOGRAPHICLIB_GRAVITY_PATH_ NAMES gravity PATH_SUFFIXES share/GeographicLib) 8 | find_path(GEOGRAPHICLIB_MAGNETIC_PATH_ NAMES magnetic PATH_SUFFIXES share/GeographicLib) 9 | 10 | if(NOT GEOGRAPHICLIB_GEOID_PATH) 11 | message(STATUS "No geoid model datasets found. This will result on a SIGINT! Please execute the script install_geographiclib_dataset.sh in /mavros/scripts") 12 | else() 13 | message(STATUS "Geoid model datasets found in: " ${GEOGRAPHICLIB_GEOID_PATH}/geoid) 14 | set(GEOGRAPHICLIB_GEOID_PATH ${GEOGRAPHICLIB_GEOID_PATH}/geoid) 15 | endif() 16 | if(NOT GEOGRAPHICLIB_GRAVITY_PATH_) 17 | message(STATUS "No gravity field model datasets found. Please execute the script install_geographiclib_dataset.sh in /mavros/scripts") 18 | else() 19 | message(STATUS "Gravity Field model datasets found in: " ${GEOGRAPHICLIB_GRAVITY_PATH_}/gravity) 20 | set(GEOGRAPHICLIB_GRAVITY_PATH ${GEOGRAPHICLIB_GRAVITY_PATH_}/gravity) 21 | endif() 22 | if(NOT GEOGRAPHICLIB_MAGNETIC_PATH_) 23 | message(STATUS "No magnetic field model datasets found. Please execute the script install_geographiclib_dataset.sh in /mavros/scripts") 24 | else() 25 | message(STATUS "Magnetic Field model datasets found in: " ${GEOGRAPHICLIB_MAGNETIC_PATH_}/magnetic) 26 | set(GEOGRAPHICLIB_MAGNETIC_PATH ${GEOGRAPHICLIB_MAGNETIC_PATH_}/magnetic) 27 | endif() 28 | -------------------------------------------------------------------------------- /libmavconn/cmake/Modules/EnableCXX11.cmake: -------------------------------------------------------------------------------- 1 | # This module enables C++11 or C++14 support 2 | # 3 | # thanks for: http://www.guyrutenberg.com/2014/01/05/enabling-c11-c0x-in-cmake/ 4 | 5 | include(CheckCXXCompilerFlag) 6 | 7 | check_cxx_compiler_flag("-std=c++14" COMPILER_SUPPORTS_STD_CXX14) 8 | if (NOT COMPILER_SUPPORTS_STD_CXX14) 9 | check_cxx_compiler_flag("-std=c++11" COMPILER_SUPPORTS_STD_CXX11) 10 | endif () 11 | 12 | if (COMPILER_SUPPORTS_STD_CXX14) 13 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") 14 | elseif (COMPILER_SUPPORTS_STD_CXX11) 15 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 16 | else () 17 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 18 | endif () 19 | 20 | # vim: set ts=2 sw=2 et: 21 | -------------------------------------------------------------------------------- /libmavconn/cmake/Modules/FindGeographicLib.cmake: -------------------------------------------------------------------------------- 1 | # Look for GeographicLib 2 | # 3 | # Set 4 | # GEOGRAPHICLIB_FOUND = TRUE 5 | # GeographicLib_INCLUDE_DIRS = /usr/local/include 6 | # GeographicLib_LIBRARIES = /usr/local/lib/libGeographic.so 7 | # GeographicLib_LIBRARY_DIRS = /usr/local/lib 8 | 9 | find_path (GeographicLib_INCLUDE_DIRS NAMES GeographicLib/Config.h) 10 | 11 | find_library (GeographicLib_LIBRARIES NAMES Geographic) 12 | 13 | include (FindPackageHandleStandardArgs) 14 | find_package_handle_standard_args (GeographicLib DEFAULT_MSG 15 | GeographicLib_LIBRARIES GeographicLib_INCLUDE_DIRS) 16 | mark_as_advanced (GeographicLib_LIBRARIES GeographicLib_INCLUDE_DIRS) 17 | 18 | #message(WARNING "GL: F:${GeographicLib_FOUND} L:${GeographicLib_LIBRARIES} I:${GeographicLib_INCLUDE_DIRS}") 19 | -------------------------------------------------------------------------------- /libmavconn/cmake/Modules/MavrosMavlink.cmake: -------------------------------------------------------------------------------- 1 | # This module includes MAVLink package and 2 | 3 | find_package(mavlink REQUIRED) 4 | 5 | # 6 | # Nothing to do since MAVLink v2.0 7 | # 8 | 9 | # vim: set ts=2 sw=2 et: 10 | -------------------------------------------------------------------------------- /libmavconn/cmake/libmavconn-extras.cmake.develspace.in: -------------------------------------------------------------------------------- 1 | # Prepend cmake modules from source directory to the cmake module path 2 | list(INSERT CMAKE_MODULE_PATH 0 "@CMAKE_CURRENT_SOURCE_DIR@/cmake/Modules") 3 | -------------------------------------------------------------------------------- /libmavconn/cmake/libmavconn-extras.cmake.installspace.in: -------------------------------------------------------------------------------- 1 | # Prepend the installed cmake modules to the cmake module path 2 | list(INSERT CMAKE_MODULE_PATH 0 "${libmavconn_DIR}/../../../@CATKIN_PACKAGE_SHARE_DESTINATION@/cmake/Modules") 3 | -------------------------------------------------------------------------------- /libmavconn/include/mavconn/console_bridge_compat.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief MAVConn console-bridge compatibility header 3 | * @file console_bridge_compat.h 4 | * @author Vladimir Ermakov 5 | * 6 | * @addtogroup mavconn 7 | * @{ 8 | */ 9 | /* 10 | * libmavconn 11 | * Copyright 2018 Vladimir Ermakov, All rights reserved. 12 | * 13 | * This file is part of the mavros package and subject to the license terms 14 | * in the top-level LICENSE file of the mavros repository. 15 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 16 | */ 17 | 18 | #pragma once 19 | 20 | #include 21 | 22 | // [[[cog: 23 | // for idx, func in enumerate(('debug', 'inform', 'warn', 'error')): 24 | // fn = f'CONSOLE_BRIDGE_log{func.title()}' 25 | // fu = func.upper() 26 | // 27 | // if func == 'inform': # NOTE: special case 28 | // fu = 'INFO' 29 | // 30 | // if idx != 0: 31 | // cog.outl() 32 | // 33 | // cog.outl(f'#ifndef {fn}') 34 | // cog.outl(f'#define {fn}(fmt, ...) \\') 35 | // cog.outl(f'\tconsole_bridge::log(__FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_{fu}, fmt, ##__VA_ARGS__)') 36 | // cog.outl(f'#endif // {fn}') 37 | // ]]] 38 | #ifndef CONSOLE_BRIDGE_logDebug 39 | #define CONSOLE_BRIDGE_logDebug(fmt, ...) \ 40 | console_bridge::log(__FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_DEBUG, fmt, ##__VA_ARGS__) 41 | #endif // CONSOLE_BRIDGE_logDebug 42 | 43 | #ifndef CONSOLE_BRIDGE_logInform 44 | #define CONSOLE_BRIDGE_logInform(fmt, ...) \ 45 | console_bridge::log(__FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_INFO, fmt, ##__VA_ARGS__) 46 | #endif // CONSOLE_BRIDGE_logInform 47 | 48 | #ifndef CONSOLE_BRIDGE_logWarn 49 | #define CONSOLE_BRIDGE_logWarn(fmt, ...) \ 50 | console_bridge::log(__FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_WARN, fmt, ##__VA_ARGS__) 51 | #endif // CONSOLE_BRIDGE_logWarn 52 | 53 | #ifndef CONSOLE_BRIDGE_logError 54 | #define CONSOLE_BRIDGE_logError(fmt, ...) \ 55 | console_bridge::log(__FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_ERROR, fmt, ##__VA_ARGS__) 56 | #endif // CONSOLE_BRIDGE_logError 57 | // [[[end]]] (checksum: c8dda3189b05a621b7244bf375275345) 58 | -------------------------------------------------------------------------------- /libmavconn/include/mavconn/mavlink_dialect.h.em: -------------------------------------------------------------------------------- 1 | /** 2 | * @@brief MAVConn mavlink.h selector 3 | * @@file mavlink_dialect.h 4 | * @@author Vladimir Ermakov 5 | * 6 | * @@addtogroup mavconn 7 | * @@{ 8 | */ 9 | /* 10 | * libmavconn 11 | * Copyright 2014,2015,2016 Vladimir Ermakov, All rights reserved. 12 | * 13 | * This file is part of the mavros package and subject to the license terms 14 | * in the top-level LICENSE file of the mavros repository. 15 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 16 | */ 17 | @# 18 | @# EmPy template of dialect include file 19 | @# 20 | 21 | #pragma once 22 | 23 | // AUTOMATIC GENERATED FILE! 24 | // from include/mavconn/mavlink_dialect.h.em 25 | 26 | #define MAVLINK_START_SIGN_STREAM(link_id) 27 | #define MAVLINK_END_SIGN_STREAM(link_id) 28 | 29 | @[for dialect in MAVLINK_V20_DIALECTS]#include 30 | @[end for] 31 | -------------------------------------------------------------------------------- /libmavconn/include/mavconn/msgbuffer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief MAVConn message buffer class (internal) 3 | * @file msgbuffer.h 4 | * @author Vladimir Ermakov 5 | * 6 | * @addtogroup mavconn 7 | * @{ 8 | */ 9 | /* 10 | * libmavconn 11 | * Copyright 2014,2015,2016 Vladimir Ermakov, All rights reserved. 12 | * 13 | * This file is part of the mavros package and subject to the license terms 14 | * in the top-level LICENSE file of the mavros repository. 15 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 16 | */ 17 | 18 | #pragma once 19 | 20 | #include 21 | #include 22 | 23 | namespace mavconn { 24 | /** 25 | * @brief Message buffer for internal use in libmavconn 26 | */ 27 | struct MsgBuffer { 28 | //! Maximum buffer size with padding for CRC bytes (280 + padding) 29 | static constexpr ssize_t MAX_SIZE = MAVLINK_MAX_PACKET_LEN + 16; 30 | uint8_t data[MAX_SIZE]; 31 | ssize_t len; 32 | ssize_t pos; 33 | 34 | MsgBuffer() : 35 | pos(0), 36 | len(0) 37 | { } 38 | 39 | /** 40 | * @brief Buffer constructor from mavlink_message_t 41 | */ 42 | explicit MsgBuffer(const mavlink::mavlink_message_t *msg) : 43 | pos(0) 44 | { 45 | len = mavlink::mavlink_msg_to_send_buffer(data, msg); 46 | // paranoic check, it must be less than MAVLINK_MAX_PACKET_LEN 47 | assert(len < MAX_SIZE); 48 | } 49 | 50 | /** 51 | * @brief Buffer constructor for mavlink::Message derived object. 52 | */ 53 | MsgBuffer(const mavlink::Message &obj, mavlink::mavlink_status_t *status, uint8_t sysid, uint8_t compid) : 54 | pos(0) 55 | { 56 | mavlink::mavlink_message_t msg; 57 | mavlink::MsgMap map(msg); 58 | 59 | auto mi = obj.get_message_info(); 60 | 61 | obj.serialize(map); 62 | mavlink::mavlink_finalize_message_buffer(&msg, sysid, compid, status, mi.min_length, mi.length, mi.crc_extra); 63 | 64 | len = mavlink::mavlink_msg_to_send_buffer(data, &msg); 65 | // paranoic check, it must be less than MAVLINK_MAX_PACKET_LEN 66 | assert(len < MAX_SIZE); 67 | } 68 | 69 | /** 70 | * @brief Buffer constructor for send_bytes() 71 | * @param[in] nbytes should be less than MAX_SIZE 72 | */ 73 | MsgBuffer(const uint8_t *bytes, ssize_t nbytes) : 74 | pos(0), 75 | len(nbytes) 76 | { 77 | assert(0 < nbytes && nbytes < MAX_SIZE); 78 | memcpy(data, bytes, nbytes); 79 | } 80 | 81 | virtual ~MsgBuffer() { 82 | pos = 0; 83 | len = 0; 84 | } 85 | 86 | uint8_t *dpos() { 87 | return data + pos; 88 | } 89 | 90 | ssize_t nbytes() { 91 | return len - pos; 92 | } 93 | }; 94 | } // namespace mavconn 95 | -------------------------------------------------------------------------------- /libmavconn/include/mavconn/serial.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief MAVConn Serial link class 3 | * @file serial.h 4 | * @author Vladimir Ermakov 5 | * 6 | * @addtogroup mavconn 7 | * @{ 8 | */ 9 | /* 10 | * libmavconn 11 | * Copyright 2013,2014,2015,2016 Vladimir Ermakov, All rights reserved. 12 | * 13 | * This file is part of the mavros package and subject to the license terms 14 | * in the top-level LICENSE file of the mavros repository. 15 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 16 | */ 17 | 18 | #pragma once 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | namespace mavconn { 26 | /** 27 | * @brief Serial interface 28 | */ 29 | class MAVConnSerial : public MAVConnInterface, 30 | public std::enable_shared_from_this { 31 | public: 32 | static constexpr auto DEFAULT_DEVICE = "/dev/ttyACM0"; 33 | static constexpr auto DEFAULT_BAUDRATE = 57600; 34 | 35 | /** 36 | * Open and run serial link. 37 | * 38 | * @param[in] device TTY device path 39 | * @param[in] baudrate serial baudrate 40 | */ 41 | MAVConnSerial(uint8_t system_id = 1, uint8_t component_id = MAV_COMP_ID_UDP_BRIDGE, 42 | std::string device = DEFAULT_DEVICE, unsigned baudrate = DEFAULT_BAUDRATE, bool hwflow = false); 43 | ~MAVConnSerial(); 44 | 45 | void close() override; 46 | 47 | void send_message(const mavlink::mavlink_message_t *message) override; 48 | void send_message(const mavlink::Message &message, const uint8_t source_compid) override; 49 | void send_bytes(const uint8_t *bytes, size_t length) override; 50 | 51 | inline bool is_open() override { 52 | return serial_dev.is_open(); 53 | } 54 | 55 | private: 56 | boost::asio::io_service io_service; 57 | std::thread io_thread; 58 | boost::asio::serial_port serial_dev; 59 | 60 | std::atomic tx_in_progress; 61 | std::deque tx_q; 62 | std::array rx_buf; 63 | std::recursive_mutex mutex; 64 | 65 | void do_read(); 66 | void do_write(bool check_tx_state); 67 | }; 68 | } // namespace mavconn 69 | -------------------------------------------------------------------------------- /libmavconn/include/mavconn/thread_utils.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief some useful utils 3 | * @file thread_utils.h 4 | * @author Vladimir Ermakov 5 | * 6 | * @addtogroup mavutils 7 | * @{ 8 | * @brief Some useful utils 9 | */ 10 | /* 11 | * libmavconn 12 | * Copyright 2014,2015,2016 Vladimir Ermakov, All rights reserved. 13 | * 14 | * This file is part of the mavros package and subject to the license terms 15 | * in the top-level LICENSE file of the mavros repository. 16 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 17 | */ 18 | 19 | #pragma once 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace mavconn { 28 | namespace utils { 29 | 30 | /** 31 | * @brief Make printf-formatted std::string 32 | * 33 | */ 34 | template 35 | std::string format(const std::string &fmt, Args ... args) 36 | { 37 | // C++11 specify that string store elements continously 38 | std::string ret; 39 | 40 | auto sz = std::snprintf(nullptr, 0, fmt.c_str(), args...); 41 | ret.reserve(sz + 1); ret.resize(sz); // to be sure there have room for \0 42 | std::snprintf(&ret.front(), ret.capacity() + 1, fmt.c_str(), args...); 43 | return ret; 44 | } 45 | 46 | /** 47 | * @brief Set name to current thread, printf-like 48 | * @param[in] name name for thread 49 | * @return true if success 50 | * 51 | * @note Only for Linux target 52 | * @todo add for other posix system 53 | */ 54 | template 55 | bool set_this_thread_name(const std::string &name, Args&& ... args) 56 | { 57 | auto new_name = format(name, std::forward(args)...); 58 | 59 | #ifdef __APPLE__ 60 | return pthread_setname_np(new_name.c_str()) == 0; 61 | #else 62 | pthread_t pth = pthread_self(); 63 | return pthread_setname_np(pth, new_name.c_str()) == 0; 64 | #endif 65 | } 66 | 67 | /** 68 | * @brief Convert to string objects with operator << 69 | */ 70 | template 71 | inline const std::string to_string_ss(T &obj) 72 | { 73 | std::ostringstream ss; 74 | ss << obj; 75 | return ss.str(); 76 | } 77 | 78 | constexpr size_t operator"" _KiB (unsigned long long sz) 79 | { 80 | return sz * 1024; 81 | } 82 | } // namespace utils 83 | } // namespace mavconn 84 | -------------------------------------------------------------------------------- /libmavconn/include/mavconn/udp.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief MAVConn UDP link class 3 | * @file mavconn_udp.h 4 | * @author Vladimir Ermakov 5 | * 6 | * @addtogroup mavconn 7 | * @{ 8 | */ 9 | /* 10 | * libmavconn 11 | * Copyright 2013,2014,2015,2016 Vladimir Ermakov, All rights reserved. 12 | * 13 | * This file is part of the mavros package and subject to the license terms 14 | * in the top-level LICENSE file of the mavros repository. 15 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 16 | */ 17 | 18 | #pragma once 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | namespace mavconn { 26 | /** 27 | * @brief UDP interface 28 | * 29 | * @note IPv4 only 30 | */ 31 | class MAVConnUDP : public MAVConnInterface, 32 | public std::enable_shared_from_this { 33 | public: 34 | static constexpr auto DEFAULT_BIND_HOST = "localhost"; 35 | static constexpr auto DEFAULT_BIND_PORT = 14555; 36 | static constexpr auto DEFAULT_REMOTE_HOST = ""; 37 | static constexpr auto DEFAULT_REMOTE_PORT = 14550; 38 | //! Markers for broadcast modes. Not valid domain names. 39 | static constexpr auto BROADCAST_REMOTE_HOST = "***i want broadcast***"; 40 | static constexpr auto PERMANENT_BROADCAST_REMOTE_HOST = "***permanent broadcast***"; 41 | 42 | /** 43 | * @param[id] bind_host bind host 44 | * @param[id] bind_port bind port 45 | * @param[id] remote_host remote host (optional) 46 | * @param[id] remote_port remote port (optional) 47 | */ 48 | MAVConnUDP(uint8_t system_id = 1, uint8_t component_id = MAV_COMP_ID_UDP_BRIDGE, 49 | std::string bind_host = DEFAULT_BIND_HOST, unsigned short bind_port = DEFAULT_BIND_PORT, 50 | std::string remote_host = DEFAULT_REMOTE_HOST, unsigned short remote_port = DEFAULT_REMOTE_PORT); 51 | ~MAVConnUDP(); 52 | 53 | void close() override; 54 | 55 | void send_message(const mavlink::mavlink_message_t *message) override; 56 | void send_message(const mavlink::Message &message, const uint8_t source_compid) override; 57 | void send_bytes(const uint8_t *bytes, size_t length) override; 58 | 59 | inline bool is_open() override { 60 | return socket.is_open(); 61 | } 62 | 63 | private: 64 | boost::asio::io_service io_service; 65 | std::unique_ptr io_work; 66 | std::thread io_thread; 67 | bool permanent_broadcast; 68 | 69 | std::atomic remote_exists; 70 | boost::asio::ip::udp::socket socket; 71 | boost::asio::ip::udp::endpoint remote_ep; 72 | boost::asio::ip::udp::endpoint recv_ep; 73 | boost::asio::ip::udp::endpoint last_remote_ep; 74 | boost::asio::ip::udp::endpoint bind_ep; 75 | 76 | std::atomic tx_in_progress; 77 | std::deque tx_q; 78 | std::array rx_buf; 79 | std::recursive_mutex mutex; 80 | 81 | void do_recvfrom(); 82 | void do_sendto(bool check_tx_state); 83 | }; 84 | } // namespace mavconn 85 | -------------------------------------------------------------------------------- /libmavconn/mavlink.context.py.in: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | MAVLINK_V20_DIALECTS = "@mavlink2_DIALECTS@".split(';') 4 | 5 | IGNORE_DIALECTS = "@IGNORE_DIALECTS@".split(';') 6 | 7 | # Most interesting dialects 8 | _COMMON = 'common' 9 | _APM = 'ardupilotmega' 10 | 11 | for dialect in IGNORE_DIALECTS: 12 | if dialect in MAVLINK_V20_DIALECTS: 13 | MAVLINK_V20_DIALECTS.remove(dialect) 14 | 15 | if _COMMON not in MAVLINK_V20_DIALECTS: 16 | raise ValueError("common dialect not listed!") 17 | 18 | # common should be first 19 | MAVLINK_V20_DIALECTS.sort() 20 | MAVLINK_V20_DIALECTS.remove(_COMMON) 21 | MAVLINK_V20_DIALECTS.insert(0, _COMMON) 22 | 23 | # APM dialect should be second 24 | if _APM in MAVLINK_V20_DIALECTS: 25 | MAVLINK_V20_DIALECTS.remove(_APM) 26 | MAVLINK_V20_DIALECTS.insert(1, _APM) 27 | -------------------------------------------------------------------------------- /libmavconn/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | libmavconn 4 | 0.33.3 5 | 6 | MAVLink communication library. 7 | This library provide unified connection handling classes 8 | and URL to connection object mapper. 9 | 10 | This library can be used in standalone programs. 11 | 12 | 13 | Vladimir Ermakov 14 | Vladimir Ermakov 15 | GPLv3 16 | LGPLv3 17 | BSD 18 | 19 | http://wiki.ros.org/mavros 20 | https://github.com/mavlink/mavros.git 21 | https://github.com/mavlink/mavros/issues 22 | 23 | catkin 24 | 25 | boost 26 | mavlink 27 | libconsole-bridge-dev 28 | 29 | rosunit 30 | gtest 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /libmavconn/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | exclude_patterns: '*/.git/* */test/*' 3 | -------------------------------------------------------------------------------- /libmavconn/src/mavlink_helpers.cpp.em: -------------------------------------------------------------------------------- 1 | /** 2 | * @@brief MAVLink helpers 3 | * @@file mavlink_helpers.cpp 4 | * @@author Vladimir Ermakov 5 | * 6 | * This file defines replace for some helper function to prevent problem #269. 7 | * 8 | * @@addtogroup mavconn 9 | * @@{ 10 | */ 11 | /* 12 | * libmavconn 13 | * Copyright 2015,2016 Vladimir Ermakov, All rights reserved. 14 | * 15 | * This file is part of the mavros package and subject to the license terms 16 | * in the top-level LICENSE file of the mavros repository. 17 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 18 | */ 19 | @# 20 | @# EmPy template of dialect helpers source file 21 | @# 22 | 23 | #include 24 | #include 25 | 26 | // AUTOMATIC GENERATED FILE! 27 | // from src/mavlink_helpers.cpp.em 28 | 29 | using mavconn::MAVConnInterface; 30 | 31 | void MAVConnInterface::init_msg_entry() 32 | { 33 | CONSOLE_BRIDGE_logDebug("mavconn: Initialize message_entries map"); 34 | 35 | auto load = [&](const char *dialect, const mavlink::mavlink_msg_entry_t & e) { 36 | auto it = message_entries.find(e.msgid); 37 | if (it != message_entries.end()) { 38 | if (memcmp(&e, it->second, sizeof(e)) != 0) { 39 | CONSOLE_BRIDGE_logDebug("mavconn: init: message from %s, MSG-ID %d ignored! Table has different entry.", dialect, e.msgid); 40 | } 41 | else { 42 | CONSOLE_BRIDGE_logDebug("mavconn: init: message from %s, MSG-ID %d in table.", dialect, e.msgid); 43 | } 44 | } 45 | else { 46 | CONSOLE_BRIDGE_logDebug("mavconn: init: add message entry for %s, MSG-ID %d", dialect, e.msgid); 47 | message_entries[e.msgid] = &e; 48 | } 49 | }; 50 | 51 | @[for dialect in MAVLINK_V20_DIALECTS]for (auto &e : mavlink::@dialect::MESSAGE_ENTRIES) @(' ' * (20 - len(dialect))) load("@dialect", e); 52 | @[end for] 53 | } 54 | 55 | std::vector MAVConnInterface::get_known_dialects() 56 | { 57 | return { 58 | @[for dialect in MAVLINK_V20_DIALECTS]"@dialect", 59 | @[end for] 60 | }; 61 | } 62 | 63 | /** 64 | * Internal function to give access to message information such as additional crc byte. 65 | */ 66 | const mavlink::mavlink_msg_entry_t* mavlink::mavlink_get_msg_entry(uint32_t msgid) 67 | { 68 | auto it = MAVConnInterface::message_entries.find(msgid); 69 | if (it != MAVConnInterface::message_entries.end()) 70 | return it->second; 71 | else 72 | return nullptr; 73 | } 74 | -------------------------------------------------------------------------------- /libmavconn/test/test_hang.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Test mavconn library 3 | * 4 | * This test created for issue #72. 5 | * It is hand test, no build rules. 6 | * 7 | * Compile command: 8 | * g++ -I/opt/ros/kinetic/include -I$HOME/ros/install/include -Iinclude \ 9 | * -std=c++11 -ggdb test/test_hang.cpp -o /tmp/hang \ 10 | * -L/opt/ros/kinetic/lib -L$HOME/ros/devel/lib -lroscpp -lrosconsole -lboost_system -lmavconn -lrt \ 11 | * -DMAVLINK_DIALECT=ardupilotmega 12 | */ 13 | 14 | #include 15 | 16 | #include 17 | 18 | using namespace mavconn; 19 | using namespace mavlink; 20 | 21 | static void send_heartbeat(MAVConnInterface *ip) { 22 | #if 0 23 | mavlink_message_t msg; 24 | mavlink_msg_heartbeat_pack_chan(ip->get_system_id(), ip->get_component_id(), ip->get_channel(), &msg, 25 | MAV_TYPE_ONBOARD_CONTROLLER, 26 | MAV_AUTOPILOT_INVALID, 27 | MAV_MODE_MANUAL_ARMED, 28 | 0, 29 | MAV_STATE_ACTIVE); 30 | ip->send_message(&msg); 31 | #endif 32 | 33 | using mavlink::common::MAV_TYPE; 34 | using mavlink::common::MAV_AUTOPILOT; 35 | using mavlink::common::MAV_MODE; 36 | using mavlink::common::MAV_STATE; 37 | 38 | mavlink::common::msg::HEARTBEAT hb {}; 39 | hb.type = int(MAV_TYPE::ONBOARD_CONTROLLER); 40 | hb.autopilot = int(MAV_AUTOPILOT::INVALID); 41 | hb.base_mode = int(MAV_MODE::MANUAL_ARMED); 42 | hb.custom_mode = 0; 43 | hb.system_status = int(MAV_STATE::ACTIVE); 44 | 45 | ip->send_message_ignore_drop(hb); 46 | } 47 | 48 | static void send_sys_status(MAVConnInterface *ip) { 49 | mavlink_message_t msg {}; 50 | mavlink::MsgMap map(msg); 51 | 52 | mavlink::common::msg::SYS_STATUS st {}; 53 | st.load = 100; 54 | 55 | st.serialize(map); 56 | mavlink::mavlink_finalize_message(&msg, ip->get_system_id(), ip->get_component_id(), st.MIN_LENGTH, st.LENGTH, st.CRC_EXTRA); 57 | 58 | //const mavlink::mavlink_msg_entry_t *e = mavlink::mavlink_get_msg_entry(msg.msgid); 59 | 60 | ip->send_message_ignore_drop(&msg); 61 | } 62 | 63 | int main(int argc, char **argv){ 64 | ros::init(argc, argv, "mavconn_test", ros::init_options::AnonymousName); 65 | 66 | MAVConnInterface::Ptr server; 67 | MAVConnInterface::Ptr client; 68 | 69 | // create echo server 70 | server = MAVConnInterface::open_url("udp://:45000@", 42, 200); 71 | server->message_received_cb = [&](const mavlink_message_t * msg, const Framing framing) { 72 | server->send_message_ignore_drop(msg); 73 | send_sys_status(server.get()); 74 | }; 75 | 76 | // create client 77 | client = MAVConnInterface::open_url("udp://:45001@localhost:45000", 44, 200); 78 | client->message_received_cb = [&](const mavlink_message_t * msg, const Framing framing) { 79 | //std::cout << "C:RECV: " << msg->msgid << std::endl; 80 | ROS_INFO("RECV: MsgID: %4u St: %d", msg->msgid, int(framing)); 81 | }; 82 | 83 | while (ros::ok()) { 84 | send_heartbeat(client.get()); 85 | } 86 | return 0; 87 | } 88 | -------------------------------------------------------------------------------- /mavros/include/mavros/mavlink_diag.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief Mavlink diag class 3 | * @file mavlink_diag.h 4 | * @author Vladimir Ermakov 5 | * 6 | * @addtogroup nodelib 7 | * @{ 8 | */ 9 | /* 10 | * Copyright 2014 Vladimir Ermakov. 11 | * 12 | * This file is part of the mavros package and subject to the license terms 13 | * in the top-level LICENSE file of the mavros repository. 14 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | #include 21 | 22 | namespace mavros { 23 | class MavlinkDiag : public diagnostic_updater::DiagnosticTask 24 | { 25 | public: 26 | explicit MavlinkDiag(std::string name); 27 | 28 | void run(diagnostic_updater::DiagnosticStatusWrapper &stat); 29 | 30 | void set_mavconn(const mavconn::MAVConnInterface::Ptr &link) { 31 | weak_link = link; 32 | } 33 | 34 | void set_connection_status(bool connected) { 35 | is_connected = connected; 36 | } 37 | 38 | private: 39 | mavconn::MAVConnInterface::WeakPtr weak_link; 40 | unsigned int last_drop_count; 41 | std::atomic is_connected; 42 | }; 43 | }; // namespace mavros 44 | 45 | -------------------------------------------------------------------------------- /mavros/include/mavros/mavros.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief MavRos node implementation class 3 | * @file mavros.h 4 | * @author Vladimir Ermakov 5 | * 6 | * @addtogroup nodelib 7 | * @{ 8 | * @brief MAVROS node implementation 9 | */ 10 | /* 11 | * Copyright 2014,2015 Vladimir Ermakov. 12 | * 13 | * This file is part of the mavros package and subject to the license terms 14 | * in the top-level LICENSE file of the mavros repository. 15 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 16 | */ 17 | 18 | #pragma once 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | namespace mavros { 29 | /** 30 | * @brief MAVROS node class 31 | * 32 | * This class implement mavros_node 33 | */ 34 | class MavRos 35 | { 36 | public: 37 | MavRos(); 38 | ~MavRos() {}; 39 | 40 | void spin(); 41 | 42 | private: 43 | ros::NodeHandle mavlink_nh; 44 | // fcu_link stored in mav_uas 45 | mavconn::MAVConnInterface::Ptr gcs_link; 46 | bool gcs_quiet_mode; 47 | ros::Time last_message_received_from_gcs; 48 | ros::Duration conn_timeout; 49 | 50 | ros::Publisher mavlink_pub; 51 | ros::Subscriber mavlink_sub; 52 | 53 | diagnostic_updater::Updater gcs_diag_updater; 54 | MavlinkDiag fcu_link_diag; 55 | MavlinkDiag gcs_link_diag; 56 | 57 | pluginlib::ClassLoader plugin_loader; 58 | std::vector loaded_plugins; 59 | 60 | //! FCU link -> router -> plugin handler 61 | std::unordered_map plugin_subscriptions; 62 | 63 | //! UAS object passed to all plugins 64 | UAS mav_uas; 65 | 66 | //! fcu link -> ros 67 | void mavlink_pub_cb(const mavlink::mavlink_message_t *mmsg, const mavconn::Framing framing); 68 | //! ros -> fcu link 69 | void mavlink_sub_cb(const mavros_msgs::Mavlink::ConstPtr &rmsg); 70 | 71 | //! message router 72 | void plugin_route_cb(const mavlink::mavlink_message_t *mmsg, const mavconn::Framing framing); 73 | 74 | //! load plugin 75 | void add_plugin(std::string &pl_name, ros::V_string &blacklist, ros::V_string &whitelist); 76 | 77 | //! start mavlink app on USB 78 | void startup_px4_usb_quirk(); 79 | void log_connect_change(bool connected); 80 | }; 81 | } // namespace mavros 82 | 83 | -------------------------------------------------------------------------------- /mavros/include/mavros/utils.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief some useful utils 3 | * @file utils.h 4 | * @author Vladimir Ermakov 5 | * 6 | * @addtogroup mavutils 7 | * @{ 8 | * @brief Some useful utils 9 | */ 10 | /* 11 | * Copyright 2014,2015,2016 Vladimir Ermakov. 12 | * 13 | * This file is part of the mavros package and subject to the license terms 14 | * in the top-level LICENSE file of the mavros repository. 15 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 16 | */ 17 | 18 | #pragma once 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | 27 | // OS X compat: missing error codes 28 | #ifdef __APPLE__ 29 | #define EBADE 50 /* Invalid exchange */ 30 | #define EBADFD 81 /* File descriptor in bad state */ 31 | #define EBADRQC 54 /* Invalid request code */ 32 | #define EBADSLT 55 /* Invalid slot */ 33 | #endif 34 | 35 | namespace mavros { 36 | namespace utils { 37 | using mavconn::utils::format; 38 | 39 | /** 40 | * Possible modes of timesync operation 41 | * 42 | * Used by UAS class, but it can't be defined inside because enum is used in utils. 43 | */ 44 | enum class timesync_mode { 45 | NONE = 0, //!< Disabled 46 | MAVLINK, //!< Via TIMESYNC message 47 | ONBOARD, 48 | PASSTHROUGH, 49 | }; 50 | 51 | /** 52 | * Helper to get enum value from strongly typed enum (enum class). 53 | */ 54 | template 55 | constexpr typename std::underlying_type<_T>::type enum_value(_T e) 56 | { 57 | return static_cast::type>(e); 58 | } 59 | 60 | /** 61 | * Get string repr for timesync_mode 62 | */ 63 | std::string to_string(timesync_mode e); 64 | 65 | /** 66 | * @brief Retrieve alias of the orientation received by MAVLink msg. 67 | */ 68 | std::string to_string(mavlink::common::MAV_SENSOR_ORIENTATION e); 69 | 70 | std::string to_string(mavlink::common::MAV_AUTOPILOT e); 71 | std::string to_string(mavlink::common::MAV_TYPE e); 72 | std::string to_string(mavlink::common::MAV_STATE e); 73 | std::string to_string(mavlink::common::MAV_COMPONENT e); 74 | std::string to_string(mavlink::common::MAV_ESTIMATOR_TYPE e); 75 | std::string to_string(mavlink::common::ADSB_ALTITUDE_TYPE e); 76 | std::string to_string(mavlink::common::ADSB_EMITTER_TYPE e); 77 | std::string to_string(mavlink::common::MAV_MISSION_RESULT e); 78 | std::string to_string(mavlink::common::MAV_FRAME e); 79 | std::string to_string(mavlink::common::MAV_DISTANCE_SENSOR e); 80 | std::string to_string(mavlink::common::LANDING_TARGET_TYPE e); 81 | /** 82 | * Helper to call to_string() for enum _T 83 | */ 84 | template 85 | std::string to_string_enum(int e) 86 | { 87 | return to_string(static_cast<_T>(e)); 88 | } 89 | 90 | /** 91 | * @brief Function to match the received orientation received by MAVLink msg 92 | * and the rotation of the sensor relative to the FCU. 93 | */ 94 | Eigen::Quaterniond sensor_orientation_matching(mavlink::common::MAV_SENSOR_ORIENTATION orientation); 95 | 96 | /** 97 | * @brief Retrieve sensor orientation number from alias name. 98 | */ 99 | int sensor_orientation_from_str(const std::string &sensor_orientation); 100 | 101 | /** 102 | * @brief Retrieve timesync mode from name 103 | */ 104 | timesync_mode timesync_mode_from_str(const std::string &mode); 105 | 106 | /** 107 | * @brief Retreive MAV_FRAME from name 108 | */ 109 | mavlink::common::MAV_FRAME mav_frame_from_str(const std::string &mav_frame); 110 | 111 | /** 112 | * @brief Retreive MAV_TYPE from name 113 | */ 114 | mavlink::common::MAV_TYPE mav_type_from_str(const std::string &mav_type); 115 | 116 | /** 117 | * @brief Retrieve landing target type from alias name 118 | */ 119 | mavlink::common::LANDING_TARGET_TYPE landing_target_type_from_str(const std::string &landing_target_type); 120 | 121 | } // namespace utils 122 | } // namespace mavros 123 | -------------------------------------------------------------------------------- /mavros/launch/apm.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /mavros/launch/apm2.launch: -------------------------------------------------------------------------------- 1 | apm.launch -------------------------------------------------------------------------------- /mavros/launch/apm_pluginlists.yaml: -------------------------------------------------------------------------------- 1 | plugin_blacklist: 2 | # common 3 | - actuator_control 4 | - ftp 5 | - safety_area 6 | - hil 7 | # extras 8 | - altitude 9 | - debug_value 10 | - image_pub 11 | - px4flow 12 | - vibration 13 | - vision_speed_estimate 14 | - wheel_odometry 15 | 16 | plugin_whitelist: [] 17 | #- 'sys_*' 18 | -------------------------------------------------------------------------------- /mavros/launch/event_launcher.yaml: -------------------------------------------------------------------------------- 1 | # Example configuration for event_launcher 2 | # vim:set ts=2 sw=2 et: 3 | 4 | # Notify on arming change 5 | arming_notify: # name of process, used only for logging 6 | event: armed, disarmed # prefer to split by comma 7 | action: run run # but space are fine too 8 | shell: notify-send 'UAV Arming status' 'Has changed' # maybe later that will provide some environment variables 9 | 10 | # you may define custom action source (std_srvs/Trigger) 11 | mytrigger: # this was name of event source 12 | service: ~trigger_notify 13 | 14 | trigger_notify: 15 | event: mytrigger 16 | action: run # trigger generate only one event 17 | shell: notify-send 'My Trigger' 'Has called' 18 | 19 | # you may use several event sources 20 | othertrigger: 21 | service: ~trigger_stop 22 | 23 | trigger_notify2: 24 | event: mytrigger, othertrigger 25 | action: run, stop # stop does nothin in this example, because notify-send don't blocks 26 | shell: # array will simplify parsing step 27 | - notify-send 28 | - Trigger \#2 29 | - Started 30 | 31 | # rosbag example 32 | start_recorder: 33 | service: start_recorder 34 | 35 | start_talker: 36 | service: start_talker 37 | 38 | start_launch: 39 | service: start_launch 40 | 41 | stop_all: 42 | service: stop_all 43 | 44 | wireshark: 45 | service: run_wireshark 46 | 47 | # env-variables are expanded for first element 48 | trigger_wireshark: 49 | event: wireshark, stop_all 50 | action: run, stop 51 | shell: 52 | - $HOME/bin/al76-wireshark.sh # will expand 53 | - $PWD # nope 54 | 55 | rosbag_armed: 56 | event: [armed, disarmed, start_recorder, stop_all] # yaml array is fine too 57 | action: run, stop, run, stop 58 | shell: rosrun rosbag record -a -x '/mavlink/.*' --lz4 -o 'uav' 59 | 60 | rosrun_talker: 61 | event: start_talker, stop_all 62 | action: run, stop 63 | shell: rosrun roscpp_tutorials talker 64 | 65 | roslaunch_triggered: 66 | event: start_launch, stop_all 67 | action: run, stop 68 | logfile: /tmp/launch.log 69 | shell: roslaunch roscpp_tutorials talker_listener.launch 70 | -------------------------------------------------------------------------------- /mavros/launch/mavlink_bridge.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /mavros/launch/node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /mavros/launch/px4.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /mavros/launch/px4_pluginlists.yaml: -------------------------------------------------------------------------------- 1 | plugin_blacklist: 2 | # common 3 | - safety_area 4 | # extras 5 | - image_pub 6 | - vibration 7 | - distance_sensor 8 | - rangefinder 9 | - home_position 10 | - wheel_odometry 11 | - obstacle_distance 12 | - log_transfer 13 | - fake_gps 14 | - px4flow 15 | - wind_estimation 16 | - vision_speed_estimate 17 | - trajectory 18 | 19 | 20 | plugin_whitelist: [] 21 | #- 'sys_*' 22 | -------------------------------------------------------------------------------- /mavros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mavros 4 | 0.33.3 5 | 6 | MAVROS -- MAVLink extendable communication node for ROS 7 | with proxy for Ground Control Station. 8 | 9 | 10 | Vladimir Ermakov 11 | Vladimir Ermakov 12 | GPLv3 13 | LGPLv3 14 | BSD 15 | 16 | http://wiki.ros.org/mavros 17 | https://github.com/mavlink/mavros.git 18 | https://github.com/mavlink/mavros/issues 19 | 20 | catkin 21 | 22 | 23 | boost 24 | eigen 25 | eigen 26 | mavlink 27 | mavlink 28 | geographiclib 29 | geographiclib 30 | geographiclib-tools 31 | geographiclib-tools 32 | 35 | 36 | 37 | angles 38 | cmake_modules 39 | diagnostic_updater 40 | eigen_conversions 41 | libmavconn 42 | pluginlib 43 | rosconsole_bridge 44 | roscpp 45 | tf2_ros 46 | tf2_eigen 47 | message_runtime 48 | rospy 49 | 50 | 51 | diagnostic_msgs 52 | geometry_msgs 53 | mavros_msgs 54 | nav_msgs 55 | sensor_msgs 56 | geographic_msgs 57 | trajectory_msgs 58 | std_msgs 59 | std_srvs 60 | mav_msgs 61 | 62 | rosunit 63 | gtest 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | uav 72 | mav 73 | mavlink 74 | plugin 75 | apm 76 | px4 77 | 78 | 79 | 80 | 81 | -------------------------------------------------------------------------------- /mavros/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | exclude_patterns: '*/.git/* */test/*' 3 | -------------------------------------------------------------------------------- /mavros/scripts/event_launcher: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # vim:set ts=4 sw=4 et: 3 | # 4 | # Copyright 2015 Vladimir Ermakov. 5 | # 6 | # This file is part of the mavros package and subject to the license terms 7 | # in the top-level LICENSE file of the mavros repository. 8 | # https://github.com/mavlink/mavros/tree/master/LICENSE.md 9 | 10 | from mavros import event_launcher 11 | event_launcher.main() 12 | -------------------------------------------------------------------------------- /mavros/scripts/install_geographiclib_datasets.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Script to install the model datasets required 3 | # to GeographicLib apply certain conversions 4 | 5 | if [[ $UID != 0 ]]; then 6 | echo "This script require root privileges!" 1>&2 7 | exit 1 8 | fi 9 | 10 | # Install datasets 11 | run_get() { 12 | local dir="$1" 13 | local tool="$2" 14 | local model="$3" 15 | 16 | files=$(shopt -s nullglob dotglob; echo /usr/share/GeographicLib/$dir/$model* /usr/local/share/GeographicLib/$dir/$model*) 17 | if (( ${#files} )); then 18 | echo "GeographicLib $tool dataset $model already exists, skipping" 19 | return 20 | fi 21 | 22 | echo "Installing GeographicLib $tool $model" 23 | geographiclib-get-$tool $model >/dev/null 2>&1 24 | } 25 | 26 | # check which command script is available 27 | if hash geographiclib-get-geoids; then 28 | run_get geoids geoids egm96-5 29 | run_get gravity gravity egm96 30 | run_get magnetic magnetic emm2015 31 | elif hash geographiclib-datasets-download; then # only allows install the goid model dataset 32 | geographiclib-datasets-download egm96_5; 33 | else 34 | echo "OS not supported! Check GeographicLib page for supported OS and lib versions." 1>&2 35 | fi 36 | -------------------------------------------------------------------------------- /mavros/scripts/mavsafety: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # vim:set ts=4 sw=4 et: 3 | # 4 | # Copyright 2014 Vladimir Ermakov. 5 | # 6 | # This file is part of the mavros package and subject to the license terms 7 | # in the top-level LICENSE file of the mavros repository. 8 | # https://github.com/mavlink/mavros/tree/master/LICENSE.md 9 | 10 | from __future__ import print_function 11 | 12 | import argparse 13 | 14 | import rospy 15 | import mavros 16 | from mavros.utils import * 17 | from geometry_msgs.msg import PolygonStamped, Point32 18 | from mavros import command 19 | 20 | 21 | def _arm(args, state): 22 | try: 23 | ret = command.arming(value=state) 24 | except rospy.ServiceException as ex: 25 | fault(ex) 26 | 27 | if not ret.success: 28 | fault("Request failed. Check mavros logs") 29 | 30 | print_if(args.verbose, "Command result:", ret.result) 31 | return ret 32 | 33 | 34 | def do_arm(args): 35 | _arm(args, True) 36 | 37 | 38 | def do_disarm(args): 39 | _arm(args, False) 40 | 41 | 42 | _ONCE_DELAY = 3 43 | def do_safetyarea(args): 44 | set_topic = mavros.get_topic('safety_area', 'set') 45 | pub = rospy.Publisher(set_topic, PolygonStamped, 46 | queue_size=10, latch=True) 47 | 48 | poly = PolygonStamped() 49 | poly.header.frame_id = 'mavsafety' 50 | poly.header.stamp = rospy.get_rostime() 51 | poly.polygon.points = [ 52 | Point32(x=args.p1[0], y=args.p1[1], z=args.p1[2]), 53 | Point32(x=args.p2[0], y=args.p2[1], z=args.p2[2]), 54 | ] 55 | 56 | pub.publish(poly) 57 | # XXX maybe not worked 58 | print_if(pub.get_num_connections() < 1, 59 | "Mavros not started, nobody subcsribes to ", set_topic) 60 | 61 | # stick around long enough for others to grab 62 | timeout_t = rospy.get_time() + _ONCE_DELAY 63 | while not rospy.is_shutdown() and rospy.get_time() < timeout_t: 64 | rospy.sleep(0.2) 65 | 66 | 67 | def main(): 68 | parser = argparse.ArgumentParser(description="Command line tool for manipulating safety on MAVLink device.") 69 | parser.add_argument('-n', '--mavros-ns', help="ROS node namespace", default=mavros.DEFAULT_NAMESPACE) 70 | parser.add_argument('-v', '--verbose', action='store_true', help="Verbose output") 71 | subarg = parser.add_subparsers() 72 | 73 | arm_args = subarg.add_parser('arm', help="Arm motors") 74 | arm_args.set_defaults(func=do_arm) 75 | 76 | disarm_args = subarg.add_parser('disarm', help="Disarm motors") 77 | disarm_args.set_defaults(func=do_disarm) 78 | 79 | safety_area_args = subarg.add_parser('safetyarea', help="Send safety area") 80 | safety_area_args.set_defaults(func=do_safetyarea) 81 | safety_area_args.add_argument('-p1', type=float, nargs=3, metavar=('x', 'y', 'z'), 82 | required=True, help="Corner 1") 83 | safety_area_args.add_argument('-p2', type=float, nargs=3, metavar=('x', 'y', 'z'), 84 | required=True, help="Corner 2") 85 | 86 | args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:]) 87 | 88 | rospy.init_node("mavsafety", anonymous=True) 89 | mavros.set_namespace(args.mavros_ns) 90 | args.func(args) 91 | 92 | 93 | if __name__ == '__main__': 94 | main() 95 | -------------------------------------------------------------------------------- /mavros/setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | packages=['mavros'], 9 | package_dir={'': 'src'}) 10 | 11 | setup(**setup_args) 12 | -------------------------------------------------------------------------------- /mavros/src/gcs_bridge.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief MAVROS GCS proxy 3 | * @file gcs_bridge.cpp 4 | * @author Vladimir Ermakov 5 | */ 6 | /* 7 | * Copyright 2014,2016 Vladimir Ermakov. 8 | * 9 | * This file is part of the mavros package and subject to the license terms 10 | * in the top-level LICENSE file of the mavros repository. 11 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 12 | */ 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | using namespace mavros; 22 | using namespace mavconn; 23 | 24 | ros::Publisher mavlink_pub; 25 | ros::Subscriber mavlink_sub; 26 | MAVConnInterface::Ptr gcs_link; 27 | 28 | 29 | void mavlink_pub_cb(const mavlink::mavlink_message_t *mmsg, const mavconn::Framing framing) 30 | { 31 | auto rmsg = boost::make_shared(); 32 | 33 | rmsg->header.stamp = ros::Time::now(); 34 | mavros_msgs::mavlink::convert(*mmsg, *rmsg, mavros::utils::enum_value(framing)); 35 | mavlink_pub.publish(rmsg); 36 | } 37 | 38 | void mavlink_sub_cb(const mavros_msgs::Mavlink::ConstPtr &rmsg) 39 | { 40 | mavlink::mavlink_message_t mmsg; 41 | 42 | if (mavros_msgs::mavlink::convert(*rmsg, mmsg)) 43 | gcs_link->send_message(&mmsg); // !!! queue exception -> fall of gcs_bridge. intentional. 44 | else 45 | ROS_ERROR("Packet drop: convert error."); 46 | } 47 | 48 | int main(int argc, char *argv[]) 49 | { 50 | ros::init(argc, argv, "gcs_bridge"); 51 | ros::NodeHandle priv_nh("~"); 52 | ros::NodeHandle mavlink_nh("mavlink"); 53 | diagnostic_updater::Updater updater; 54 | mavros::MavlinkDiag gcs_link_diag("GCS bridge"); 55 | 56 | std::string gcs_url; 57 | priv_nh.param("gcs_url", gcs_url, "udp://@"); 58 | 59 | try { 60 | gcs_link = MAVConnInterface::open_url(gcs_url); 61 | gcs_link_diag.set_mavconn(gcs_link); 62 | gcs_link_diag.set_connection_status(true); 63 | } 64 | catch (mavconn::DeviceError &ex) { 65 | ROS_FATAL("GCS: %s", ex.what()); 66 | return 1; 67 | } 68 | 69 | mavlink_pub = mavlink_nh.advertise("to", 10); 70 | gcs_link->message_received_cb = mavlink_pub_cb; 71 | 72 | // prefer UDPROS, but allow TCPROS too 73 | mavlink_sub = mavlink_nh.subscribe("from", 10, mavlink_sub_cb, 74 | ros::TransportHints() 75 | .unreliable().maxDatagramSize(1024) 76 | .reliable()); 77 | 78 | // setup updater 79 | updater.setHardwareID(gcs_url); 80 | updater.add(gcs_link_diag); 81 | 82 | // updater spinner 83 | auto diag_timer = priv_nh.createTimer(ros::Duration(0.5), 84 | [&](const ros::TimerEvent &evt) { 85 | updater.update(); 86 | }); 87 | diag_timer.start(); 88 | 89 | ros::spin(); 90 | return 0; 91 | } 92 | -------------------------------------------------------------------------------- /mavros/src/lib/ftf_quaternion_utils.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief Eigen::Quaternion helter functions 3 | * @file uas_quaternion_utils.cpp 4 | * @author Vladimir Ermakov 5 | * 6 | * @addtogroup nodelib 7 | * @{ 8 | */ 9 | /* 10 | * Copyright 2015,2016 Vladimir Ermakov. 11 | * 12 | * This file is part of the mavros package and subject to the license terms 13 | * in the top-level LICENSE file of the mavros repository. 14 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 15 | */ 16 | #include 17 | 18 | namespace mavros { 19 | namespace ftf { 20 | 21 | /* 22 | * Note: order of axis are match tf2::LinearMath (bullet). 23 | * YPR rotation convention -> YAW first, Pitch second, Roll third 24 | * Compatibility checked by unittests. 25 | */ 26 | 27 | Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d &rpy) 28 | { 29 | // YPR - ZYX 30 | return Eigen::Quaterniond( 31 | Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * 32 | Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * 33 | Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()) 34 | ); 35 | } 36 | 37 | Eigen::Vector3d quaternion_to_rpy(const Eigen::Quaterniond &q) 38 | { 39 | // YPR - ZYX 40 | return q.toRotationMatrix().eulerAngles(2, 1, 0).reverse(); 41 | } 42 | 43 | double quaternion_get_yaw(const Eigen::Quaterniond &q) 44 | { 45 | // to match equation from: 46 | // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles 47 | const double &q0 = q.w(); 48 | const double &q1 = q.x(); 49 | const double &q2 = q.y(); 50 | const double &q3 = q.z(); 51 | 52 | return std::atan2(2. * (q0*q3 + q1*q2), 1. - 2. * (q2*q2 + q3*q3)); 53 | } 54 | 55 | } // namesapace ftf 56 | } // namesapace mavros 57 | -------------------------------------------------------------------------------- /mavros/src/lib/mavlink_diag.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief Mavlink diag class 3 | * @file mavlink_diag.cpp 4 | * @author Vladimir Ermakov 5 | */ 6 | /* 7 | * Copyright 2014,2015 Vladimir Ermakov. 8 | * 9 | * This file is part of the mavros package and subject to the license terms 10 | * in the top-level LICENSE file of the mavros repository. 11 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 12 | */ 13 | 14 | #include 15 | 16 | using namespace mavros; 17 | 18 | MavlinkDiag::MavlinkDiag(std::string name) : 19 | diagnostic_updater::DiagnosticTask(name), 20 | last_drop_count(0), 21 | is_connected(false) 22 | { }; 23 | 24 | void MavlinkDiag::run(diagnostic_updater::DiagnosticStatusWrapper &stat) 25 | { 26 | if (auto link = weak_link.lock()) { 27 | auto mav_status = link->get_status(); 28 | auto iostat = link->get_iostat(); 29 | 30 | stat.addf("Received packets:", "%u", mav_status.packet_rx_success_count); 31 | stat.addf("Dropped packets:", "%u", mav_status.packet_rx_drop_count); 32 | stat.addf("Buffer overruns:", "%u", mav_status.buffer_overrun); 33 | stat.addf("Parse errors:", "%u", mav_status.parse_error); 34 | stat.addf("Rx sequence number:", "%u", mav_status.current_rx_seq); 35 | stat.addf("Tx sequence number:", "%u", mav_status.current_tx_seq); 36 | 37 | stat.addf("Rx total bytes:", "%u", iostat.rx_total_bytes); 38 | stat.addf("Tx total bytes:", "%u", iostat.tx_total_bytes); 39 | stat.addf("Rx speed:", "%f", iostat.rx_speed); 40 | stat.addf("Tx speed:", "%f", iostat.tx_speed); 41 | 42 | if (mav_status.packet_rx_drop_count > last_drop_count) 43 | stat.summaryf(1, "%d packeges dropped since last report", 44 | mav_status.packet_rx_drop_count - last_drop_count); 45 | else if (is_connected) 46 | stat.summary(0, "connected"); 47 | else 48 | // link operational, but not connected 49 | stat.summary(1, "not connected"); 50 | 51 | last_drop_count = mav_status.packet_rx_drop_count; 52 | } else { 53 | stat.summary(2, "not connected"); 54 | } 55 | } 56 | 57 | -------------------------------------------------------------------------------- /mavros/src/lib/rosconsole_bridge.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief Enable console bridge 3 | * @file rosconsole_bridge.cpp 4 | * @author Vladimir Ermakov 5 | */ 6 | /* 7 | * Copyright 2014 Vladimir Ermakov. 8 | * 9 | * This file is part of the mavros package and subject to the license terms 10 | * in the top-level LICENSE file of the mavros repository. 11 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 12 | */ 13 | 14 | #include 15 | 16 | REGISTER_ROSCONSOLE_BRIDGE; 17 | -------------------------------------------------------------------------------- /mavros/src/lib/uas_timesync.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief MAVROS UAS manager 3 | * @file uas.cpp 4 | * @author Vladimir Ermakov 5 | */ 6 | /* 7 | * Copyright 2014,2017 Vladimir Ermakov, M.H.Kabir. 8 | * 9 | * This file is part of the mavros package and subject to the license terms 10 | * in the top-level LICENSE file of the mavros repository. 11 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 12 | */ 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | using namespace mavros; 22 | 23 | 24 | /* -*- time syncronise functions -*- */ 25 | 26 | static inline ros::Time ros_time_from_ns(const uint64_t stamp_ns) { 27 | return ros::Time( 28 | stamp_ns / 1000000000UL, // t_sec 29 | stamp_ns % 1000000000UL); // t_nsec 30 | } 31 | 32 | ros::Time UAS::synchronise_stamp(uint32_t time_boot_ms) { 33 | // copy offset from atomic var 34 | uint64_t offset_ns = time_offset; 35 | 36 | if (offset_ns > 0 || tsync_mode == timesync_mode::PASSTHROUGH) { 37 | uint64_t stamp_ns = static_cast(time_boot_ms) * 1000000UL + offset_ns; 38 | return ros_time_from_ns(stamp_ns); 39 | } 40 | else 41 | return ros::Time::now(); 42 | } 43 | 44 | ros::Time UAS::synchronise_stamp(uint64_t time_usec) { 45 | uint64_t offset_ns = time_offset; 46 | 47 | if (offset_ns > 0 || tsync_mode == timesync_mode::PASSTHROUGH) { 48 | uint64_t stamp_ns = time_usec * 1000UL + offset_ns; 49 | return ros_time_from_ns(stamp_ns); 50 | } 51 | else 52 | return ros::Time::now(); 53 | } 54 | 55 | -------------------------------------------------------------------------------- /mavros/src/mavros/__init__.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # vim:set ts=4 sw=4 et: 3 | 4 | __all__ = ( 5 | 'get_namespace', 6 | 'set_namespace', 7 | 'get_topic', 8 | 'DEFAULT_NAMESPACE' 9 | ) 10 | 11 | DEFAULT_NAMESPACE = 'mavros' 12 | 13 | # global namespace storage 14 | _mavros_ns = None 15 | _mavros_ns_update = [] 16 | 17 | 18 | def get_namespace(): 19 | """ 20 | Returns mavros node namespace 21 | """ 22 | global _mavros_ns 23 | if _mavros_ns is None: 24 | raise RuntimeError("mavros namespace is uninitialized! " 25 | "Call mavros.set_namespace() first!") 26 | return _mavros_ns 27 | 28 | 29 | def set_namespace(ns=DEFAULT_NAMESPACE): 30 | """ 31 | Sets namespace of mavros node 32 | """ 33 | global _mavros_ns, _mavros_ns_update 34 | _mavros_ns = ns 35 | 36 | for cb in _mavros_ns_update: 37 | if callable(cb): 38 | cb() 39 | 40 | 41 | def register_on_namespace_update(cb): 42 | """ 43 | Call callback after namespace update 44 | """ 45 | global _mavros_ns_update 46 | _mavros_ns_update.append(cb) 47 | 48 | 49 | def get_topic(*args): 50 | """ 51 | Create topic name for mavros node 52 | """ 53 | return '/'+'/'.join((get_namespace(), ) + args) 54 | -------------------------------------------------------------------------------- /mavros/src/mavros/command.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # vim:set ts=4 sw=4 et: 3 | # 4 | # Copyright 2014,2015 Vladimir Ermakov. 5 | # 6 | # This file is part of the mavros package and subject to the license terms 7 | # in the top-level LICENSE file of the mavros repository. 8 | # https://github.com/mavlink/mavros/tree/master/LICENSE.md 9 | 10 | __all__ = ( 11 | 'long', 12 | 'int', 13 | 'arming', 14 | 'set_home', 15 | 'takeoff', 16 | 'land', 17 | 'trigger_control', 18 | ) 19 | 20 | import rospy 21 | import mavros 22 | 23 | from mavros_msgs.srv import CommandLong, CommandInt, CommandBool, CommandHome, CommandTOL, CommandTriggerControl, CommandTriggerInterval 24 | 25 | 26 | def _get_proxy(service, type): 27 | return rospy.ServiceProxy(mavros.get_topic('cmd', service), type) 28 | 29 | 30 | long = None 31 | int = None 32 | arming = None 33 | set_home = None 34 | takeoff = None 35 | land = None 36 | trigger_control = None 37 | 38 | 39 | def _setup_services(): 40 | global long, int, arming, set_home, takeoff, land, trigger_control 41 | long = _get_proxy('command', CommandLong) 42 | int = _get_proxy('command_int', CommandInt) 43 | arming = _get_proxy('arming', CommandBool) 44 | set_home = _get_proxy('set_home', CommandHome) 45 | takeoff = _get_proxy('takeoff', CommandTOL) 46 | land = _get_proxy('land', CommandTOL) 47 | trigger_control = _get_proxy('trigger_control', CommandTriggerControl) 48 | trigger_interval = _get_proxy('trigger_interval', CommandTriggerInterval) 49 | 50 | 51 | # register updater 52 | mavros.register_on_namespace_update(_setup_services) 53 | -------------------------------------------------------------------------------- /mavros/src/mavros/nuttx_crc32.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # vim:set ts=4 sw=4 et: 3 | # 4 | # Copyright 2014 Vladimir Ermakov. 5 | # 6 | # This file is part of the mavros package and subject to the license terms 7 | # in the top-level LICENSE file of the mavros repository. 8 | # https://github.com/mavlink/mavros/tree/master/LICENSE.md 9 | 10 | __all__ = ( 11 | 'nuttx_crc32', 12 | ) 13 | 14 | CRC32_TAB = ( 15 | 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, 16 | 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, 17 | 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, 18 | 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, 19 | 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, 20 | 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, 21 | 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, 22 | 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, 23 | 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, 24 | 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, 25 | 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, 26 | 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, 27 | 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, 28 | 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, 29 | 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, 30 | 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, 31 | 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, 32 | 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, 33 | 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, 34 | 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, 35 | 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, 36 | 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, 37 | 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, 38 | 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, 39 | 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, 40 | 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, 41 | 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, 42 | 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, 43 | 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, 44 | 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, 45 | 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, 46 | 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d 47 | ) 48 | 49 | 50 | def nuttx_crc32(bytes, crc32val=0): 51 | """ 52 | CRC32 algo from NuttX. 53 | """ 54 | for b in bytearray(bytes): 55 | crc32val = CRC32_TAB[(crc32val ^ b) & 0xff] ^ (crc32val >> 8) 56 | 57 | return crc32val 58 | -------------------------------------------------------------------------------- /mavros/src/mavros/setpoint.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # vim:set ts=4 sw=4 et: 3 | # 4 | # Copyright 2015 Vladimir Ermakov. 5 | # 6 | # This file is part of the mavros package and subject to the license terms 7 | # in the top-level LICENSE file of the mavros repository. 8 | # https://github.com/mavlink/mavros/tree/master/LICENSE.md 9 | 10 | import rospy 11 | import mavros 12 | 13 | from std_msgs.msg import Header, Float64 14 | from geometry_msgs.msg import TwistStamped, PoseStamped, PoseWithCovarianceStamped, \ 15 | Vector3, Vector3Stamped, Point, Quaternion 16 | 17 | 18 | def get_pub_accel_accel(**kvargs): 19 | """ 20 | Returns publisher for :setpoint_accel: plugin, :accel: topic 21 | """ 22 | return rospy.Publisher(mavros.get_topic('setpoint_accel', 'accel'), Vector3Stamped, **kvargs) 23 | 24 | 25 | def get_pub_attitude_cmd_vel(**kvargs): 26 | """ 27 | Returns publisher for :setpoint_attitude: plugin, :cmd_vel: topic 28 | """ 29 | return rospy.Publisher(mavros.get_topic('setpoint_attitude', 'cmd_vel'), PoseStamped, **kvargs) 30 | 31 | 32 | def get_pub_attitude_throttle(**kvargs): 33 | """ 34 | Returns publisher for :setpoint_attitude: plugin, :cmd_vel: topic 35 | """ 36 | return rospy.Publisher(mavros.get_topic('setpoint_attitude', 'att_throttle'), Float64, **kvargs) 37 | 38 | 39 | def get_pub_attitude_pose(**kvargs): 40 | """ 41 | Returns publisher for :setpoint_attitude: plugin, :attituse: topic 42 | """ 43 | return rospy.Publisher(mavros.get_topic('setpoint_attitude', 'attitude'), PoseStamped, **kvargs) 44 | 45 | 46 | def get_pub_attitude_posecov(**kvargs): 47 | """ 48 | Returns publisher for :setpoint_attitude: plugin, :attituse: topic (with covariance) 49 | """ 50 | raise DeprecationWarning("PoseWithCovarianceStamped subscriber removed.") 51 | 52 | 53 | def get_pub_position_local(**kvargs): 54 | """ 55 | Returns publisher for :setpoint_position: plugin, :local: topic 56 | """ 57 | return rospy.Publisher(mavros.get_topic('setpoint_position', 'local'), PoseStamped, **kvargs) 58 | 59 | 60 | def get_pub_velocity_cmd_vel(**kvargs): 61 | """ 62 | Returns publisher for :setpoint_velocity: plugin, :cmd_vel: topic 63 | """ 64 | return rospy.Publisher(mavros.get_topic('setpoint_velocity', 'cmd_vel'), TwistStamped, **kvargs) 65 | 66 | 67 | -------------------------------------------------------------------------------- /mavros/src/mavros/utils.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # vim:set ts=4 sw=4 et: 3 | # 4 | # Copyright 2014 Vladimir Ermakov. 5 | # 6 | # This file is part of the mavros package and subject to the license terms 7 | # in the top-level LICENSE file of the mavros repository. 8 | # https://github.com/mavlink/mavros/tree/master/LICENSE.md 9 | 10 | from __future__ import print_function 11 | 12 | import os 13 | import sys 14 | 15 | import rospy 16 | import mavros 17 | import threading 18 | 19 | 20 | def print_if(cond, *args, **kvargs): 21 | if cond: 22 | print(*args, **kvargs) 23 | 24 | 25 | def fault(*args, **kvargs): 26 | kvargs['file'] = sys.stderr 27 | print(*args, **kvargs) 28 | sys.exit(1) 29 | 30 | 31 | def wait_fcu_connection(timeout=None): 32 | """ 33 | Wait until establishing FCU connection 34 | """ 35 | from mavros_msgs.msg import State 36 | try: 37 | msg = rospy.wait_for_message(mavros.get_topic('state'), State, timeout) 38 | if msg.connected: 39 | return True 40 | except rospy.ROSException as e: 41 | return False 42 | 43 | connected = threading.Event() 44 | def handler(msg): 45 | if msg.connected: 46 | connected.set() 47 | 48 | sub = rospy.Subscriber( 49 | mavros.get_topic('state'), 50 | State, 51 | handler 52 | ) 53 | 54 | return connected.wait(timeout) 55 | -------------------------------------------------------------------------------- /mavros/src/mavros_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief MAVROS Node 3 | * @file mavros_node.cpp 4 | * @author Vladimir Ermakov 5 | */ 6 | /* 7 | * Copyright 2013,2014,2015 Vladimir Ermakov. 8 | * 9 | * This file is part of the mavros package and subject to the license terms 10 | * in the top-level LICENSE file of the mavros repository. 11 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 12 | */ 13 | 14 | #include 15 | 16 | int main(int argc, char *argv[]) 17 | { 18 | ros::init(argc, argv, "mavros"); 19 | 20 | mavros::MavRos mavros; 21 | mavros.spin(); 22 | 23 | return 0; 24 | } 25 | 26 | -------------------------------------------------------------------------------- /mavros/src/plugins/actuator_control.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief ActuatorControl plugin 3 | * @file actuator_control.cpp 4 | * @author Marcel Stüttgen 5 | * 6 | * @addtogroup plugin 7 | * @{ 8 | */ 9 | /* 10 | * Copyright 2015 Marcel Stüttgen 11 | * 12 | * This file is part of the mavros package and subject to the license terms 13 | * in the top-level LICENSE file of the mavros repository. 14 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 15 | */ 16 | 17 | #include 18 | 19 | #include 20 | 21 | namespace mavros { 22 | namespace std_plugins { 23 | /** 24 | * @brief ActuatorControl plugin 25 | * 26 | * Sends actuator controls to FCU controller. 27 | */ 28 | class ActuatorControlPlugin : public plugin::PluginBase { 29 | public: 30 | ActuatorControlPlugin() : PluginBase(), 31 | nh("~") 32 | { } 33 | 34 | void initialize(UAS &uas_) 35 | { 36 | PluginBase::initialize(uas_); 37 | 38 | target_actuator_control_pub = nh.advertise("target_actuator_control", 10); 39 | actuator_control_sub = nh.subscribe("actuator_control", 10, &ActuatorControlPlugin::actuator_control_cb, this); 40 | } 41 | 42 | Subscriptions get_subscriptions() 43 | { 44 | return { 45 | make_handler(&ActuatorControlPlugin::handle_actuator_control_target), 46 | }; 47 | } 48 | 49 | private: 50 | ros::NodeHandle nh; 51 | 52 | ros::Publisher target_actuator_control_pub; 53 | ros::Subscriber actuator_control_sub; 54 | 55 | /* -*- rx handlers -*- */ 56 | 57 | void handle_actuator_control_target(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ACTUATOR_CONTROL_TARGET &actuator_control_target) 58 | { 59 | auto actuator_control_target_msg = boost::make_shared(); 60 | actuator_control_target_msg->header.stamp = m_uas->synchronise_stamp(actuator_control_target.time_usec); 61 | 62 | actuator_control_target_msg->group_mix = actuator_control_target.group_mlx; 63 | const auto &arr = actuator_control_target.controls; 64 | std::copy(arr.cbegin(), arr.cend(), actuator_control_target_msg->controls.begin()); 65 | 66 | target_actuator_control_pub.publish(actuator_control_target_msg); 67 | } 68 | 69 | /* -*- callbacks -*- */ 70 | 71 | void actuator_control_cb(const mavros_msgs::ActuatorControl::ConstPtr &req) { 72 | //! about groups, mixing and channels: @p https://pixhawk.org/dev/mixing 73 | //! message definiton here: @p https://mavlink.io/en/messages/common.html#SET_ACTUATOR_CONTROL_TARGET 74 | mavlink::common::msg::SET_ACTUATOR_CONTROL_TARGET act{}; 75 | 76 | act.time_usec = req->header.stamp.toNSec() / 1000; 77 | act.group_mlx = req->group_mix; 78 | act.target_system = m_uas->get_tgt_system(); 79 | act.target_component = m_uas->get_tgt_component(); 80 | std::copy(req->controls.begin(), req->controls.end(), act.controls.begin()); // std::array = boost::array 81 | 82 | UAS_FCU(m_uas)->send_message_ignore_drop(act); 83 | } 84 | }; 85 | } // namespace std_plugins 86 | } // namespace mavros 87 | 88 | #include 89 | PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::ActuatorControlPlugin, mavros::plugin::PluginBase) 90 | -------------------------------------------------------------------------------- /mavros/src/plugins/altitude.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief Altitude plugin 3 | * @file altitude.cpp 4 | * @author Andreas Antener 5 | * 6 | * @addtogroup plugin 7 | * @{ 8 | */ 9 | /* 10 | * Copyright 2015 Andreas Antener . 11 | * 12 | * This file is part of the mavros package and subject to the license terms 13 | * in the top-level LICENSE file of the mavros repository. 14 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 15 | */ 16 | 17 | #include 18 | 19 | #include 20 | 21 | namespace mavros { 22 | namespace std_plugins { 23 | /** 24 | * @brief Altitude plugin. 25 | */ 26 | class AltitudePlugin : public plugin::PluginBase { 27 | public: 28 | AltitudePlugin() : PluginBase(), 29 | nh("~") 30 | { } 31 | 32 | /** 33 | * Plugin initializer. Constructor should not do this. 34 | */ 35 | void initialize(UAS &uas_) 36 | { 37 | PluginBase::initialize(uas_); 38 | 39 | nh.param("frame_id", frame_id, "map"); 40 | altitude_pub = nh.advertise("altitude", 10); 41 | } 42 | 43 | Subscriptions get_subscriptions() 44 | { 45 | return { 46 | make_handler(&AltitudePlugin::handle_altitude), 47 | }; 48 | } 49 | 50 | private: 51 | ros::NodeHandle nh; 52 | std::string frame_id; 53 | 54 | ros::Publisher altitude_pub; 55 | 56 | void handle_altitude(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ALTITUDE &altitude) 57 | { 58 | auto ros_msg = boost::make_shared(); 59 | ros_msg->header = m_uas->synchronized_header(frame_id, altitude.time_usec); 60 | 61 | ros_msg->monotonic = altitude.altitude_monotonic; 62 | ros_msg->amsl = altitude.altitude_amsl; 63 | ros_msg->local = altitude.altitude_local; 64 | ros_msg->relative = altitude.altitude_relative; 65 | ros_msg->terrain = altitude.altitude_terrain; 66 | ros_msg->bottom_clearance = altitude.bottom_clearance; 67 | 68 | altitude_pub.publish(ros_msg); 69 | } 70 | }; 71 | } // namespace std_plugins 72 | } // namespace mavros 73 | 74 | #include 75 | PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::AltitudePlugin, mavros::plugin::PluginBase) 76 | -------------------------------------------------------------------------------- /mavros/src/plugins/dummy.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief Dummy plugin 3 | * @file dummy.cpp 4 | * @author Vladimir Ermakov 5 | * 6 | * @example dummy.cpp 7 | * @addtogroup plugin 8 | * @{ 9 | */ 10 | /* 11 | * Copyright 2013,2016 Vladimir Ermakov. 12 | * 13 | * This file is part of the mavros package and subject to the license terms 14 | * in the top-level LICENSE file of the mavros repository. 15 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 16 | */ 17 | 18 | #include 19 | 20 | namespace mavros { 21 | namespace std_plugins { 22 | 23 | /** 24 | * @brief Dummy plugin. 25 | * 26 | * Example and "how to" for users. 27 | */ 28 | class DummyPlugin : public plugin::PluginBase { 29 | public: 30 | DummyPlugin() : PluginBase(), 31 | nh("~") 32 | { } 33 | 34 | /** 35 | * Plugin initializer. Constructor should not do this. 36 | */ 37 | void initialize(UAS &uas_) 38 | { 39 | PluginBase::initialize(uas_); 40 | 41 | ROS_INFO_NAMED("dummy", "Dummy::initialize"); 42 | } 43 | 44 | /** 45 | * This function returns message subscriptions. 46 | * 47 | * Each subscription made by PluginBase::make_handler() template. 48 | * Two variations: 49 | * - With automatic decoding and framing error filtering (see handle_heartbeat) 50 | * - Raw message with framig status (see handle_systemtext) 51 | */ 52 | Subscriptions get_subscriptions() { 53 | return { 54 | /* automatic message deduction by second argument */ 55 | make_handler(&DummyPlugin::handle_heartbeat), 56 | make_handler(&DummyPlugin::handle_sys_status), 57 | /* handle raw message, check framing! */ 58 | make_handler(mavlink::common::msg::STATUSTEXT::MSG_ID, &DummyPlugin::handle_statustext_raw), 59 | make_handler(&DummyPlugin::handle_statustext), 60 | }; 61 | } 62 | 63 | private: 64 | ros::NodeHandle nh; 65 | 66 | void handle_heartbeat(const mavlink::mavlink_message_t *msg, mavlink::common::msg::HEARTBEAT &hb) { 67 | ROS_INFO_STREAM_NAMED("dummy", "Dummy::handle_heartbeat: " << hb.to_yaml()); 68 | } 69 | 70 | void handle_sys_status(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SYS_STATUS &st) { 71 | ROS_INFO_STREAM_NAMED("dummy", "Dummy::handle_sys_status: " << st.to_yaml()); 72 | } 73 | 74 | void handle_statustext(const mavlink::mavlink_message_t *msg, mavlink::common::msg::STATUSTEXT &st) { 75 | ROS_INFO_STREAM_NAMED("dummy", "Dummy::handle_statustext: " << st.to_yaml()); 76 | } 77 | 78 | void handle_statustext_raw(const mavlink::mavlink_message_t *msg, const mavconn::Framing f) { 79 | ROS_INFO_NAMED("dummy", "Dummy::handle_statustext_raw(%p, %d) from %u.%u", msg, utils::enum_value(f), msg->sysid, msg->compid); 80 | } 81 | }; 82 | 83 | } // namespace std_plugins 84 | } // namespace mavros 85 | 86 | #include 87 | PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::DummyPlugin, mavros::plugin::PluginBase) 88 | -------------------------------------------------------------------------------- /mavros/src/plugins/manual_control.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief ManualControls plugin 3 | * @file manual_controls.cpp 4 | * @author Matias Nitsche 5 | * 6 | * @addtogroup plugin 7 | * @{ 8 | */ 9 | /* 10 | * Copyright 2015 Matias Nitsche. 11 | * 12 | * This file is part of the mavros package and subject to the license terms 13 | * in the top-level LICENSE file of the mavros repository. 14 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 15 | */ 16 | 17 | #include 18 | 19 | #include 20 | 21 | namespace mavros { 22 | namespace std_plugins { 23 | /** 24 | * @brief Manual Control plugin 25 | */ 26 | class ManualControlPlugin : public plugin::PluginBase { 27 | public: 28 | ManualControlPlugin() : PluginBase(), 29 | manual_control_nh("~manual_control") 30 | { } 31 | 32 | void initialize(UAS &uas_) 33 | { 34 | PluginBase::initialize(uas_); 35 | 36 | control_pub = manual_control_nh.advertise("control", 10); 37 | send_sub = manual_control_nh.subscribe("send", 1, &ManualControlPlugin::send_cb, this); 38 | } 39 | 40 | Subscriptions get_subscriptions() { 41 | return { 42 | make_handler(&ManualControlPlugin::handle_manual_control), 43 | }; 44 | } 45 | 46 | private: 47 | ros::NodeHandle manual_control_nh; 48 | 49 | ros::Publisher control_pub; 50 | ros::Subscriber send_sub; 51 | 52 | /* -*- rx handlers -*- */ 53 | 54 | void handle_manual_control(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MANUAL_CONTROL &manual_control) 55 | { 56 | auto manual_control_msg = boost::make_shared(); 57 | 58 | manual_control_msg->header.stamp = ros::Time::now(); 59 | manual_control_msg->x = (manual_control.x / 1000.0); 60 | manual_control_msg->y = (manual_control.y / 1000.0); 61 | manual_control_msg->z = (manual_control.z / 1000.0); 62 | manual_control_msg->r = (manual_control.r / 1000.0); 63 | manual_control_msg->buttons = manual_control.buttons; 64 | 65 | control_pub.publish(manual_control_msg); 66 | } 67 | 68 | /* -*- callbacks -*- */ 69 | 70 | void send_cb(const mavros_msgs::ManualControl::ConstPtr req) 71 | { 72 | mavlink::common::msg::MANUAL_CONTROL msg; 73 | msg.target = m_uas->get_tgt_system(); 74 | 75 | msg.x = req->x; 76 | msg.y = req->y; 77 | msg.z = req->z; 78 | msg.r = req->r; 79 | msg.buttons = req->buttons; 80 | 81 | UAS_FCU(m_uas)->send_message_ignore_drop(msg); 82 | } 83 | }; 84 | } // namespace std_plugins 85 | } // namespace mavros 86 | 87 | #include 88 | PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::ManualControlPlugin, mavros::plugin::PluginBase) 89 | -------------------------------------------------------------------------------- /mavros/src/plugins/setpoint_accel.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief SetpointAcceleration plugin 3 | * @file setpoint_accel.cpp 4 | * @author Nuno Marques 5 | * @author Vladimir Ermakov 6 | * 7 | * @addtogroup plugin 8 | * @{ 9 | */ 10 | /* 11 | * Copyright 2014 Nuno Marques. 12 | * 13 | * This file is part of the mavros package and subject to the license terms 14 | * in the top-level LICENSE file of the mavros repository. 15 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 16 | */ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | 25 | namespace mavros { 26 | namespace std_plugins { 27 | /** 28 | * @brief Setpoint acceleration/force plugin 29 | * 30 | * Send setpoint accelerations/forces to FCU controller. 31 | */ 32 | class SetpointAccelerationPlugin : public plugin::PluginBase, 33 | private plugin::SetPositionTargetLocalNEDMixin { 34 | public: 35 | SetpointAccelerationPlugin() : PluginBase(), 36 | sp_nh("~setpoint_accel"), 37 | send_force(false) 38 | { } 39 | 40 | void initialize(UAS &uas_) 41 | { 42 | PluginBase::initialize(uas_); 43 | 44 | sp_nh.param("send_force", send_force, false); 45 | 46 | accel_sub = sp_nh.subscribe("accel", 10, &SetpointAccelerationPlugin::accel_cb, this); 47 | } 48 | 49 | Subscriptions get_subscriptions() 50 | { 51 | return { /* Rx disabled */ }; 52 | } 53 | 54 | private: 55 | friend class SetPositionTargetLocalNEDMixin; 56 | ros::NodeHandle sp_nh; 57 | 58 | ros::Subscriber accel_sub; 59 | 60 | bool send_force; 61 | 62 | /* -*- mid-level helpers -*- */ 63 | 64 | /** 65 | * @brief Send acceleration/force to FCU acceleration controller. 66 | * 67 | * @warning Send only AFX AFY AFZ. ENU frame. 68 | */ 69 | void send_setpoint_acceleration(const ros::Time &stamp, Eigen::Vector3d &accel_enu) 70 | { 71 | using mavlink::common::MAV_FRAME; 72 | 73 | /* Documentation start from bit 1 instead 0. 74 | * Ignore position and velocity vectors, yaw and yaw rate 75 | */ 76 | uint16_t ignore_all_except_a_xyz = (3 << 10) | (7 << 3) | (7 << 0); 77 | 78 | if (send_force) 79 | ignore_all_except_a_xyz |= (1 << 9); 80 | 81 | auto accel = ftf::transform_frame_enu_ned(accel_enu); 82 | 83 | set_position_target_local_ned(stamp.toNSec() / 1000000, 84 | utils::enum_value(MAV_FRAME::LOCAL_NED), 85 | ignore_all_except_a_xyz, 86 | Eigen::Vector3d::Zero(), 87 | Eigen::Vector3d::Zero(), 88 | accel, 89 | 0.0, 0.0); 90 | } 91 | 92 | /* -*- callbacks -*- */ 93 | 94 | void accel_cb(const geometry_msgs::Vector3Stamped::ConstPtr &req) { 95 | Eigen::Vector3d accel_enu; 96 | 97 | tf::vectorMsgToEigen(req->vector, accel_enu); 98 | send_setpoint_acceleration(req->header.stamp, accel_enu); 99 | } 100 | }; 101 | } // namespace std_plugins 102 | } // namespace mavros 103 | 104 | #include 105 | PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::SetpointAccelerationPlugin, mavros::plugin::PluginBase) 106 | -------------------------------------------------------------------------------- /mavros/src/plugins/vfr_hud.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief VFR HUD plugin 3 | * @file vfr_hud.cpp 4 | * @author Vladimir Ermakov 5 | * 6 | * @addtogroup plugin 7 | * @{ 8 | */ 9 | /* 10 | * Copyright 2014,2016 Vladimir Ermakov. 11 | * 12 | * This file is part of the mavros package and subject to the license terms 13 | * in the top-level LICENSE file of the mavros repository. 14 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 15 | */ 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | namespace mavros { 23 | namespace std_plugins { 24 | /** 25 | * @brief VFR HUD plugin. 26 | */ 27 | class VfrHudPlugin : public plugin::PluginBase { 28 | public: 29 | VfrHudPlugin() : PluginBase(), 30 | nh("~") 31 | { } 32 | 33 | /** 34 | * Plugin initializer. Constructor should not do this. 35 | */ 36 | void initialize(UAS &uas_) 37 | { 38 | PluginBase::initialize(uas_); 39 | 40 | vfr_pub = nh.advertise("vfr_hud", 10); 41 | } 42 | 43 | Subscriptions get_subscriptions() 44 | { 45 | return { 46 | make_handler(&VfrHudPlugin::handle_vfr_hud), 47 | }; 48 | } 49 | 50 | private: 51 | ros::NodeHandle nh; 52 | 53 | ros::Publisher vfr_pub; 54 | 55 | void handle_vfr_hud(const mavlink::mavlink_message_t *msg, mavlink::common::msg::VFR_HUD &vfr_hud) 56 | { 57 | auto vmsg = boost::make_shared(); 58 | vmsg->header.stamp = ros::Time::now(); 59 | vmsg->airspeed = vfr_hud.airspeed; 60 | vmsg->groundspeed = vfr_hud.groundspeed; 61 | vmsg->heading = vfr_hud.heading; 62 | vmsg->throttle = vfr_hud.throttle / 100.0; // comes in 0..100 range 63 | vmsg->altitude = vfr_hud.alt; 64 | vmsg->climb = vfr_hud.climb; 65 | 66 | vfr_pub.publish(vmsg); 67 | } 68 | }; 69 | } // namespace std_plugins 70 | } // namespace mavros 71 | 72 | #include 73 | PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::VfrHudPlugin, mavros::plugin::PluginBase) 74 | -------------------------------------------------------------------------------- /mavros/src/plugins/wind_estimation.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief Wind estimation plugin 3 | * @file wind_estimation.cpp 4 | * @author Thomas Stastny 5 | * 6 | * @addtogroup plugin 7 | * @{ 8 | */ 9 | /* 10 | * Copyright 2018 Thomas Stastny 11 | * 12 | * This file is part of the mavros package and subject to the license terms 13 | * in the top-level LICENSE file of the mavros repository. 14 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 15 | */ 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | 24 | namespace mavros { 25 | namespace std_plugins { 26 | /** 27 | * @brief Wind estimation plugin. 28 | */ 29 | class WindEstimationPlugin : public plugin::PluginBase { 30 | public: 31 | WindEstimationPlugin() : PluginBase(), 32 | nh("~") 33 | { } 34 | 35 | /** 36 | * Plugin initializer. Constructor should not do this. 37 | */ 38 | void initialize(UAS &uas_) 39 | { 40 | PluginBase::initialize(uas_); 41 | 42 | wind_pub = nh.advertise("wind_estimation", 10); 43 | } 44 | 45 | Subscriptions get_subscriptions() 46 | { 47 | return { 48 | make_handler(&WindEstimationPlugin::handle_apm_wind), 49 | make_handler(&WindEstimationPlugin::handle_px4_wind), 50 | }; 51 | } 52 | 53 | private: 54 | ros::NodeHandle nh; 55 | 56 | ros::Publisher wind_pub; 57 | 58 | /** 59 | * Handle APM specific wind estimation message 60 | */ 61 | void handle_apm_wind(const mavlink::mavlink_message_t *msg, mavlink::ardupilotmega::msg::WIND &wind) 62 | { 63 | const double speed = wind.speed; 64 | const double course = -angles::from_degrees(wind.direction); // direction "from" -> direction "to" 65 | 66 | auto twist_cov = boost::make_shared(); 67 | twist_cov->header.stamp = ros::Time::now(); 68 | // TODO: check math's 69 | twist_cov->twist.twist.linear.x = speed * std::sin(course); // E 70 | twist_cov->twist.twist.linear.y = speed * std::cos(course); // N 71 | twist_cov->twist.twist.linear.z = -wind.speed_z;// D -> U 72 | 73 | // covariance matrix unknown in APM msg 74 | ftf::EigenMapCovariance6d cov_map(twist_cov->twist.covariance.data()); 75 | cov_map.setZero(); 76 | cov_map(0, 0) = -1.0; 77 | 78 | wind_pub.publish(twist_cov); 79 | } 80 | 81 | /** 82 | * Handle PX4 specific wind estimation message 83 | */ 84 | void handle_px4_wind(const mavlink::mavlink_message_t *msg, mavlink::common::msg::WIND_COV &wind) 85 | { 86 | auto twist_cov = boost::make_shared(); 87 | twist_cov->header.stamp = m_uas->synchronise_stamp(wind.time_usec); 88 | 89 | tf::vectorEigenToMsg(ftf::transform_frame_ned_enu(Eigen::Vector3d(wind.wind_x, wind.wind_y, wind.wind_z)), 90 | twist_cov->twist.twist.linear); 91 | 92 | // fill available covariance elements 93 | ftf::EigenMapCovariance6d cov_map(twist_cov->twist.covariance.data()); 94 | cov_map.setZero(); 95 | cov_map(0, 0) = wind.var_horiz; // NOTE: this is a summed covariance for both x and y horizontal wind components 96 | cov_map(2, 2) = wind.var_vert; 97 | 98 | wind_pub.publish(twist_cov); 99 | } 100 | }; 101 | } // namespace std_plugins 102 | } // namespace mavros 103 | 104 | #include 105 | PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::WindEstimationPlugin, mavros::plugin::PluginBase) 106 | -------------------------------------------------------------------------------- /mavros/test/test_sensor_orientation.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Test libmavros sensor orientation utilities 3 | * 4 | * We look up for some rotations, not fill list. 5 | */ 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | using namespace mavros; 15 | using mavros::utils::enum_value; 16 | using SO = mavlink::common::MAV_SENSOR_ORIENTATION; 17 | 18 | static const double epsilon = 1e-9; 19 | 20 | #define EXPECT_QUATERNION(exp, res) \ 21 | EXPECT_NEAR(exp.w(), res.w(), epsilon); \ 22 | EXPECT_NEAR(exp.x(), res.x(), epsilon); \ 23 | EXPECT_NEAR(exp.y(), res.y(), epsilon); \ 24 | EXPECT_NEAR(exp.z(), res.z(), epsilon) 25 | 26 | TEST(UTILS, sensor_orientation_matching__none) 27 | { 28 | auto expected = ftf::quaternion_from_rpy(0.0, 0.0, 0.0); 29 | auto out = utils::sensor_orientation_matching(SO::ROTATION_NONE); 30 | 31 | EXPECT_QUATERNION(expected, out); 32 | } 33 | 34 | TEST(UTILS, sensor_orientation_matching__roll_180) 35 | { 36 | auto expected = ftf::quaternion_from_rpy(M_PI, 0.0, 0.0); 37 | auto out = utils::sensor_orientation_matching(SO::ROTATION_ROLL_180); 38 | 39 | EXPECT_QUATERNION(expected, out); 40 | } 41 | 42 | TEST(UTILS, sensor_orientation_matching__roll_180_yaw_90) 43 | { 44 | auto expected = ftf::quaternion_from_rpy(M_PI, 0.0, M_PI/2); 45 | auto out = utils::sensor_orientation_matching(SO::ROTATION_ROLL_180_YAW_90); 46 | 47 | EXPECT_QUATERNION(expected, out); 48 | } 49 | 50 | TEST(UTILS, to_string__none) 51 | { 52 | EXPECT_EQ("NONE", utils::to_string(SO::ROTATION_NONE)); 53 | } 54 | 55 | TEST(UTILS, to_string__roll_180) 56 | { 57 | EXPECT_EQ("ROLL_180", utils::to_string(SO::ROTATION_ROLL_180)); 58 | } 59 | 60 | TEST(UTILS, to_string__roll_180_yaw_90) 61 | { 62 | EXPECT_EQ("ROLL_180_YAW_90", utils::to_string(SO::ROTATION_ROLL_180_YAW_90)); 63 | } 64 | 65 | TEST(UTILS, sensor_orientation_from_str__none) 66 | { 67 | EXPECT_EQ(enum_value(SO::ROTATION_NONE), utils::sensor_orientation_from_str("NONE")); 68 | } 69 | 70 | TEST(UTILS, sensor_orientation_from_str__unknown) 71 | { 72 | EXPECT_LT(utils::sensor_orientation_from_str("completely wrong identificator"), 0); 73 | } 74 | 75 | TEST(UTILS, sensor_orientation_from_str__number) 76 | { 77 | EXPECT_EQ(enum_value(SO::ROTATION_ROLL_270), utils::sensor_orientation_from_str("20")); 78 | } 79 | 80 | TEST(UTILS, sensor_orientation_from_str__wrong_number) 81 | { 82 | // 123 >> 38 (max) 83 | EXPECT_LT(utils::sensor_orientation_from_str("123"), 0); 84 | } 85 | 86 | TEST(UTILS, sensor_orientation_from_str__roll_180) 87 | { 88 | EXPECT_EQ(enum_value(SO::ROTATION_ROLL_180), utils::sensor_orientation_from_str("ROLL_180")); 89 | } 90 | 91 | TEST(UTILS, sensor_orientation_from_str__roll_180_yaw_90) 92 | { 93 | EXPECT_EQ(enum_value(SO::ROTATION_ROLL_180_YAW_90), utils::sensor_orientation_from_str("ROLL_180_YAW_90")); 94 | } 95 | 96 | TEST(UTILS, sensor_orientation_from_str__last_element_roll_90_yaw_270) 97 | { 98 | EXPECT_EQ(enum_value(SO::ROTATION_ROLL_90_YAW_270), utils::sensor_orientation_from_str("ROLL_90_YAW_270")); 99 | } 100 | 101 | int main(int argc, char **argv) 102 | { 103 | ::testing::InitGoogleTest(&argc, argv); 104 | return RUN_ALL_TESTS(); 105 | } 106 | -------------------------------------------------------------------------------- /mavros/tools/cogall.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | set -x 4 | 5 | fgrep '[[[cog' --exclude 'cogall.sh' --exclude '*.md' --exclude '*.swp' -lr | cog.py -cr @- 6 | -------------------------------------------------------------------------------- /mavros/tools/uncrustify-cpp.cfg: -------------------------------------------------------------------------------- 1 | tok_split_gte=false 2 | utf8_byte=false 3 | utf8_force=true 4 | indent_cmt_with_tabs=true 5 | indent_align_string=false 6 | indent_braces=false 7 | indent_braces_no_func=false 8 | indent_braces_no_class=false 9 | indent_braces_no_struct=false 10 | indent_brace_parent=false 11 | indent_namespace=false 12 | indent_extern=false 13 | indent_class=true 14 | indent_class_colon=false 15 | indent_else_if=false 16 | indent_var_def_cont=false 17 | indent_func_call_param=true 18 | indent_func_def_param=false 19 | indent_func_proto_param=false 20 | indent_func_class_param=false 21 | indent_func_ctor_var_param=false 22 | indent_template_param=false 23 | indent_func_param_double=false 24 | indent_relative_single_line_comments=false 25 | indent_col1_comment=false 26 | indent_access_spec_body=false 27 | indent_paren_nl=false 28 | indent_comma_paren=false 29 | indent_bool_paren=false 30 | indent_first_bool_expr=false 31 | indent_square_nl=false 32 | indent_preserve_sql=false 33 | indent_align_assign=true 34 | sp_balance_nested_parens=false 35 | align_keep_tabs=true 36 | align_with_tabs=true 37 | align_on_tabstop=false 38 | align_number_left=false 39 | align_func_params=false 40 | align_same_func_call_params=false 41 | align_var_def_colon=false 42 | align_var_def_attribute=false 43 | align_var_def_inline=false 44 | align_right_cmt_mix=false 45 | align_on_operator=false 46 | align_mix_var_proto=false 47 | align_single_line_func=false 48 | align_single_line_brace=false 49 | align_nl_cont=false 50 | align_left_shift=true 51 | align_oc_decl_colon=false 52 | nl_collapse_empty_body=true 53 | nl_assign_leave_one_liners=false 54 | nl_class_leave_one_liners=false 55 | nl_enum_leave_one_liners=false 56 | nl_getset_leave_one_liners=false 57 | nl_func_leave_one_liners=false 58 | nl_if_leave_one_liners=false 59 | nl_multi_line_cond=false 60 | nl_multi_line_define=false 61 | nl_before_case=false 62 | nl_after_case=false 63 | nl_after_return=false 64 | nl_after_semicolon=false 65 | nl_after_brace_open=false 66 | nl_after_brace_open_cmt=false 67 | nl_after_vbrace_open=false 68 | nl_after_vbrace_open_empty=false 69 | nl_after_brace_close=false 70 | nl_after_vbrace_close=false 71 | nl_define_macro=false 72 | nl_squeeze_ifdef=false 73 | nl_ds_struct_enum_cmt=false 74 | nl_ds_struct_enum_close_brace=false 75 | nl_create_if_one_liner=false 76 | nl_create_for_one_liner=false 77 | nl_create_while_one_liner=false 78 | ls_for_split_full=false 79 | ls_func_split_full=false 80 | nl_after_multiline_comment=false 81 | eat_blanks_after_open_brace=true 82 | eat_blanks_before_close_brace=true 83 | mod_full_brace_if_chain=false 84 | mod_pawn_semicolon=false 85 | mod_full_paren_if_bool=false 86 | mod_remove_extra_semicolon=false 87 | mod_sort_import=false 88 | mod_sort_using=false 89 | mod_sort_include=false 90 | mod_move_case_break=false 91 | mod_remove_empty_return=false 92 | cmt_indent_multi=true 93 | cmt_c_group=false 94 | cmt_c_nl_start=false 95 | cmt_c_nl_end=false 96 | cmt_cpp_group=false 97 | cmt_cpp_nl_start=false 98 | cmt_cpp_nl_end=false 99 | cmt_cpp_to_c=false 100 | cmt_star_cont=false 101 | cmt_multi_check_last=true 102 | cmt_insert_before_preproc=false 103 | pp_indent_at_level=false 104 | pp_region_indent_code=false 105 | pp_if_indent_code=false 106 | pp_define_at_level=false 107 | input_tab_size=8 108 | output_tab_size=8 109 | indent_columns=8 110 | indent_continue=8 111 | nl_max=3 112 | utf8_bom=remove 113 | indent_with_tabs=2 114 | sp_arith=add 115 | sp_assign=add 116 | sp_bool=add 117 | sp_compare=add 118 | sp_after_type=add 119 | sp_before_sparen=add 120 | sp_else_brace=add 121 | sp_brace_else=add 122 | sp_catch_brace=add 123 | sp_try_brace=add 124 | sp_cond_colon=add 125 | sp_cond_question=ignore 126 | -------------------------------------------------------------------------------- /mavros_cog.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Module with common cog generators used several times. 3 | 4 | Install: 5 | 6 | ln -s $PWD/mavros_cog.py $HOME/.local/lib/python2.7/site-packages/ 7 | ''' 8 | 9 | import cog 10 | -------------------------------------------------------------------------------- /mavros_extras/README.md: -------------------------------------------------------------------------------- 1 | mavros extras 2 | ============= 3 | 4 | Some extra plugins and nodes for [mavros][mr]. 5 | 6 | 7 | ADSB 8 | ---- 9 | 10 | Publish/subscribe to the location and information of an ADS-B vehicle. 11 | 12 | 13 | cam\_imu\_sync 14 | -------------- 15 | 16 | Publish camera trigger data for synchronisation of IMU and camera frames. 17 | 18 | 19 | debug\_value 20 | ------------ 21 | 22 | Subs/Pubs debug msgs from and to the FCU. 23 | 24 | 25 | distance\_sensor 26 | ---------------- 27 | 28 | Publish DISTANCE\_SENSOR message data from FCU or connected sensors in companion computer. 29 | 30 | 31 | fake\_gps 32 | --------- 33 | 34 | Sends fake GPS from local position estimation source data (motion capture, vision) to FCU. 35 | 36 | 37 | gps\_rtk 38 | -------- 39 | 40 | Sends the RTCM messages to the FCU for the RTK Fix. 41 | 42 | 43 | log\_transfer 44 | ------------- 45 | 46 | Expose firmware functionality, that is related to log transfer 47 | 48 | 49 | mocap\_pose\_estimate 50 | --------------------- 51 | 52 | Send motion capture pose estimate to FCU. Currently, not used by the FCU. 53 | Data can be send via `vision_position` plugin. 54 | 55 | 56 | obstacle\_distance 57 | ------------------ 58 | 59 | Send obstacle distance report to the FCU. 60 | 61 | 62 | odom 63 | ---- 64 | 65 | Send odometry to FCU from another estimator. 66 | 67 | 68 | px4flow 69 | ------- 70 | 71 | Publish `OPTICAL_FLOW_RAD` data from FCU or PX4Flow module. 72 | 73 | 74 | rangefinder 75 | ----------- 76 | 77 | Publish RANGEFINDER message data from FCU sensors in companion computer. 78 | 79 | 80 | trajectory 81 | ---------- 82 | 83 | Receive planned path from the FCU and send back corrected path (collision free, smoothed) to the FCU. 84 | 85 | 86 | wheel\_odometry 87 | --------------- 88 | 89 | Compute and publish wheel odometry coming from FCU sensors. 90 | 91 | 92 | vibration 93 | --------- 94 | 95 | Publish VIBRATION message data from FCU. 96 | 97 | 98 | vision\_pose\_estimate 99 | ---------------------- 100 | 101 | Send vision pose estimate to FCU. 102 | 103 | 104 | vision\_speed\_estimate 105 | ----------------------- 106 | 107 | Send vision speed estimate to FCU. 108 | 109 | 110 | companion\_process\_status 111 | -------------------------- 112 | 113 | Send companion process status report to the FCU. 114 | 115 | 116 | servo\_state\_publisher 117 | ----------------------- 118 | 119 | Convert `mavros_msgs/RCOut` to `sensor_msgs/JointState` messages. 120 | It is required to bind URDF model and real servos. 121 | 122 | 123 | px-ros-pkg replacement 124 | ---------------------- 125 | 126 | Use `roslaunch mavros_extras px4flow.launch` for that. 127 | 128 | 129 | [mr]: https://github.com/mavlink/mavros 130 | -------------------------------------------------------------------------------- /mavros_extras/launch/f710_joy.yaml: -------------------------------------------------------------------------------- 1 | # Joystic configuration for Logitech F710 gamepad 2 | # TODO: check scale, button map 3 | axes_map: 4 | roll: 3 5 | pitch: 4 6 | yaw: 0 7 | throttle: 1 8 | axes_scale: 9 | roll: 1.0 10 | pitch: 1.0 11 | yaw: 1.0 12 | throttle: 1.0 13 | button_map: 14 | takeoff: 0 15 | land: 1 16 | enable: 2 17 | rc_modes: 18 | offboard: 19 | rc_channel: 4 20 | rc_value: 1000 21 | joy_flags: [ [2,1] ] 22 | auto: 23 | rc_channel: 4 24 | rc_value: 1500 25 | joy_flags: [ [3,1] ] 26 | manual: 27 | rc_channel: 4 28 | rc_value: 2000 29 | joy_flags: [ [0,1] ] 30 | -------------------------------------------------------------------------------- /mavros_extras/launch/px4_image.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /mavros_extras/launch/px4flow.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /mavros_extras/launch/px4flow_config.yaml: -------------------------------------------------------------------------------- 1 | # Common configuration for PX4Flow module 2 | # 3 | # node: 4 | startup_px4_usb_quirk: false 5 | 6 | # --- system plugins --- 7 | 8 | # sys_status & sys_time connection options 9 | conn: 10 | heartbeat_rate: 1.0 # send hertbeat rate in Hertz 11 | timeout: 10.0 # hertbeat timeout in seconds 12 | timesync_rate: 0.0 # TIMESYNC rate in Hertz (feature disabled if 0.0) 13 | system_time_rate: 1.0 # send system time to FCU rate in Hertz (disabled if 0.0) 14 | 15 | # sys_status 16 | sys: 17 | min_voltage: 4.5 # diagnostics min voltage 18 | disable_diag: true # disable all sys_status diagnostics, except heartbeat 19 | 20 | # --- mavros extras plugins (same order) --- 21 | 22 | # image_pub 23 | image: 24 | frame_id: "px4flow" 25 | 26 | px4flow: 27 | frame_id: "px4flow" 28 | ranger_fov: 0.118682 # 6.8 degree at 5 meters, 31 degrees at 1 meter 29 | ranger_min_range: 0.3 # meters 30 | ranger_max_range: 5.0 # meters 31 | 32 | # vim:set ts=2 sw=2 et: 33 | -------------------------------------------------------------------------------- /mavros_extras/launch/px4flow_pluginlists.yaml: -------------------------------------------------------------------------------- 1 | plugin_blacklist: 2 | - '*' 3 | 4 | plugin_whitelist: 5 | - 'sys_*' 6 | - command 7 | - param 8 | - image_pub 9 | - px4flow 10 | -------------------------------------------------------------------------------- /mavros_extras/launch/servo_state_publisher.yaml: -------------------------------------------------------------------------------- 1 | # Example config for servo_state_publisher 2 | # vim:set ts=2 sw=2 et: 3 | # 4 | aileron: &default 5 | rc_channel: 1 6 | rc_min: 1000 # for APM this values can be copied from RCx_MIN/MAX/TRIM 7 | rc_max: 2000 8 | rc_trim: 1500 9 | 10 | elevator: 11 | <<: *default 12 | rc_channel: 2 13 | 14 | # error if URDF don't define that 15 | throttle: 16 | rc_channel: 3 17 | 18 | rudder: 19 | rc_channel: 4 20 | rc_min: 1100 21 | rc_max: 1950 # rc_trim should calculate to 1525 22 | rc_rev: true # reverse 23 | -------------------------------------------------------------------------------- /mavros_extras/launch/teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /mavros_extras/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mavros_extras 4 | 0.33.3 5 | 6 | Extra nodes and plugins for MAVROS. 7 | 8 | 9 | Vladimir Ermakov 10 | Vladimir Ermakov 11 | GPLv3 12 | LGPLv3 13 | BSD 14 | 15 | http://wiki.ros.org/mavros_extras 16 | https://github.com/mavlink/mavros.git 17 | https://github.com/mavlink/mavros/issues 18 | 19 | catkin 20 | 21 | cmake_modules 22 | mavros 23 | roscpp 24 | geometry_msgs 25 | mavros_msgs 26 | sensor_msgs 27 | std_msgs 28 | visualization_msgs 29 | urdf 30 | tf 31 | tf2_eigen 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /mavros_extras/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | exclude_patterns: '*/.git/*' 3 | -------------------------------------------------------------------------------- /mavros_extras/src/plugins/cam_imu_sync.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief Camera IMU synchronisation plugin 3 | * @file cam_imu_sync.cpp 4 | * @author Mohammed Kabir < mhkabir98@gmail.com > 5 | * 6 | * @addtogroup plugin 7 | * @{ 8 | */ 9 | /* 10 | * Copyright 2015 Mohammed Kabir. 11 | * 12 | * This file is part of the mavros package and subject to the license terms 13 | * in the top-level LICENSE file of the mavros repository. 14 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 15 | */ 16 | 17 | #include 18 | 19 | #include 20 | 21 | namespace mavros { 22 | namespace extra_plugins{ 23 | /** 24 | * @brief Camera IMU synchronisation plugin 25 | * 26 | * This plugin publishes a timestamp for when a external camera system was 27 | * triggered by the FCU. Sequence ID from the message and the image sequence from 28 | * camera can be corellated to get the exact shutter trigger time. 29 | */ 30 | class CamIMUSyncPlugin : public plugin::PluginBase { 31 | public: 32 | CamIMUSyncPlugin() : PluginBase(), 33 | cam_imu_sync_nh("~cam_imu_sync") 34 | { } 35 | 36 | void initialize(UAS &uas_) 37 | { 38 | PluginBase::initialize(uas_); 39 | 40 | cam_imu_pub = cam_imu_sync_nh.advertise("cam_imu_stamp", 10); 41 | } 42 | 43 | Subscriptions get_subscriptions() 44 | { 45 | return { 46 | make_handler(&CamIMUSyncPlugin::handle_cam_trig) 47 | }; 48 | } 49 | 50 | private: 51 | ros::NodeHandle cam_imu_sync_nh; 52 | 53 | ros::Publisher cam_imu_pub; 54 | 55 | void handle_cam_trig(const mavlink::mavlink_message_t *msg, mavlink::common::msg::CAMERA_TRIGGER &ctrig) 56 | { 57 | auto sync_msg = boost::make_shared(); 58 | 59 | sync_msg->frame_stamp = m_uas->synchronise_stamp(ctrig.time_usec); 60 | sync_msg->frame_seq_id = ctrig.seq; 61 | 62 | cam_imu_pub.publish(sync_msg); 63 | } 64 | }; 65 | } // namespace extra_plugins 66 | } // namespace mavros 67 | 68 | #include 69 | PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::CamIMUSyncPlugin, mavros::plugin::PluginBase) 70 | -------------------------------------------------------------------------------- /mavros_extras/src/plugins/companion_process_status.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief Companion Status plugin 3 | * @file companion_status.cpp 4 | * @author Tanja Baumann 5 | * 6 | * @addtogroup plugin 7 | * @{ 8 | */ 9 | /* 10 | * Copyright 2018 Tanja Baumann. 11 | * 12 | * This file is part of the mavros package and subject to the license terms 13 | * in the top-level LICENSE file of the mavros repository. 14 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 15 | */ 16 | 17 | #include 18 | 19 | #include 20 | 21 | namespace mavros { 22 | namespace extra_plugins { 23 | 24 | //! Mavlink enumerations 25 | using mavlink::common::MAV_TYPE; 26 | using mavlink::common::MAV_STATE; 27 | using mavlink::common::MAV_COMPONENT; 28 | using utils::enum_value; 29 | 30 | /** 31 | * @brief Obstacle companion process status plugin 32 | * 33 | * Publishes the status of components running on the companion computer 34 | * @see status_cb() 35 | */ 36 | class CompanionProcessStatusPlugin : public plugin::PluginBase { 37 | public: 38 | CompanionProcessStatusPlugin() : PluginBase(), 39 | status_nh("~companion_process") 40 | { } 41 | 42 | void initialize(UAS &uas_) 43 | { 44 | PluginBase::initialize(uas_); 45 | 46 | status_sub = status_nh.subscribe("status", 10, &CompanionProcessStatusPlugin::status_cb, this); 47 | } 48 | 49 | Subscriptions get_subscriptions() 50 | { 51 | return { /* Rx disabled */ }; 52 | } 53 | 54 | private: 55 | ros::NodeHandle status_nh; 56 | ros::Subscriber status_sub; 57 | 58 | /** 59 | * @brief Send companion process status to FCU over a heartbeat message 60 | * 61 | * Message specification: https://mavlink.io/en/messages/common.html#HEARTBEAT 62 | * @param req received CompanionProcessStatus msg 63 | */ 64 | void status_cb(const mavros_msgs::CompanionProcessStatus::ConstPtr &req) 65 | { 66 | mavlink::common::msg::HEARTBEAT heartbeat {}; 67 | 68 | heartbeat.type = enum_value(mavlink::common::MAV_TYPE::ONBOARD_CONTROLLER); 69 | heartbeat.autopilot = enum_value(mavlink::common::MAV_AUTOPILOT::PX4); 70 | heartbeat.base_mode = enum_value(mavlink::common::MAV_MODE_FLAG::CUSTOM_MODE_ENABLED); 71 | heartbeat.system_status = req->state; //enum="MAV_STATE" System status flag 72 | 73 | ROS_DEBUG_STREAM_NAMED("companion_process_status", "companion process component id: " << 74 | utils::to_string_enum(req->component) << " companion process status: " << 75 | utils::to_string_enum(heartbeat.system_status) << std::endl << heartbeat.to_yaml()); 76 | 77 | UAS_FCU(m_uas)->send_message_ignore_drop(heartbeat, req->component); 78 | } 79 | }; 80 | } // namespace extra_plugins 81 | } // namespace mavros 82 | 83 | #include 84 | PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::CompanionProcessStatusPlugin, mavros::plugin::PluginBase) 85 | -------------------------------------------------------------------------------- /mavros_extras/src/plugins/gps_rtk.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief GPS RTK plugin 3 | * @file gps_rtk.cpp 4 | * @author Alexis Paques 5 | * 6 | * @addtogroup plugin 7 | * @{ 8 | */ 9 | /* 10 | * Copyright 2018 Alexis Paques. 11 | * 12 | * This file is part of the mavros package and subject to the license terms 13 | * in the top-level LICENSE file of the mavros repository. 14 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | namespace mavros { 22 | namespace extra_plugins { 23 | /** 24 | * @brief GPS RTK plugin 25 | * 26 | * Publish the RTCM messages from ROS to the FCU 27 | */ 28 | class GpsRtkPlugin : public plugin::PluginBase { 29 | public: 30 | GpsRtkPlugin() : PluginBase(), 31 | gps_rtk_nh("~gps_rtk") 32 | { } 33 | 34 | void initialize(UAS &uas_) 35 | { 36 | PluginBase::initialize(uas_); 37 | gps_rtk_sub = gps_rtk_nh.subscribe("send_rtcm", 10, &GpsRtkPlugin::rtcm_cb, this); 38 | } 39 | 40 | Subscriptions get_subscriptions() 41 | { 42 | return {}; 43 | } 44 | 45 | private: 46 | ros::NodeHandle gps_rtk_nh; 47 | ros::Subscriber gps_rtk_sub; 48 | 49 | /* -*- callbacks -*- */ 50 | /** 51 | * @brief Handle mavros_msgs::RTCM message 52 | * It converts the message to the MAVLink GPS_RTCM_DATA message for GPS injection. 53 | * Message specification: https://mavlink.io/en/messages/common.html#GPS_RTCM_DATA 54 | * @param msg Received ROS msg 55 | */ 56 | void rtcm_cb(const mavros_msgs::RTCM::ConstPtr &msg) 57 | { 58 | mavlink::common::msg::GPS_RTCM_DATA rtcm_data; 59 | const size_t max_frag_len = rtcm_data.data.size(); 60 | 61 | uint8_t seq_u5 = uint8_t(msg->header.seq & 0x1F) << 3; 62 | 63 | if (msg->data.size() > 4 * max_frag_len) { 64 | ROS_FATAL("gps_rtk: RTCM message received is bigger than the maximal possible size."); 65 | return; 66 | } 67 | 68 | auto data_it = msg->data.begin(); 69 | auto end_it = msg->data.end(); 70 | 71 | if (msg->data.size() <= max_frag_len) { 72 | rtcm_data.len = msg->data.size(); 73 | rtcm_data.flags = seq_u5; 74 | std::copy(data_it, end_it, rtcm_data.data.begin()); 75 | std::fill(rtcm_data.data.begin() + rtcm_data.len, rtcm_data.data.end(), 0); 76 | UAS_FCU(m_uas)->send_message(rtcm_data); 77 | } else { 78 | for (uint8_t fragment_id = 0; fragment_id < 4 && data_it < end_it; fragment_id++) { 79 | uint8_t len = std::min((size_t) std::distance(data_it, end_it), max_frag_len); 80 | rtcm_data.flags = 1; // LSB set indicates message is fragmented 81 | rtcm_data.flags |= fragment_id << 1; // Next 2 bits are fragment id 82 | rtcm_data.flags |= seq_u5; // Next 5 bits are sequence id 83 | rtcm_data.len = len; 84 | 85 | std::copy(data_it, data_it + len, rtcm_data.data.begin()); 86 | std::fill(rtcm_data.data.begin() + len, rtcm_data.data.end(), 0); 87 | UAS_FCU(m_uas)->send_message(rtcm_data); 88 | std::advance(data_it, len); 89 | } 90 | } 91 | } 92 | }; 93 | } // namespace extra_plugins 94 | } // namespace mavros 95 | 96 | #include 97 | PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::GpsRtkPlugin, mavros::plugin::PluginBase) 98 | -------------------------------------------------------------------------------- /mavros_extras/src/plugins/log_transfer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace mavros { 9 | namespace extra_plugins { 10 | class LogTransferPlugin : public plugin::PluginBase { 11 | public: 12 | LogTransferPlugin() : 13 | nh("~log_transfer") {} 14 | 15 | void initialize(UAS& uas) override 16 | { 17 | PluginBase::initialize(uas); 18 | 19 | log_entry_pub = nh.advertise("raw/log_entry", 1000); 20 | log_data_pub = nh.advertise("raw/log_data", 1000); 21 | 22 | log_request_list_srv = nh.advertiseService("raw/log_request_list", 23 | &LogTransferPlugin::log_request_list_cb, this); 24 | log_request_data_srv = nh.advertiseService("raw/log_request_data", 25 | &LogTransferPlugin::log_request_data_cb, this); 26 | log_request_end_srv = nh.advertiseService("raw/log_request_end", 27 | &LogTransferPlugin::log_request_end_cb, this); 28 | } 29 | 30 | Subscriptions get_subscriptions() override 31 | { 32 | return { 33 | make_handler(&LogTransferPlugin::handle_log_entry), 34 | make_handler(&LogTransferPlugin::handle_log_data), 35 | }; 36 | } 37 | 38 | private: 39 | ros::NodeHandle nh; 40 | ros::Publisher log_entry_pub, log_data_pub; 41 | ros::ServiceServer log_request_list_srv, log_request_data_srv, log_request_end_srv; 42 | 43 | void handle_log_entry(const mavlink::mavlink_message_t*, mavlink::common::msg::LOG_ENTRY& le) 44 | { 45 | auto msg = boost::make_shared(); 46 | msg->header.stamp = ros::Time::now(); 47 | msg->id = le.id; 48 | msg->num_logs = le.num_logs; 49 | msg->last_log_num = le.last_log_num; 50 | msg->time_utc = ros::Time(le.time_utc); 51 | msg->size = le.size; 52 | log_entry_pub.publish(msg); 53 | } 54 | 55 | void handle_log_data(const mavlink::mavlink_message_t*, mavlink::common::msg::LOG_DATA& ld) 56 | { 57 | auto msg = boost::make_shared(); 58 | msg->header.stamp = ros::Time::now(); 59 | msg->id = ld.id; 60 | msg->offset = ld.ofs; 61 | 62 | auto count = ld.count; 63 | if (count > ld.data.max_size()) { 64 | count = ld.data.max_size(); 65 | } 66 | msg->data.insert(msg->data.cbegin(), ld.data.cbegin(), ld.data.cbegin() + count); 67 | log_data_pub.publish(msg); 68 | } 69 | 70 | bool log_request_list_cb(mavros_msgs::LogRequestList::Request &req, 71 | mavros_msgs::LogRequestList::Response &res) 72 | { 73 | mavlink::common::msg::LOG_REQUEST_LIST msg; 74 | m_uas->msg_set_target(msg); 75 | msg.start = req.start; 76 | msg.end = req.end; 77 | 78 | res.success = true; 79 | try { 80 | UAS_FCU(m_uas)->send_message(msg); 81 | } catch (std::length_error&) { 82 | res.success = false; 83 | } 84 | return true; 85 | } 86 | 87 | bool log_request_data_cb(mavros_msgs::LogRequestData::Request &req, 88 | mavros_msgs::LogRequestData::Response &res) 89 | { 90 | mavlink::common::msg::LOG_REQUEST_DATA msg; 91 | m_uas->msg_set_target(msg); 92 | msg.id = req.id; 93 | msg.ofs = req.offset; 94 | msg.count = req.count; 95 | 96 | res.success = true; 97 | try { 98 | UAS_FCU(m_uas)->send_message(msg); 99 | } catch (std::length_error&) { 100 | res.success = false; 101 | } 102 | return true; 103 | } 104 | 105 | bool log_request_end_cb(mavros_msgs::LogRequestEnd::Request &, 106 | mavros_msgs::LogRequestEnd::Response &res) 107 | { 108 | mavlink::common::msg::LOG_REQUEST_END msg; 109 | m_uas->msg_set_target(msg); 110 | res.success = true; 111 | try { 112 | UAS_FCU(m_uas)->send_message(msg); 113 | } catch (std::length_error&) { 114 | res.success = false; 115 | } 116 | return true; 117 | } 118 | }; 119 | } // namespace extra_plugins 120 | } // namespace mavros 121 | 122 | #include 123 | PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::LogTransferPlugin, mavros::plugin::PluginBase) 124 | -------------------------------------------------------------------------------- /mavros_extras/src/plugins/mocap_pose_estimate.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief MocapPoseEstimate plugin 3 | * @file mocap_pose_estimate.cpp 4 | * @author Tony Baltovski 5 | * @author Vladimir Ermakov 6 | * 7 | * @addtogroup plugin 8 | * @{ 9 | */ 10 | /* 11 | * Copyright 2014,2015,2016 Vladimir Ermakov, Tony Baltovski. 12 | * 13 | * This file is part of the mavros package and subject to the license terms 14 | * in the top-level LICENSE file of the mavros repository. 15 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 16 | */ 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | 25 | namespace mavros { 26 | namespace extra_plugins{ 27 | /** 28 | * @brief MocapPoseEstimate plugin 29 | * 30 | * Sends motion capture data to FCU. 31 | */ 32 | class MocapPoseEstimatePlugin : public plugin::PluginBase 33 | { 34 | public: 35 | MocapPoseEstimatePlugin() : PluginBase(), 36 | mp_nh("~mocap") 37 | { } 38 | 39 | void initialize(UAS &uas_) 40 | { 41 | PluginBase::initialize(uas_); 42 | 43 | bool use_tf; 44 | bool use_pose; 45 | 46 | /** @note For VICON ROS package, subscribe to TransformStamped topic */ 47 | mp_nh.param("use_tf", use_tf, false); 48 | 49 | /** @note For Optitrack ROS package, subscribe to PoseStamped topic */ 50 | mp_nh.param("use_pose", use_pose, true); 51 | 52 | if (use_tf && !use_pose) { 53 | mocap_tf_sub = mp_nh.subscribe("tf", 1, &MocapPoseEstimatePlugin::mocap_tf_cb, this); 54 | } 55 | else if (use_pose && !use_tf) { 56 | mocap_pose_sub = mp_nh.subscribe("pose", 1, &MocapPoseEstimatePlugin::mocap_pose_cb, this); 57 | } 58 | else { 59 | ROS_ERROR_NAMED("mocap", "Use one motion capture source."); 60 | } 61 | } 62 | 63 | Subscriptions get_subscriptions() 64 | { 65 | return { /* Rx disabled */ }; 66 | } 67 | 68 | private: 69 | ros::NodeHandle mp_nh; 70 | 71 | ros::Subscriber mocap_pose_sub; 72 | ros::Subscriber mocap_tf_sub; 73 | 74 | /* -*- low-level send -*- */ 75 | void mocap_pose_send 76 | (uint64_t usec, 77 | Eigen::Quaterniond &q, 78 | Eigen::Vector3d &v) 79 | { 80 | mavlink::common::msg::ATT_POS_MOCAP pos; 81 | 82 | pos.time_usec = usec; 83 | ftf::quaternion_to_mavlink(q, pos.q); 84 | pos.x = v.x(); 85 | pos.y = v.y(); 86 | pos.z = v.z(); 87 | 88 | UAS_FCU(m_uas)->send_message_ignore_drop(pos); 89 | } 90 | 91 | /* -*- mid-level helpers -*- */ 92 | void mocap_pose_cb(const geometry_msgs::PoseStamped::ConstPtr &pose) 93 | { 94 | Eigen::Quaterniond q_enu; 95 | 96 | tf::quaternionMsgToEigen(pose->pose.orientation, q_enu); 97 | auto q = ftf::transform_orientation_enu_ned( 98 | ftf::transform_orientation_baselink_aircraft(q_enu)); 99 | 100 | auto position = ftf::transform_frame_enu_ned( 101 | Eigen::Vector3d( 102 | pose->pose.position.x, 103 | pose->pose.position.y, 104 | pose->pose.position.z)); 105 | 106 | mocap_pose_send(pose->header.stamp.toNSec() / 1000, 107 | q, 108 | position); 109 | } 110 | 111 | /* -*- callbacks -*- */ 112 | void mocap_tf_cb(const geometry_msgs::TransformStamped::ConstPtr &trans) 113 | { 114 | Eigen::Quaterniond q_enu; 115 | 116 | tf::quaternionMsgToEigen(trans->transform.rotation, q_enu); 117 | auto q = ftf::transform_orientation_enu_ned( 118 | ftf::transform_orientation_baselink_aircraft(q_enu)); 119 | 120 | auto position = ftf::transform_frame_enu_ned( 121 | Eigen::Vector3d( 122 | trans->transform.translation.x, 123 | trans->transform.translation.y, 124 | trans->transform.translation.z)); 125 | 126 | mocap_pose_send(trans->header.stamp.toNSec() / 1000, 127 | q, 128 | position); 129 | } 130 | }; 131 | } // namespace extra_plugins 132 | } // namespace mavros 133 | 134 | #include 135 | PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::MocapPoseEstimatePlugin, mavros::plugin::PluginBase) 136 | -------------------------------------------------------------------------------- /mavros_extras/src/plugins/rangefinder.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief Rangefinder plugin 3 | * @file rangefinder.cpp 4 | * @author Pierre Kancir 5 | * 6 | * @addtogroup plugin 7 | * @{ 8 | */ 9 | /* 10 | * Copyright 2016 Ardupilot. 11 | * 12 | * This file is part of the mavros package and subject to the license terms 13 | * in the top-level LICENSE file of the mavros repository. 14 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 15 | */ 16 | 17 | #include 18 | #include 19 | 20 | namespace mavros { 21 | namespace extra_plugins { 22 | /** 23 | * @brief Ardupilot Rangefinder plugin. 24 | * 25 | * This plugin allows publishing rangefinder sensor data from Ardupilot FCU to ROS. 26 | * 27 | */ 28 | class RangefinderPlugin : public plugin::PluginBase { 29 | public: 30 | RangefinderPlugin() : PluginBase(), 31 | rangefinder_nh("~rangefinder") 32 | { } 33 | 34 | void initialize(UAS &uas_) 35 | { 36 | PluginBase::initialize(uas_); 37 | 38 | rangefinder_pub = rangefinder_nh.advertise("rangefinder", 10); 39 | } 40 | 41 | Subscriptions get_subscriptions() 42 | { 43 | return { 44 | make_handler(&RangefinderPlugin::handle_rangefinder) 45 | }; 46 | } 47 | 48 | private: 49 | ros::NodeHandle rangefinder_nh; 50 | 51 | ros::Publisher rangefinder_pub; 52 | 53 | void handle_rangefinder(const mavlink::mavlink_message_t *msg, mavlink::ardupilotmega::msg::RANGEFINDER &rangefinder) { 54 | auto rangefinder_msg = boost::make_shared(); 55 | rangefinder_msg->header.stamp = ros::Time::now(); 56 | rangefinder_msg->header.frame_id = "/rangefinder"; 57 | rangefinder_msg->radiation_type = sensor_msgs::Range::INFRARED; 58 | rangefinder_msg->field_of_view = 0; 59 | rangefinder_msg->min_range = 0; 60 | rangefinder_msg->max_range = 1000; 61 | rangefinder_msg->range = rangefinder.distance; 62 | 63 | rangefinder_pub.publish(rangefinder_msg); 64 | } 65 | }; 66 | } // namespace extra_plugins 67 | } // namespace mavros 68 | 69 | #include 70 | PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::RangefinderPlugin, mavros::plugin::PluginBase) 71 | -------------------------------------------------------------------------------- /mavros_extras/src/plugins/vibration.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief Vibration plugin 3 | * @file vibration.cpp 4 | * @author Nuno Marques 5 | * 6 | * @addtogroup plugin 7 | * @{ 8 | */ 9 | /* 10 | * Copyright 2015 Nuno Marques. 11 | * 12 | * This file is part of the mavros package and subject to the license terms 13 | * in the top-level LICENSE file of the mavros repository. 14 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 15 | */ 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | namespace mavros { 23 | namespace extra_plugins { 24 | /** 25 | * @brief Vibration plugin 26 | * 27 | * This plugin is intended to publish MAV vibration levels and accelerometer clipping from FCU. 28 | */ 29 | class VibrationPlugin : public plugin::PluginBase { 30 | public: 31 | VibrationPlugin() : PluginBase(), 32 | vibe_nh("~vibration") 33 | { } 34 | 35 | void initialize(UAS &uas_) 36 | { 37 | PluginBase::initialize(uas_); 38 | 39 | vibe_nh.param("frame_id", frame_id, "base_link"); 40 | 41 | vibration_pub = vibe_nh.advertise("raw/vibration", 10); 42 | } 43 | 44 | Subscriptions get_subscriptions() 45 | { 46 | return { 47 | make_handler(&VibrationPlugin::handle_vibration) 48 | }; 49 | } 50 | 51 | private: 52 | ros::NodeHandle vibe_nh; 53 | 54 | std::string frame_id; 55 | 56 | ros::Publisher vibration_pub; 57 | 58 | void handle_vibration(const mavlink::mavlink_message_t *msg, mavlink::common::msg::VIBRATION &vibration) 59 | { 60 | auto vibe_msg = boost::make_shared(); 61 | 62 | vibe_msg->header = m_uas->synchronized_header(frame_id, vibration.time_usec); 63 | 64 | Eigen::Vector3d vib_enu = {vibration.vibration_x, vibration.vibration_y, vibration.vibration_z}; 65 | tf::vectorEigenToMsg(ftf::transform_frame_ned_enu(vib_enu), vibe_msg->vibration); 66 | 67 | vibe_msg->clipping[0] = vibration.clipping_0; 68 | vibe_msg->clipping[1] = vibration.clipping_1; 69 | vibe_msg->clipping[2] = vibration.clipping_2; 70 | 71 | vibration_pub.publish(vibe_msg); 72 | } 73 | }; 74 | } // namespace extra_plugins 75 | } // namespace mavros 76 | 77 | #include 78 | PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::VibrationPlugin, mavros::plugin::PluginBase) 79 | -------------------------------------------------------------------------------- /mavros_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mavros_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS geographic_msgs geometry_msgs message_generation sensor_msgs std_msgs) 5 | 6 | include_directories(include) 7 | 8 | add_message_files( 9 | DIRECTORY msg 10 | FILES 11 | ADSBVehicle.msg 12 | ActuatorControl.msg 13 | AllocationMatrix.msg 14 | Altitude.msg 15 | AttitudeTarget.msg 16 | AttitudeThrustTarget.msg 17 | BatteryStatus.msg 18 | CamIMUStamp.msg 19 | CommandCode.msg 20 | CompanionProcessStatus.msg 21 | DebugValue.msg 22 | DynamixelStatus.msg 23 | ExtendedState.msg 24 | FileEntry.msg 25 | GlobalPositionTarget.msg 26 | HilActuatorControls.msg 27 | HilControls.msg 28 | HilGPS.msg 29 | HilSensor.msg 30 | HilStateQuaternion.msg 31 | HomePosition.msg 32 | LandingTarget.msg 33 | LogData.msg 34 | LogEntry.msg 35 | ManualControl.msg 36 | Mavlink.msg 37 | MountControl.msg 38 | OnboardComputerStatus.msg 39 | OpticalFlowRad.msg 40 | OverrideRCIn.msg 41 | Param.msg 42 | ParamValue.msg 43 | PositionTarget.msg 44 | RCIn.msg 45 | RCOut.msg 46 | RTCM.msg 47 | RadioStatus.msg 48 | State.msg 49 | StatusText.msg 50 | Thrust.msg 51 | TiltAngleTarget.msg 52 | TiltrotorActuatorCommands.msg 53 | TimesyncStatus.msg 54 | Trajectory.msg 55 | VFR_HUD.msg 56 | VehicleInfo.msg 57 | Vibration.msg 58 | Waypoint.msg 59 | WaypointList.msg 60 | WaypointReached.msg 61 | WheelOdomStamped.msg 62 | WrenchTarget.msg 63 | ) 64 | 65 | add_service_files( 66 | DIRECTORY srv 67 | FILES 68 | CommandBool.srv 69 | CommandHome.srv 70 | CommandInt.srv 71 | CommandLong.srv 72 | CommandTOL.srv 73 | CommandTriggerControl.srv 74 | CommandTriggerInterval.srv 75 | CommandVtolTransition.srv 76 | FileChecksum.srv 77 | FileClose.srv 78 | FileList.srv 79 | FileMakeDir.srv 80 | FileOpen.srv 81 | FileRead.srv 82 | FileRemove.srv 83 | FileRemoveDir.srv 84 | FileRename.srv 85 | FileTruncate.srv 86 | FileWrite.srv 87 | LogRequestData.srv 88 | LogRequestEnd.srv 89 | LogRequestList.srv 90 | MountConfigure.srv 91 | MessageInterval.srv 92 | ParamGet.srv 93 | ParamPull.srv 94 | ParamPush.srv 95 | ParamSet.srv 96 | SetMavFrame.srv 97 | SetMode.srv 98 | StreamRate.srv 99 | VehicleInfoGet.srv 100 | WaypointClear.srv 101 | WaypointPull.srv 102 | WaypointPush.srv 103 | WaypointSetCurrent.srv 104 | ) 105 | 106 | # add_action_files( 107 | # DIRECTORY action 108 | # FILES 109 | # Action1.action 110 | # Action2.action 111 | # ) 112 | 113 | generate_messages(DEPENDENCIES geographic_msgs geometry_msgs sensor_msgs std_msgs) 114 | 115 | catkin_package( 116 | INCLUDE_DIRS include 117 | CATKIN_DEPENDS geographic_msgs geometry_msgs message_runtime sensor_msgs std_msgs) 118 | 119 | install(DIRECTORY include/${PROJECT_NAME}/ 120 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 121 | FILES_MATCHING PATTERN "*.h") 122 | -------------------------------------------------------------------------------- /mavros_msgs/include/mavros_msgs/mavlink_convert.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief Mavlink convert utils 3 | * @file 4 | * @author Vladimir Ermakov 5 | */ 6 | /* 7 | * Copyright 2015,2016 Vladimir Ermakov. 8 | * 9 | * This file is part of the mavros package and subject to the license terms 10 | * in the top-level LICENSE file of the mavros repository. 11 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 12 | */ 13 | 14 | #pragma once 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | namespace mavros_msgs { 21 | namespace mavlink { 22 | 23 | using ::mavlink::mavlink_message_t; 24 | 25 | // [[[cog: 26 | // FIELD_NAMES = [ 27 | // "magic", 28 | // "len", 29 | // "incompat_flags", 30 | // "compat_flags", 31 | // "seq", 32 | // "sysid", 33 | // "compid", 34 | // "msgid", 35 | // "checksum", 36 | // ] 37 | // ]]] 38 | // [[[end]]] (checksum: d41d8cd98f00b204e9800998ecf8427e) 39 | 40 | /** 41 | * @brief Convert mavros_msgs/Mavlink message to mavlink_message_t 42 | * 43 | * @note signature vector should be empty for unsigned OR 44 | * MAVLINK_SIGNATURE_BLOCK size for signed messages 45 | * 46 | * @param[in] rmsg mavros_msgs/Mavlink message 47 | * @param[out] mmsg mavlink_message_t struct 48 | * @return true if success 49 | */ 50 | inline bool convert(const mavros_msgs::Mavlink &rmsg, mavlink_message_t &mmsg) 51 | { 52 | if (rmsg.payload64.size() > sizeof(mmsg.payload64) / sizeof(mmsg.payload64[0])) { 53 | return false; 54 | } 55 | 56 | if (!rmsg.signature.empty() && rmsg.signature.size() != sizeof(mmsg.signature)) { 57 | return false; 58 | } 59 | 60 | // [[[cog: 61 | // for f in FIELD_NAMES: 62 | // cog.outl("mmsg.%s = rmsg.%s;" % (f, f)) 63 | // ]]] 64 | mmsg.magic = rmsg.magic; 65 | mmsg.len = rmsg.len; 66 | mmsg.incompat_flags = rmsg.incompat_flags; 67 | mmsg.compat_flags = rmsg.compat_flags; 68 | mmsg.seq = rmsg.seq; 69 | mmsg.sysid = rmsg.sysid; 70 | mmsg.compid = rmsg.compid; 71 | mmsg.msgid = rmsg.msgid; 72 | mmsg.checksum = rmsg.checksum; 73 | // [[[end]]] (checksum: 2ef42a7798f261bfd367bf4157b11ec0) 74 | std::copy(rmsg.payload64.begin(), rmsg.payload64.end(), mmsg.payload64); 75 | std::copy(rmsg.signature.begin(), rmsg.signature.end(), mmsg.signature); 76 | 77 | return true; 78 | } 79 | 80 | /** 81 | * @brief Convert mavlink_message_t to mavros/Mavlink 82 | * 83 | * @param[in] mmsg mavlink_message_t struct 84 | * @param[out] rmsg mavros_msgs/Mavlink message 85 | * @param[in] framing_status framing parse result (OK, BAD_CRC or BAD_SIGNATURE) 86 | * @return true, this convertion can't fail 87 | */ 88 | inline bool convert(const mavlink_message_t &mmsg, mavros_msgs::Mavlink &rmsg, uint8_t framing_status = mavros_msgs::Mavlink::FRAMING_OK) 89 | { 90 | const size_t payload64_len = (mmsg.len + 7) / 8; 91 | 92 | rmsg.framing_status = framing_status; 93 | 94 | // [[[cog: 95 | // for f in FIELD_NAMES: 96 | // cog.outl("rmsg.%s = mmsg.%s;" % (f, f)) 97 | // ]]] 98 | rmsg.magic = mmsg.magic; 99 | rmsg.len = mmsg.len; 100 | rmsg.incompat_flags = mmsg.incompat_flags; 101 | rmsg.compat_flags = mmsg.compat_flags; 102 | rmsg.seq = mmsg.seq; 103 | rmsg.sysid = mmsg.sysid; 104 | rmsg.compid = mmsg.compid; 105 | rmsg.msgid = mmsg.msgid; 106 | rmsg.checksum = mmsg.checksum; 107 | // [[[end]]] (checksum: 4f0a50d2fcd7eb8823aea3e0806cd698) 108 | rmsg.payload64 = std::move(mavros_msgs::Mavlink::_payload64_type(mmsg.payload64, mmsg.payload64 + payload64_len)); 109 | 110 | // copy signature block only if message is signed 111 | if (mmsg.incompat_flags & MAVLINK_IFLAG_SIGNED) 112 | rmsg.signature = std::move(mavros_msgs::Mavlink::_signature_type(mmsg.signature, mmsg.signature + sizeof(mmsg.signature))); 113 | else 114 | rmsg.signature.clear(); 115 | 116 | return true; 117 | } 118 | 119 | } // namespace mavlink 120 | } // namespace mavros_msgs 121 | -------------------------------------------------------------------------------- /mavros_msgs/msg/ADSBVehicle.msg: -------------------------------------------------------------------------------- 1 | # The location and information of an ADSB vehicle 2 | # 3 | # https://mavlink.io/en/messages/common.html#ADSB_VEHICLE 4 | 5 | # [[[cog: 6 | # from pymavlink.dialects.v20 import common 7 | # 8 | # def decl_enum(ename, pfx='', bsz=8): 9 | # enum = sorted(common.enums[ename].items()) 10 | # enum.pop() # remove ENUM_END 11 | # 12 | # cog.outl("# " + ename) 13 | # for k, e in enum: 14 | # sn = e.name[len(ename) + 1:] 15 | # l = "uint{bsz} {pfx}{sn} = {k}".format(**locals()) 16 | # if e.description: 17 | # l += ' ' * (40 - len(l)) + ' # ' + e.description 18 | # cog.outl(l) 19 | # 20 | # decl_enum('ADSB_ALTITUDE_TYPE', 'ALT_') 21 | # decl_enum('ADSB_EMITTER_TYPE', 'EMITTER_') 22 | # decl_enum('ADSB_FLAGS', 'FLAG_', 16) 23 | # ]]] 24 | # ADSB_ALTITUDE_TYPE 25 | uint8 ALT_PRESSURE_QNH = 0 # Altitude reported from a Baro source using QNH reference 26 | uint8 ALT_GEOMETRIC = 1 # Altitude reported from a GNSS source 27 | # ADSB_EMITTER_TYPE 28 | uint8 EMITTER_NO_INFO = 0 29 | uint8 EMITTER_LIGHT = 1 30 | uint8 EMITTER_SMALL = 2 31 | uint8 EMITTER_LARGE = 3 32 | uint8 EMITTER_HIGH_VORTEX_LARGE = 4 33 | uint8 EMITTER_HEAVY = 5 34 | uint8 EMITTER_HIGHLY_MANUV = 6 35 | uint8 EMITTER_ROTOCRAFT = 7 36 | uint8 EMITTER_UNASSIGNED = 8 37 | uint8 EMITTER_GLIDER = 9 38 | uint8 EMITTER_LIGHTER_AIR = 10 39 | uint8 EMITTER_PARACHUTE = 11 40 | uint8 EMITTER_ULTRA_LIGHT = 12 41 | uint8 EMITTER_UNASSIGNED2 = 13 42 | uint8 EMITTER_UAV = 14 43 | uint8 EMITTER_SPACE = 15 44 | uint8 EMITTER_UNASSGINED3 = 16 45 | uint8 EMITTER_EMERGENCY_SURFACE = 17 46 | uint8 EMITTER_SERVICE_SURFACE = 18 47 | uint8 EMITTER_POINT_OBSTACLE = 19 48 | # ADSB_FLAGS 49 | uint16 FLAG_VALID_COORDS = 1 50 | uint16 FLAG_VALID_ALTITUDE = 2 51 | uint16 FLAG_VALID_HEADING = 4 52 | uint16 FLAG_VALID_VELOCITY = 8 53 | uint16 FLAG_VALID_CALLSIGN = 16 54 | uint16 FLAG_VALID_SQUAWK = 32 55 | uint16 FLAG_SIMULATED = 64 56 | # [[[end]]] (checksum: e35e77be43548bd572a3cb24138fc2fd) 57 | 58 | std_msgs/Header header 59 | 60 | uint32 ICAO_address 61 | string callsign 62 | 63 | float64 latitude 64 | float64 longitude 65 | float32 altitude # AMSL 66 | 67 | float32 heading # deg [0..360) 68 | float32 hor_velocity # m/s 69 | float32 ver_velocity # m/s 70 | 71 | uint8 altitude_type # Type from ADSB_ALTITUDE_TYPE enum 72 | uint8 emitter_type # Type from ADSB_EMITTER_TYPE enum 73 | 74 | duration tslc # Duration from last communication, seconds [0..255] 75 | uint16 flags # ADSB_FLAGS bit field 76 | uint16 squawk # Squawk code 77 | -------------------------------------------------------------------------------- /mavros_msgs/msg/ActuatorControl.msg: -------------------------------------------------------------------------------- 1 | # raw servo values for direct actuator controls 2 | # 3 | # about groups, mixing and channels: 4 | # https://pixhawk.org/dev/mixing 5 | 6 | # constant for mixer group 7 | uint8 PX4_MIX_FLIGHT_CONTROL = 0 8 | uint8 PX4_MIX_FLIGHT_CONTROL_VTOL_ALT = 1 9 | uint8 PX4_MIX_PAYLOAD = 2 10 | uint8 PX4_MIX_MANUAL_PASSTHROUGH = 3 11 | #uint8 PX4_MIX_FC_MC_VIRT = 4 12 | #uint8 PX4_MIX_FC_FW_VIRT = 5 13 | 14 | std_msgs/Header header 15 | uint8 group_mix 16 | float32[8] controls 17 | -------------------------------------------------------------------------------- /mavros_msgs/msg/AllocationMatrix.msg: -------------------------------------------------------------------------------- 1 | # Message for SET_ALLOCATION_MATRIX 2 | # 3 | 4 | std_msgs/Header header 5 | 6 | float32[36] allocation_matrix 7 | float32[6] tilt_angles 8 | -------------------------------------------------------------------------------- /mavros_msgs/msg/Altitude.msg: -------------------------------------------------------------------------------- 1 | # Altitude information 2 | # 3 | # https://mavlink.io/en/messages/common.html#ALTITUDE 4 | 5 | std_msgs/Header header 6 | 7 | float32 monotonic 8 | float32 amsl 9 | float32 local 10 | float32 relative 11 | float32 terrain 12 | float32 bottom_clearance 13 | -------------------------------------------------------------------------------- /mavros_msgs/msg/AttitudeTarget.msg: -------------------------------------------------------------------------------- 1 | # Message for SET_ATTITUDE_TARGET 2 | # 3 | # Some complex system requires all feautures that mavlink 4 | # message provide. See issue #402, #418. 5 | 6 | std_msgs/Header header 7 | 8 | uint8 type_mask 9 | uint8 IGNORE_ROLL_RATE = 1 # body_rate.x 10 | uint8 IGNORE_PITCH_RATE = 2 # body_rate.y 11 | uint8 IGNORE_YAW_RATE = 4 # body_rate.z 12 | uint8 IGNORE_THRUST = 64 13 | uint8 IGNORE_ATTITUDE = 128 # orientation field 14 | 15 | geometry_msgs/Quaternion orientation 16 | geometry_msgs/Vector3 body_rate 17 | float32 thrust 18 | -------------------------------------------------------------------------------- /mavros_msgs/msg/AttitudeThrustTarget.msg: -------------------------------------------------------------------------------- 1 | # Message for SET_WRENCH_TARGET_BODY_NED 2 | # 3 | 4 | std_msgs/Header header 5 | 6 | geometry_msgs/Vector3 linear_acceleration 7 | geometry_msgs/Vector3 angular_acceleration 8 | geometry_msgs/Quaternion orientation 9 | geometry_msgs/Vector3 rates_sp 10 | -------------------------------------------------------------------------------- /mavros_msgs/msg/BatteryStatus.msg: -------------------------------------------------------------------------------- 1 | # Represent battery status from SYSTEM_STATUS 2 | # 3 | # To be replaced when sensor_msgs/BatteryState PR will be merged 4 | # https://github.com/ros/common_msgs/pull/74 5 | 6 | std_msgs/Header header 7 | float32 voltage # [V] 8 | float32 current # [A] 9 | float32 remaining # 0..1 10 | -------------------------------------------------------------------------------- /mavros_msgs/msg/CamIMUStamp.msg: -------------------------------------------------------------------------------- 1 | # IMU-Camera synchronisation data 2 | 3 | time frame_stamp # Timestamp when the camera was triggered 4 | int32 frame_seq_id # Sequence number of the image frame 5 | -------------------------------------------------------------------------------- /mavros_msgs/msg/CompanionProcessStatus.msg: -------------------------------------------------------------------------------- 1 | # Mavros message: COMPANIONPROCESSSTATUS 2 | 3 | std_msgs/Header header 4 | 5 | uint8 state # See enum COMPANION_PROCESS_STATE 6 | uint8 component # See enum MAV_COMPONENT 7 | 8 | uint8 MAV_STATE_UNINIT = 0 9 | uint8 MAV_STATE_BOOT = 1 10 | uint8 MAV_STATE_CALIBRATING = 2 11 | uint8 MAV_STATE_STANDBY = 3 12 | uint8 MAV_STATE_ACTIVE = 4 13 | uint8 MAV_STATE_CRITICAL = 5 14 | uint8 MAV_STATE_EMERGENCY = 6 15 | uint8 MAV_STATE_POWEROFF = 7 16 | uint8 MAV_STATE_FLIGHT_TERMINATION = 8 17 | 18 | uint8 MAV_COMP_ID_OBSTACLE_AVOIDANCE = 196 19 | uint8 MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY = 197 20 | -------------------------------------------------------------------------------- /mavros_msgs/msg/DebugValue.msg: -------------------------------------------------------------------------------- 1 | # Msg for Debug MAVLink API 2 | # 3 | # Supported types: 4 | # DEBUG https://mavlink.io/en/messages/common.html#DEBUG 5 | # DEBUG_VECTOR https://mavlink.io/en/messages/common.html#DEBUG_VECT 6 | # NAMED_VALUE_FLOAT https://mavlink.io/en/messages/common.html#NAMED_VALUE_FLOAT 7 | # NAMED_VALUE_INT https://mavlink.io/en/messages/common.html#NAMED_VALUE_INT 8 | # @TODO: add support for DEBUG_ARRAY (https://github.com/mavlink/mavlink/pull/734) 9 | 10 | std_msgs/Header header 11 | 12 | int32 index # index value of DEBUG value (-1 if not indexed) 13 | 14 | string name # value name/key 15 | 16 | float32 value_float # float value for NAMED_VALUE_FLOAT and DEBUG 17 | int32 value_int # int value for NAMED_VALUE_INT 18 | float32[] data # DEBUG vector or array 19 | 20 | uint8 type 21 | uint8 TYPE_DEBUG = 0 22 | uint8 TYPE_DEBUG_VECT = 1 23 | uint8 TYPE_DEBUG_ARRAY = 2 24 | uint8 TYPE_NAMED_VALUE_FLOAT = 3 25 | uint8 TYPE_NAMED_VALUE_INT = 4 26 | -------------------------------------------------------------------------------- /mavros_msgs/msg/DynamixelStatus.msg: -------------------------------------------------------------------------------- 1 | # Tilt angles cmd and measurements for omav 2 | # 3 | 4 | std_msgs/Header header 5 | time msg_arrival_time 6 | float32[6] measured_angles #[rad] 7 | float32[6] cmd_angles #[rad] 8 | uint8 noutputs -------------------------------------------------------------------------------- /mavros_msgs/msg/ExtendedState.msg: -------------------------------------------------------------------------------- 1 | # Extended autopilot state 2 | # 3 | # https://mavlink.io/en/messages/common.html#EXTENDED_SYS_STATE 4 | 5 | uint8 VTOL_STATE_UNDEFINED = 0 6 | uint8 VTOL_STATE_TRANSITION_TO_FW = 1 7 | uint8 VTOL_STATE_TRANSITION_TO_MC = 2 8 | uint8 VTOL_STATE_MC = 3 9 | uint8 VTOL_STATE_FW = 4 10 | 11 | uint8 LANDED_STATE_UNDEFINED = 0 12 | uint8 LANDED_STATE_ON_GROUND = 1 13 | uint8 LANDED_STATE_IN_AIR = 2 14 | uint8 LANDED_STATE_TAKEOFF = 3 15 | uint8 LANDED_STATE_LANDING = 4 16 | 17 | std_msgs/Header header 18 | uint8 vtol_state 19 | uint8 landed_state 20 | -------------------------------------------------------------------------------- /mavros_msgs/msg/FileEntry.msg: -------------------------------------------------------------------------------- 1 | # File/Dir information 2 | 3 | uint8 TYPE_FILE = 0 4 | uint8 TYPE_DIRECTORY = 1 5 | 6 | string name 7 | uint8 type 8 | uint64 size 9 | 10 | # Not supported by MAVLink FTP 11 | #time atime 12 | #int32 access_flags 13 | -------------------------------------------------------------------------------- /mavros_msgs/msg/GlobalPositionTarget.msg: -------------------------------------------------------------------------------- 1 | # Message for SET_POSITION_TARGET_GLOBAL_INT 2 | # 3 | # https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_GLOBAL_INT 4 | # Some complex system requires all feautures that mavlink 5 | # message provide. See issue #402, #415. 6 | 7 | std_msgs/Header header 8 | 9 | uint8 coordinate_frame 10 | uint8 FRAME_GLOBAL_INT = 5 11 | uint8 FRAME_GLOBAL_REL_ALT = 6 12 | uint8 FRAME_GLOBAL_TERRAIN_ALT = 11 13 | 14 | uint16 type_mask 15 | uint16 IGNORE_LATITUDE = 1 # Position ignore flags 16 | uint16 IGNORE_LONGITUDE = 2 17 | uint16 IGNORE_ALTITUDE = 4 18 | uint16 IGNORE_VX = 8 # Velocity vector ignore flags 19 | uint16 IGNORE_VY = 16 20 | uint16 IGNORE_VZ = 32 21 | uint16 IGNORE_AFX = 64 # Acceleration/Force vector ignore flags 22 | uint16 IGNORE_AFY = 128 23 | uint16 IGNORE_AFZ = 256 24 | uint16 FORCE = 512 # Force in af vector flag 25 | uint16 IGNORE_YAW = 1024 26 | uint16 IGNORE_YAW_RATE = 2048 27 | 28 | float64 latitude 29 | float64 longitude 30 | float32 altitude # in meters, AMSL or above terrain 31 | geometry_msgs/Vector3 velocity 32 | geometry_msgs/Vector3 acceleration_or_force 33 | float32 yaw 34 | float32 yaw_rate 35 | -------------------------------------------------------------------------------- /mavros_msgs/msg/HilActuatorControls.msg: -------------------------------------------------------------------------------- 1 | # HilActuatorControls.msg 2 | # 3 | # ROS representation of MAVLink HIL_ACTUATOR_CONTROLS 4 | # See mavlink message documentation here: 5 | # https://mavlink.io/en/messages/common.html#HIL_ACTUATOR_CONTROLS 6 | 7 | std_msgs/Header header 8 | float32[16] controls 9 | uint8 mode 10 | uint64 flags 11 | -------------------------------------------------------------------------------- /mavros_msgs/msg/HilControls.msg: -------------------------------------------------------------------------------- 1 | # HilControls.msg 2 | # 3 | # ROS representation of MAVLink HIL_CONTROLS 4 | # (deprecated, use HIL_ACTUATOR_CONTROLS instead) 5 | # See mavlink message documentation here: 6 | # https://mavlink.io/en/messages/common.html#HIL_CONTROLS 7 | 8 | std_msgs/Header header 9 | float32 roll_ailerons 10 | float32 pitch_elevator 11 | float32 yaw_rudder 12 | float32 throttle 13 | float32 aux1 14 | float32 aux2 15 | float32 aux3 16 | float32 aux4 17 | uint8 mode 18 | uint8 nav_mode 19 | -------------------------------------------------------------------------------- /mavros_msgs/msg/HilGPS.msg: -------------------------------------------------------------------------------- 1 | # HilControls.msg 2 | # 3 | # ROS representation of MAVLink HIL_GPS 4 | # See mavlink message documentation here: 5 | # https://mavlink.io/en/messages/common.html#HIL_GPS 6 | 7 | std_msgs/Header header 8 | uint8 fix_type 9 | geographic_msgs/GeoPoint geo 10 | uint16 eph 11 | uint16 epv 12 | uint16 vel 13 | int16 vn 14 | int16 ve 15 | int16 vd 16 | uint16 cog 17 | uint8 satellites_visible 18 | -------------------------------------------------------------------------------- /mavros_msgs/msg/HilSensor.msg: -------------------------------------------------------------------------------- 1 | # HilSensor.msg 2 | # 3 | # ROS representation of MAVLink HIL_SENSOR 4 | # See mavlink message documentation here: 5 | # https://mavlink.io/en/messages/common.html#HIL_SENSOR 6 | 7 | std_msgs/Header header 8 | 9 | geometry_msgs/Vector3 acc 10 | geometry_msgs/Vector3 gyro 11 | geometry_msgs/Vector3 mag 12 | float32 abs_pressure 13 | float32 diff_pressure 14 | float32 pressure_alt 15 | float32 temperature 16 | uint32 fields_updated 17 | -------------------------------------------------------------------------------- /mavros_msgs/msg/HilStateQuaternion.msg: -------------------------------------------------------------------------------- 1 | # HilStateQuaternion.msg 2 | # 3 | # ROS representation of MAVLink HIL_STATE_QUATERNION 4 | # See mavlink message documentation here: 5 | # https://mavlink.io/en/messages/common.html#HIL_STATE_QUATERNION 6 | 7 | std_msgs/Header header 8 | 9 | geometry_msgs/Quaternion orientation 10 | geometry_msgs/Vector3 angular_velocity 11 | geometry_msgs/Vector3 linear_acceleration 12 | geometry_msgs/Vector3 linear_velocity 13 | geographic_msgs/GeoPoint geo 14 | float32 ind_airspeed 15 | float32 true_airspeed 16 | -------------------------------------------------------------------------------- /mavros_msgs/msg/HomePosition.msg: -------------------------------------------------------------------------------- 1 | # MAVLink message: HOME_POSITION 2 | # https://mavlink.io/en/messages/common.html#HOME_POSITION 3 | 4 | std_msgs/Header header 5 | 6 | geographic_msgs/GeoPoint geo # geodetic coordinates in WGS-84 datum 7 | 8 | geometry_msgs/Point position # local position 9 | geometry_msgs/Quaternion orientation # XXX: verify field name (q[4]) 10 | geometry_msgs/Vector3 approach # position of the end of approach vector 11 | -------------------------------------------------------------------------------- /mavros_msgs/msg/LandingTarget.msg: -------------------------------------------------------------------------------- 1 | # MAVLink message: LANDING_TARGET 2 | # https://mavlink.io/en/messages/common.html 3 | 4 | std_msgs/Header header 5 | 6 | ## MAV_FRAME enum 7 | uint8 GLOBAL = 0 # Global coordinate frame, WGS84 coordinate system. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL) 8 | uint8 LOCAL_NED = 2 # Local coordinate frame, Z-up (x: north, y: east, z: down). 9 | uint8 MISSION = 3 # NOT a coordinate frame, indicates a mission command. 10 | uint8 GLOBAL_RELATIVE_ALT = 4 # Global coordinate frame, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. 11 | uint8 LOCAL_ENU = 5 # Local coordinate frame, Z-down (x: east, y: north, z: up) 12 | uint8 GLOBAL_INT = 6 # Global coordinate frame, WGS84 coordinate system. First value / x: latitude in degrees*1.0e-7, second value / y: longitude in degrees*1.0e-7, third value / z: positive altitude over mean sea level (MSL) 13 | uint8 GLOBAL_RELATIVE_ALT_INT = 7 # Global coordinate frame, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude with 0 being at the altitude of the home location. 14 | uint8 LOCAL_OFFSET_NED = 8 # Offset to the current local frame. Anything expressed in this frame should be added to the current local frame position. 15 | uint8 BODY_NED = 9 # Setpoint in body NED frame. This makes sense if all position control is externalized - e.g. useful to command 2 m/s^2 acceleration to the right. 16 | uint8 BODY_OFFSET_NED = 10 # Offset in body NED frame. This makes sense if adding setpoints to the current flight path, to avoid an obstacle - e.g. useful to command 2 m/s^2 acceleration to the east. 17 | uint8 GLOBAL_TERRAIN_ALT = 11 # Global coordinate frame with above terrain level altitude. WGS84 coordinate system, relative altitude over terrain with respect to the waypoint coordinate. First value / x: latitude in degrees, second value / y: longitude in degrees, third value / z: positive altitude in meters with 0 being at ground level in terrain model. 18 | uint8 GLOBAL_TERRAIN_ALT_INT = 12 # Global coordinate frame with above terrain level altitude. WGS84 coordinate system, relative altitude over terrain with respect to the waypoint coordinate. First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude in meters with 0 being at ground level in terrain model. 19 | 20 | ## LANDING_TARGET_TYPE enum 21 | uint8 LIGHT_BEACON = 0 # Landing target signaled by light beacon (ex: IR-LOCK) 22 | uint8 RADIO_BEACON = 1 # Landing target signaled by radio beacon (ex: ILS, NDB) 23 | uint8 VISION_FIDUCIAL = 2 # Landing target represented by a fiducial marker (ex: ARTag) 24 | uint8 VISION_OTHER = 3 # Landing target represented by a pre-defined visual shape/feature (ex: X-marker, H-marker, square) 25 | 26 | uint8 target_num 27 | uint8 frame 28 | float32[2] angle 29 | float32 distance 30 | float32[2] size 31 | geometry_msgs/Pose pose 32 | uint8 type 33 | -------------------------------------------------------------------------------- /mavros_msgs/msg/LogData.msg: -------------------------------------------------------------------------------- 1 | # Reply to LogRequestData, - a chunk of a log 2 | # 3 | # :id: - log id 4 | # :offset: - offset into the log 5 | # :data: - chunk of data (if zero-sized, then there are no more chunks) 6 | 7 | std_msgs/Header header 8 | 9 | uint16 id 10 | uint32 offset 11 | uint8[] data 12 | -------------------------------------------------------------------------------- /mavros_msgs/msg/LogEntry.msg: -------------------------------------------------------------------------------- 1 | # Information about a single log 2 | # 3 | # :id: - log id 4 | # :num_logs: - total number of logs 5 | # :last_log_num: - id of last log 6 | # :time_utc: - UTC timestamp of log (ros::Time(0) if not available) 7 | # :size: - size of log in bytes (may be approximate) 8 | 9 | std_msgs/Header header 10 | 11 | uint16 id 12 | uint16 num_logs 13 | uint16 last_log_num 14 | time time_utc 15 | uint32 size 16 | -------------------------------------------------------------------------------- /mavros_msgs/msg/ManualControl.msg: -------------------------------------------------------------------------------- 1 | # Manual Control state 2 | std_msgs/Header header 3 | float32 x 4 | float32 y 5 | float32 z 6 | float32 r 7 | uint16 buttons 8 | -------------------------------------------------------------------------------- /mavros_msgs/msg/Mavlink.msg: -------------------------------------------------------------------------------- 1 | # Mavlink message transport type. 2 | # 3 | # Used to transport mavlink_message_t via ROS topic 4 | # 5 | # :framing_status: 6 | # Frame decoding status: OK, CRC error, bad Signature (mavlink v2.0) 7 | # You may simply drop all non valid messages. 8 | # Used for GCS Bridge to transport unknown messages. 9 | # 10 | # :magic: 11 | # STX byte, used to determine protocol version v1.0 or v2.0. 12 | # 13 | # Please use mavros_msgs::mavlink::convert() from 14 | # to convert between ROS and MAVLink message type 15 | 16 | # mavlink_framing_t enum 17 | uint8 FRAMING_OK = 1 18 | uint8 FRAMING_BAD_CRC = 2 19 | uint8 FRAMING_BAD_SIGNATURE = 3 20 | 21 | # stx values 22 | uint8 MAVLINK_V10 = 254 23 | uint8 MAVLINK_V20 = 253 24 | 25 | std_msgs/Header header 26 | uint8 framing_status 27 | 28 | uint8 magic # STX byte 29 | uint8 len 30 | uint8 incompat_flags 31 | uint8 compat_flags 32 | uint8 seq 33 | uint8 sysid 34 | uint8 compid 35 | uint32 msgid # 24-bit message id 36 | uint16 checksum 37 | uint64[] payload64 38 | uint8[] signature # optional signature 39 | -------------------------------------------------------------------------------- /mavros_msgs/msg/MountControl.msg: -------------------------------------------------------------------------------- 1 | # MAVLink message: DO_MOUNT_CONTROL 2 | # https://mavlink.io/en/messages/common.html#MAV_CMD_DO_MOUNT_CONTROL 3 | 4 | std_msgs/Header header 5 | 6 | uint8 mode # See enum MAV_MOUNT_MODE. 7 | uint8 MAV_MOUNT_MODE_RETRACT = 0 8 | uint8 MAV_MOUNT_MODE_NEUTRAL = 1 9 | uint8 MAV_MOUNT_MODE_MAVLINK_TARGETING = 2 10 | uint8 MAV_MOUNT_MODE_RC_TARGETING = 3 11 | uint8 MAV_MOUNT_MODE_GPS_POINT = 4 12 | 13 | float32 pitch # roll degrees or degrees/second depending on mount mode. 14 | float32 roll # roll degrees or degrees/second depending on mount mode. 15 | float32 yaw # roll degrees or degrees/second depending on mount mode. 16 | float32 altitude # altitude depending on mount mode. 17 | float32 latitude # latitude in degrees * 1E7, set if appropriate mount mode. 18 | float32 longitude # longitude in degrees * 1E7, set if appropriate mount mode. -------------------------------------------------------------------------------- /mavros_msgs/msg/OnboardComputerStatus.msg: -------------------------------------------------------------------------------- 1 | # Mavros message: ONBOARDCOMPUTERSTATUS 2 | 3 | std_msgs/Header header 4 | 5 | uint8 component # See enum MAV_COMPONENT 6 | 7 | uint32 uptime # [ms] Time since system boot 8 | uint8 type # Type of the onboard computer: 0: Mission computer primary, 1: Mission computer backup 1, 2: Mission computer backup 2, 3: Compute node, 4-5: Compute spares, 6-9: Payload computers. 9 | uint8[8] cpu_cores # CPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused. 10 | uint8[10] cpu_combined # Combined CPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused 11 | uint8[4] gpu_cores # GPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused 12 | uint8[10] gpu_combined # Combined GPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused. 13 | int8 temperature_board # [degC] Temperature of the board. A value of INT8_MAX implies the field is unused. 14 | int8[8] temperature_core # [degC] Temperature of the CPU core. A value of INT8_MAX implies the field is unused. 15 | int16[4] fan_speed # [rpm] Fan speeds. A value of INT16_MAX implies the field is unused. 16 | uint32 ram_usage # [MiB] Amount of used RAM on the component system. A value of UINT32_MAX implies the field is unused. 17 | uint32 ram_total # [MiB] Total amount of RAM on the component system. A value of UINT32_MAX implies the field is unused. 18 | uint32[4] storage_type # Storage type: 0: HDD, 1: SSD, 2: EMMC, 3: SD card (non-removable), 4: SD card (removable). A value of UINT32_MAX implies the field is unused. 19 | uint32[4] storage_usage # [MiB] Amount of used storage space on the component system. A value of UINT32_MAX implies the field is unused. 20 | uint32[4] storage_total # [MiB] Total amount of storage space on the component system. A value of UINT32_MAX implies the field is unused. 21 | uint32[6] link_type # Link type: 0-9: UART, 10-19: Wired network, 20-29: Wifi, 30-39: Point-to-point proprietary, 40-49: Mesh proprietary. 22 | uint32[6] link_tx_rate # [KiB/s] Network traffic from the component system. A value of UINT32_MAX implies the field is unused. 23 | uint32[6] link_rx_rate # [KiB/s] Network traffic to the component system. A value of UINT32_MAX implies the field is unused. 24 | uint32[6] link_tx_max # [KiB/s] Network capacity from the component system. A value of UINT32_MAX implies the field is unused. 25 | uint32[6] link_rx_max # [KiB/s] Network capacity to the component system. A value of UINT32_MAX implies the field is unused. -------------------------------------------------------------------------------- /mavros_msgs/msg/OpticalFlowRad.msg: -------------------------------------------------------------------------------- 1 | # OPTICAL_FLOW_RAD message data 2 | 3 | std_msgs/Header header 4 | 5 | uint32 integration_time_us 6 | float32 integrated_x 7 | float32 integrated_y 8 | float32 integrated_xgyro 9 | float32 integrated_ygyro 10 | float32 integrated_zgyro 11 | int16 temperature 12 | uint8 quality 13 | uint32 time_delta_distance_us 14 | float32 distance 15 | -------------------------------------------------------------------------------- /mavros_msgs/msg/OverrideRCIn.msg: -------------------------------------------------------------------------------- 1 | # Override RC Input 2 | # Currently MAVLink defines override for 8 channel 3 | 4 | uint16 CHAN_RELEASE=0 5 | uint16 CHAN_NOCHANGE=65535 6 | 7 | uint16[8] channels 8 | -------------------------------------------------------------------------------- /mavros_msgs/msg/Param.msg: -------------------------------------------------------------------------------- 1 | # Parameter msg. 2 | 3 | std_msgs/Header header 4 | 5 | string param_id 6 | mavros_msgs/ParamValue value 7 | 8 | uint16 param_index 9 | uint16 param_count 10 | -------------------------------------------------------------------------------- /mavros_msgs/msg/ParamValue.msg: -------------------------------------------------------------------------------- 1 | # Parameter value storage type. 2 | # 3 | # Integer and float fields: 4 | # 5 | # if integer != 0: it is integer value 6 | # else if real != 0.0: it is float value 7 | # else: it is zero. 8 | 9 | int64 integer 10 | float64 real 11 | -------------------------------------------------------------------------------- /mavros_msgs/msg/PositionTarget.msg: -------------------------------------------------------------------------------- 1 | # Message for SET_POSITION_TARGET_LOCAL_NED 2 | # 3 | # Some complex system requires all feautures that mavlink 4 | # message provide. See issue #402. 5 | 6 | std_msgs/Header header 7 | 8 | uint8 coordinate_frame 9 | uint8 FRAME_LOCAL_NED = 1 10 | uint8 FRAME_LOCAL_OFFSET_NED = 7 11 | uint8 FRAME_BODY_NED = 8 12 | uint8 FRAME_BODY_OFFSET_NED = 9 13 | 14 | uint16 type_mask 15 | uint16 IGNORE_PX = 1 # Position ignore flags 16 | uint16 IGNORE_PY = 2 17 | uint16 IGNORE_PZ = 4 18 | uint16 IGNORE_VX = 8 # Velocity vector ignore flags 19 | uint16 IGNORE_VY = 16 20 | uint16 IGNORE_VZ = 32 21 | uint16 IGNORE_AFX = 64 # Acceleration/Force vector ignore flags 22 | uint16 IGNORE_AFY = 128 23 | uint16 IGNORE_AFZ = 256 24 | uint16 FORCE = 512 # Force in af vector flag 25 | uint16 IGNORE_YAW = 1024 26 | uint16 IGNORE_YAW_RATE = 2048 27 | 28 | geometry_msgs/Point position 29 | geometry_msgs/Vector3 velocity 30 | geometry_msgs/Vector3 acceleration_or_force 31 | float32 yaw 32 | float32 yaw_rate 33 | -------------------------------------------------------------------------------- /mavros_msgs/msg/RCIn.msg: -------------------------------------------------------------------------------- 1 | # RAW RC input state 2 | 3 | std_msgs/Header header 4 | uint8 rssi 5 | uint16[] channels 6 | -------------------------------------------------------------------------------- /mavros_msgs/msg/RCOut.msg: -------------------------------------------------------------------------------- 1 | # RAW Servo out state 2 | 3 | std_msgs/Header header 4 | uint16[] channels 5 | -------------------------------------------------------------------------------- /mavros_msgs/msg/RTCM.msg: -------------------------------------------------------------------------------- 1 | # RTCM message for the gps_rtk plugin 2 | # The gps_rtk plugin will fragment the data if necessary and 3 | # forward it to the FCU via Mavlink through the available link. 4 | # data should be <= 4*180, higher will be discarded. 5 | std_msgs/Header header 6 | uint8[] data 7 | -------------------------------------------------------------------------------- /mavros_msgs/msg/RadioStatus.msg: -------------------------------------------------------------------------------- 1 | # RADIO_STATUS message 2 | 3 | std_msgs/Header header 4 | 5 | # message data 6 | uint8 rssi 7 | uint8 remrssi 8 | uint8 txbuf 9 | uint8 noise 10 | uint8 remnoise 11 | uint16 rxerrors 12 | uint16 fixed 13 | 14 | # calculated 15 | float32 rssi_dbm 16 | float32 remrssi_dbm 17 | -------------------------------------------------------------------------------- /mavros_msgs/msg/State.msg: -------------------------------------------------------------------------------- 1 | # Current autopilot state 2 | # 3 | # Known modes listed here: 4 | # http://wiki.ros.org/mavros/CustomModes 5 | # 6 | # For system_status values 7 | # see https://mavlink.io/en/messages/common.html#MAV_STATE 8 | # 9 | 10 | std_msgs/Header header 11 | bool connected 12 | bool armed 13 | bool guided 14 | bool manual_input 15 | string mode 16 | uint8 system_status 17 | -------------------------------------------------------------------------------- /mavros_msgs/msg/StatusText.msg: -------------------------------------------------------------------------------- 1 | # STATUSTEXT message representation 2 | # https://mavlink.io/en/messages/common.html#STATUSTEXT 3 | 4 | # Severity levels 5 | uint8 EMERGENCY = 0 6 | uint8 ALERT = 1 7 | uint8 CRITICAL = 2 8 | uint8 ERROR = 3 9 | uint8 WARNING = 4 10 | uint8 NOTICE = 5 11 | uint8 INFO = 6 12 | uint8 DEBUG = 7 13 | 14 | # Fields 15 | std_msgs/Header header 16 | uint8 severity 17 | string text 18 | -------------------------------------------------------------------------------- /mavros_msgs/msg/Thrust.msg: -------------------------------------------------------------------------------- 1 | # Thrust to send to the FCU 2 | 3 | std_msgs/Header header 4 | 5 | float32 thrust 6 | -------------------------------------------------------------------------------- /mavros_msgs/msg/TiltAngleTarget.msg: -------------------------------------------------------------------------------- 1 | # Tilt angles for overactuated vehicles 2 | # 3 | 4 | std_msgs/Header header 5 | 6 | float32[6] alpha 7 | -------------------------------------------------------------------------------- /mavros_msgs/msg/TiltrotorActuatorCommands.msg: -------------------------------------------------------------------------------- 1 | # Tiltrotor actuator commands for overactuated vehicles 2 | # 3 | 4 | std_msgs/Header header 5 | 6 | float32[6] u_tiltangles 7 | float32[12] u_rotors 8 | -------------------------------------------------------------------------------- /mavros_msgs/msg/TimesyncStatus.msg: -------------------------------------------------------------------------------- 1 | # Status of the MAVLink time synchroniser 2 | 3 | std_msgs/Header header 4 | uint64 remote_timestamp_ns # remote system timestamp (nanoseconds) 5 | int64 observed_offset_ns # raw time offset directly observed from this timesync packet (nanoseconds) 6 | int64 estimated_offset_ns # smoothed time offset between companion system and Mavros (nanoseconds) 7 | float32 round_trip_time_ms # round trip time of this timesync packet (milliseconds) -------------------------------------------------------------------------------- /mavros_msgs/msg/Trajectory.msg: -------------------------------------------------------------------------------- 1 | # MAVLink message: TRAJECTORY 2 | # https://mavlink.io/en/messages/common.html#TRAJECTORY 3 | 4 | std_msgs/Header header 5 | 6 | uint8 type # See enum MAV_TRAJECTORY_REPRESENTATION. 7 | uint8 MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS = 0 8 | uint8 MAV_TRAJECTORY_REPRESENTATION_BEZIER = 1 9 | 10 | mavros_msgs/PositionTarget point_1 11 | mavros_msgs/PositionTarget point_2 12 | mavros_msgs/PositionTarget point_3 13 | mavros_msgs/PositionTarget point_4 14 | mavros_msgs/PositionTarget point_5 15 | 16 | uint8[5] point_valid # States if respective point is valid. 17 | uint16[5] command # MAV_CMD associated with each point. UINT16_MAX if unused. 18 | 19 | float32[5] time_horizon # if type MAV_TRAJECTORY_REPRESENTATION_BEZIER, it represents the time horizon for each point, otherwise set to NaN 20 | -------------------------------------------------------------------------------- /mavros_msgs/msg/VFR_HUD.msg: -------------------------------------------------------------------------------- 1 | # Metrics typically displayed on a HUD for fixed wing aircraft 2 | # 3 | # VFR_HUD message 4 | 5 | std_msgs/Header header 6 | float32 airspeed # m/s 7 | float32 groundspeed # m/s 8 | int16 heading # degrees 0..360 9 | float32 throttle # normalized to 0.0..1.0 10 | float32 altitude # MSL 11 | float32 climb # current climb rate m/s 12 | -------------------------------------------------------------------------------- /mavros_msgs/msg/VehicleInfo.msg: -------------------------------------------------------------------------------- 1 | # Vehicle Info msg 2 | 3 | std_msgs/Header header 4 | 5 | uint8 HAVE_INFO_HEARTBEAT = 1 6 | uint8 HAVE_INFO_AUTOPILOT_VERSION = 2 7 | uint8 available_info # Bitmap shows what info is available 8 | 9 | # Vehicle address 10 | uint8 sysid # SYSTEM ID 11 | uint8 compid # COMPONENT ID 12 | 13 | # -*- Heartbeat info -*- 14 | uint8 autopilot # MAV_AUTOPILOT 15 | uint8 type # MAV_TYPE 16 | uint8 system_status # MAV_STATE 17 | uint8 base_mode 18 | uint32 custom_mode 19 | string mode # MAV_MODE string 20 | uint32 mode_id # MAV_MODE number 21 | 22 | # -*- Autopilot version -*- 23 | uint64 capabilities # MAV_PROTOCOL_CAPABILITY 24 | uint32 flight_sw_version # Firmware version number 25 | uint32 middleware_sw_version # Middleware version number 26 | uint32 os_sw_version # Operating system version number 27 | uint32 board_version # HW / board version (last 8 bytes should be silicon ID, if any) 28 | uint16 vendor_id # ID of the board vendor 29 | uint16 product_id # ID of the product 30 | uint64 uid # UID if provided by hardware 31 | -------------------------------------------------------------------------------- /mavros_msgs/msg/Vibration.msg: -------------------------------------------------------------------------------- 1 | # VIBRATION message data 2 | # @description: Vibration levels and accelerometer clipping 3 | 4 | std_msgs/Header header 5 | 6 | geometry_msgs/Vector3 vibration # 3-axis vibration levels 7 | float32[3] clipping # Accelerometers clipping -------------------------------------------------------------------------------- /mavros_msgs/msg/Waypoint.msg: -------------------------------------------------------------------------------- 1 | # Waypoint.msg 2 | # 3 | # ROS representation of MAVLink MISSION_ITEM 4 | # See mavlink documentation 5 | 6 | 7 | 8 | # see enum MAV_FRAME 9 | uint8 frame 10 | uint8 FRAME_GLOBAL = 0 11 | uint8 FRAME_LOCAL_NED = 1 12 | uint8 FRAME_MISSION = 2 13 | uint8 FRAME_GLOBAL_REL_ALT = 3 14 | uint8 FRAME_LOCAL_ENU = 4 15 | 16 | # see enum MAV_CMD and CommandCode.msg 17 | uint16 command 18 | 19 | bool is_current 20 | bool autocontinue 21 | # meaning of this params described in enum MAV_CMD 22 | float32 param1 23 | float32 param2 24 | float32 param3 25 | float32 param4 26 | float64 x_lat 27 | float64 y_long 28 | float64 z_alt 29 | -------------------------------------------------------------------------------- /mavros_msgs/msg/WaypointList.msg: -------------------------------------------------------------------------------- 1 | # WaypointList.msg 2 | # 3 | # :current_seq: seq nr of currently active waypoint 4 | # waypoints[current_seq].is_current == True 5 | # 6 | # :waypoints: list of waypoints 7 | 8 | uint16 current_seq 9 | mavros_msgs/Waypoint[] waypoints 10 | -------------------------------------------------------------------------------- /mavros_msgs/msg/WaypointReached.msg: -------------------------------------------------------------------------------- 1 | # That message represent MISSION_ITEM_REACHED 2 | # 3 | # :wp_seq: index number of reached waypoint 4 | 5 | Header header 6 | 7 | uint16 wp_seq 8 | -------------------------------------------------------------------------------- /mavros_msgs/msg/WheelOdomStamped.msg: -------------------------------------------------------------------------------- 1 | # Stamped wheel odometry message 2 | # 3 | # For streaming timestamped data from FCU wheel encoders (RPM or WHEEL_DISTANCE). 4 | 5 | std_msgs/Header header 6 | float64[] data 7 | -------------------------------------------------------------------------------- /mavros_msgs/msg/WrenchTarget.msg: -------------------------------------------------------------------------------- 1 | # Message for SET_WRENCH_TARGET_BODY_NED 2 | # 3 | 4 | std_msgs/Header header 5 | 6 | geometry_msgs/Vector3 linear_acceleration 7 | geometry_msgs/Vector3 angular_acceleration 8 | -------------------------------------------------------------------------------- /mavros_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mavros_msgs 4 | 0.33.3 5 | 6 | mavros_msgs defines messages for MAVROS. 7 | 8 | 9 | Vladimir Ermakov 10 | Vladimir Ermakov 11 | GPLv3 12 | LGPLv3 13 | BSD 14 | 15 | http://wiki.ros.org/mavros_msgs 16 | https://github.com/mavlink/mavros.git 17 | https://github.com/mavlink/mavros/issues 18 | 19 | catkin 20 | 21 | message_generation 22 | message_runtime 23 | std_msgs 24 | geometry_msgs 25 | sensor_msgs 26 | geographic_msgs 27 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /mavros_msgs/srv/CommandBool.srv: -------------------------------------------------------------------------------- 1 | # Common type for switch commands 2 | 3 | bool value 4 | --- 5 | bool success 6 | uint8 result 7 | -------------------------------------------------------------------------------- /mavros_msgs/srv/CommandHome.srv: -------------------------------------------------------------------------------- 1 | # request set new home position 2 | 3 | bool current_gps 4 | float32 latitude 5 | float32 longitude 6 | float32 altitude 7 | --- 8 | bool success 9 | uint8 result 10 | -------------------------------------------------------------------------------- /mavros_msgs/srv/CommandInt.srv: -------------------------------------------------------------------------------- 1 | # Generic COMMAND_INT 2 | 3 | bool broadcast # send this command in broadcast mode 4 | 5 | uint8 frame 6 | uint16 command 7 | uint8 current 8 | uint8 autocontinue 9 | float32 param1 10 | float32 param2 11 | float32 param3 12 | float32 param4 13 | int32 x # latitude in deg * 1E7 or local x * 1E4 m 14 | int32 y # longitude in deg * 1E7 or local y * 1E4 m 15 | float32 z # altitude 16 | --- 17 | bool success 18 | # seems that this message don't produce andy COMMAND_ACK messages 19 | # so no result field 20 | -------------------------------------------------------------------------------- /mavros_msgs/srv/CommandLong.srv: -------------------------------------------------------------------------------- 1 | # Generic COMMAND_LONG 2 | 3 | bool broadcast # send this command in broadcast mode 4 | 5 | uint16 command 6 | uint8 confirmation 7 | float32 param1 8 | float32 param2 9 | float32 param3 10 | float32 param4 11 | float32 param5 # x_lat 12 | float32 param6 # y_lon 13 | float32 param7 # z_alt 14 | --- 15 | bool success 16 | # raw result returned by COMMAND_ACK 17 | uint8 result 18 | -------------------------------------------------------------------------------- /mavros_msgs/srv/CommandTOL.srv: -------------------------------------------------------------------------------- 1 | # Common type for Take Off and Landing 2 | 3 | float32 min_pitch # used by takeoff 4 | float32 yaw 5 | float32 latitude 6 | float32 longitude 7 | float32 altitude 8 | --- 9 | bool success 10 | uint8 result 11 | -------------------------------------------------------------------------------- /mavros_msgs/srv/CommandTriggerControl.srv: -------------------------------------------------------------------------------- 1 | # Type for controlling onboard camera triggering system 2 | 3 | bool trigger_enable # Trigger enable/disable 4 | bool sequence_reset # Reset the trigger sequence 5 | bool trigger_pause # Pause triggering, but without switching the camera off or retracting it. 6 | --- 7 | bool success 8 | uint8 result 9 | -------------------------------------------------------------------------------- /mavros_msgs/srv/CommandTriggerInterval.srv: -------------------------------------------------------------------------------- 1 | # Type for controlling camera trigger interval and integration time 2 | 3 | float32 cycle_time # Trigger cycle_time (interval between to triggers) - set to 0 to ignore command 4 | float32 integration_time # Camera shutter integration_time - set to 0 to ignore command 5 | --- 6 | bool success 7 | uint8 result -------------------------------------------------------------------------------- /mavros_msgs/srv/CommandVtolTransition.srv: -------------------------------------------------------------------------------- 1 | 2 | # MAVLink command: DO_VTOL_TRANSITION 3 | # https://mavlink.io/en/messages/common.html#MAV_CMD_DO_VTOL_TRANSITION 4 | 5 | std_msgs/Header header 6 | 7 | # MAV_VTOL_STATE 8 | uint8 STATE_MC = 3 9 | uint8 STATE_FW = 4 10 | 11 | uint8 state # See enum MAV_VTOL_STATE. 12 | 13 | --- 14 | bool success 15 | uint8 result # Raw result returned by COMMAND_ACK 16 | -------------------------------------------------------------------------------- /mavros_msgs/srv/FileChecksum.srv: -------------------------------------------------------------------------------- 1 | # FTP::Checksum 2 | # 3 | # :file_path: file to calculate checksum 4 | # :crc32: file checksum 5 | # :success: indicates success end of request 6 | # :r_errno: remote errno if applicapable 7 | 8 | string file_path 9 | --- 10 | uint32 crc32 11 | bool success 12 | int32 r_errno 13 | -------------------------------------------------------------------------------- /mavros_msgs/srv/FileClose.srv: -------------------------------------------------------------------------------- 1 | # FTP::Close 2 | # 3 | # Call FTP::Open first. 4 | # :success: indicates success end of request 5 | # :r_errno: remote errno if applicapable 6 | 7 | string file_path 8 | --- 9 | bool success 10 | int32 r_errno 11 | -------------------------------------------------------------------------------- /mavros_msgs/srv/FileList.srv: -------------------------------------------------------------------------------- 1 | # FTP::List 2 | # 3 | # :success: indicates success end of request 4 | # :r_errno: remote errno if applicapable 5 | 6 | string dir_path 7 | --- 8 | mavros_msgs/FileEntry[] list 9 | bool success 10 | int32 r_errno 11 | -------------------------------------------------------------------------------- /mavros_msgs/srv/FileMakeDir.srv: -------------------------------------------------------------------------------- 1 | # FTP::MakeDir 2 | # 3 | # :success: indicates success end of request 4 | # :r_errno: remote errno if applicapable 5 | 6 | string dir_path 7 | --- 8 | bool success 9 | int32 r_errno 10 | -------------------------------------------------------------------------------- /mavros_msgs/srv/FileOpen.srv: -------------------------------------------------------------------------------- 1 | # FTP::Open 2 | # 3 | # :file_path: used as session id in read/write/close services 4 | # :size: file size returned for MODE_READ 5 | # :success: indicates success end of request 6 | # :r_errno: remote errno if applicapable 7 | 8 | uint8 MODE_READ = 0 # open for read 9 | uint8 MODE_WRITE = 1 # open for write 10 | uint8 MODE_CREATE = 2 # do creat() 11 | 12 | string file_path 13 | uint8 mode 14 | --- 15 | uint32 size 16 | bool success 17 | int32 r_errno 18 | -------------------------------------------------------------------------------- /mavros_msgs/srv/FileRead.srv: -------------------------------------------------------------------------------- 1 | # FTP::Read 2 | # 3 | # Call FTP::Open first. 4 | # :success: indicates success end of request 5 | # :r_errno: remote errno if applicapable 6 | 7 | string file_path 8 | uint64 offset 9 | uint64 size 10 | --- 11 | uint8[] data 12 | bool success 13 | int32 r_errno 14 | -------------------------------------------------------------------------------- /mavros_msgs/srv/FileRemove.srv: -------------------------------------------------------------------------------- 1 | # FTP::Remove 2 | # 3 | # :success: indicates success end of request 4 | # :r_errno: remote errno if applicapable 5 | 6 | string file_path 7 | --- 8 | bool success 9 | int32 r_errno 10 | -------------------------------------------------------------------------------- /mavros_msgs/srv/FileRemoveDir.srv: -------------------------------------------------------------------------------- 1 | # FTP::RemoveDir 2 | # 3 | # :success: indicates success end of request 4 | # :r_errno: remote errno if applicapable 5 | 6 | string dir_path 7 | --- 8 | bool success 9 | int32 r_errno 10 | -------------------------------------------------------------------------------- /mavros_msgs/srv/FileRename.srv: -------------------------------------------------------------------------------- 1 | # FTP::Rename 2 | # 3 | # :success: indicates success end of request 4 | # :r_errno: remote errno if applicapable 5 | 6 | string old_path 7 | string new_path 8 | --- 9 | bool success 10 | int32 r_errno 11 | -------------------------------------------------------------------------------- /mavros_msgs/srv/FileTruncate.srv: -------------------------------------------------------------------------------- 1 | # FTP::Truncate 2 | # 3 | # :success: indicates success end of request 4 | # :r_errno: remote errno if applicapable 5 | 6 | string file_path 7 | uint64 length 8 | --- 9 | bool success 10 | int32 r_errno 11 | -------------------------------------------------------------------------------- /mavros_msgs/srv/FileWrite.srv: -------------------------------------------------------------------------------- 1 | # FTP::Write 2 | # 3 | # Call FTP::Open first. 4 | # :success: indicates success end of request 5 | # :r_errno: remote errno if applicapable 6 | 7 | string file_path 8 | uint64 offset 9 | uint8[] data 10 | --- 11 | bool success 12 | int32 r_errno 13 | -------------------------------------------------------------------------------- /mavros_msgs/srv/LogRequestData.srv: -------------------------------------------------------------------------------- 1 | # Request a chunk of a log 2 | # 3 | # :id: - log id from LogEntry message 4 | # :offset: - offset into the log 5 | # :count: - number of bytes to get 6 | 7 | uint16 id 8 | uint32 offset 9 | uint32 count 10 | --- 11 | bool success 12 | -------------------------------------------------------------------------------- /mavros_msgs/srv/LogRequestEnd.srv: -------------------------------------------------------------------------------- 1 | # Stop log transfer and resume normal logging 2 | 3 | --- 4 | bool success 5 | -------------------------------------------------------------------------------- /mavros_msgs/srv/LogRequestList.srv: -------------------------------------------------------------------------------- 1 | # Request a list of available logs 2 | # 3 | # :start: - first log id (0 for first available) 4 | # :end: - last log id (0xffff for last available) 5 | 6 | uint16 start 7 | uint16 end 8 | --- 9 | bool success 10 | -------------------------------------------------------------------------------- /mavros_msgs/srv/MessageInterval.srv: -------------------------------------------------------------------------------- 1 | # sets message interval 2 | # See MAV_CMD_SET_MESSAGE_INTERVAL 3 | 4 | uint32 message_id 5 | float32 message_rate 6 | --- 7 | bool success 8 | -------------------------------------------------------------------------------- /mavros_msgs/srv/MountConfigure.srv: -------------------------------------------------------------------------------- 1 | # MAVLink message: DO_MOUNT_CONTROL 2 | # https://mavlink.io/en/messages/common.html#MAV_CMD_DO_MOUNT_CONFIGURE 3 | 4 | std_msgs/Header header 5 | 6 | uint8 mode # See enum MAV_MOUNT_MODE. 7 | #MAV_MOUNT_MODE 8 | uint8 MODE_RETRACT = 0 9 | uint8 MODE_NEUTRAL = 1 10 | uint8 MODE_MAVLINK_TARGETING = 2 11 | uint8 MODE_RC_TARGETING = 3 12 | uint8 MODE_GPS_POINT = 4 13 | 14 | bool stabilize_roll # stabilize roll? (1 = yes, 0 = no) 15 | bool stabilize_pitch # stabilize pitch? (1 = yes, 0 = no) 16 | bool stabilize_yaw # stabilize yaw? (1 = yes, 0 = no) 17 | uint8 roll_input # roll input (See enum MOUNT_INPUT) 18 | uint8 pitch_input # pitch input (See enum MOUNT_INPUT) 19 | uint8 yaw_input # yaw input (See enum MOUNT_INPUT) 20 | 21 | #MOUNT_INPUT 22 | uint8 INPUT_ANGLE_BODY_FRAME = 0 23 | uint8 INPUT_ANGULAR_RATE = 1 24 | uint8 INPUT_ANGLE_ABSOLUTE_FRAME = 2 25 | --- 26 | bool success 27 | # raw result returned by COMMAND_ACK 28 | uint8 result -------------------------------------------------------------------------------- /mavros_msgs/srv/ParamGet.srv: -------------------------------------------------------------------------------- 1 | # Request parameter from attached device 2 | 3 | string param_id 4 | --- 5 | bool success 6 | ParamValue value 7 | -------------------------------------------------------------------------------- /mavros_msgs/srv/ParamPull.srv: -------------------------------------------------------------------------------- 1 | # Request parameters from device 2 | # 3 | # Returns success status and param_recived count 4 | 5 | bool force_pull 6 | --- 7 | bool success 8 | uint32 param_received 9 | -------------------------------------------------------------------------------- /mavros_msgs/srv/ParamPush.srv: -------------------------------------------------------------------------------- 1 | # Send current send 2 | # 3 | # Returns success status and param_transfered count 4 | 5 | --- 6 | bool success 7 | uint32 param_transfered 8 | -------------------------------------------------------------------------------- /mavros_msgs/srv/ParamSet.srv: -------------------------------------------------------------------------------- 1 | # Request set parameter value 2 | 3 | string param_id 4 | mavros_msgs/ParamValue value 5 | --- 6 | bool success 7 | mavros_msgs/ParamValue value 8 | -------------------------------------------------------------------------------- /mavros_msgs/srv/SetMode.srv: -------------------------------------------------------------------------------- 1 | # set FCU mode 2 | # 3 | # Known custom modes listed here: 4 | # http://wiki.ros.org/mavros/CustomModes 5 | 6 | # basic modes from MAV_MODE 7 | uint8 MAV_MODE_PREFLIGHT = 0 8 | uint8 MAV_MODE_STABILIZE_DISARMED = 80 9 | uint8 MAV_MODE_STABILIZE_ARMED = 208 10 | uint8 MAV_MODE_MANUAL_DISARMED = 64 11 | uint8 MAV_MODE_MANUAL_ARMED = 192 12 | uint8 MAV_MODE_GUIDED_DISARMED = 88 13 | uint8 MAV_MODE_GUIDED_ARMED = 216 14 | uint8 MAV_MODE_AUTO_DISARMED = 92 15 | uint8 MAV_MODE_AUTO_ARMED = 220 16 | uint8 MAV_MODE_TEST_DISARMED = 66 17 | uint8 MAV_MODE_TEST_ARMED = 194 18 | 19 | uint8 base_mode # filled by MAV_MODE enum value or 0 if custom_mode != '' 20 | string custom_mode # string mode representation or integer 21 | --- 22 | bool mode_sent # Mode known/parsed correctly and SET_MODE are sent 23 | -------------------------------------------------------------------------------- /mavros_msgs/srv/StreamRate.srv: -------------------------------------------------------------------------------- 1 | # sets stream rate 2 | # See REQUEST_DATA_STREAM message 3 | 4 | uint8 STREAM_ALL = 0 5 | uint8 STREAM_RAW_SENSORS = 1 6 | uint8 STREAM_EXTENDED_STATUS = 2 7 | uint8 STREAM_RC_CHANNELS = 3 8 | uint8 STREAM_RAW_CONTROLLER = 4 9 | uint8 STREAM_POSITION = 6 10 | uint8 STREAM_EXTRA1 = 10 11 | uint8 STREAM_EXTRA2 = 11 12 | uint8 STREAM_EXTRA3 = 12 13 | 14 | uint8 stream_id 15 | uint16 message_rate 16 | bool on_off 17 | --- 18 | -------------------------------------------------------------------------------- /mavros_msgs/srv/VehicleInfoGet.srv: -------------------------------------------------------------------------------- 1 | # Request the Vehicle Info 2 | # use this to request the current target sysid / compid defined in mavros 3 | # set get_all = True to request all available vehicles 4 | 5 | uint8 GET_MY_SYSID = 0 6 | uint8 GET_MY_COMPID = 0 7 | 8 | uint8 sysid 9 | uint8 compid 10 | bool get_all 11 | --- 12 | bool success 13 | mavros_msgs/VehicleInfo[] vehicles 14 | 15 | -------------------------------------------------------------------------------- /mavros_msgs/srv/WaypointClear.srv: -------------------------------------------------------------------------------- 1 | # Request clear waypoint 2 | 3 | --- 4 | bool success 5 | -------------------------------------------------------------------------------- /mavros_msgs/srv/WaypointPull.srv: -------------------------------------------------------------------------------- 1 | # Requests waypoints from device 2 | # 3 | # Returns success status and received count 4 | 5 | --- 6 | bool success 7 | uint32 wp_received 8 | -------------------------------------------------------------------------------- /mavros_msgs/srv/WaypointPush.srv: -------------------------------------------------------------------------------- 1 | # Send waypoints to device 2 | # 3 | # :start_index: will define a partial waypoint update. Set to 0 for full update 4 | # 5 | # Returns success status and transfered count 6 | 7 | uint16 start_index 8 | mavros_msgs/Waypoint[] waypoints 9 | --- 10 | bool success 11 | uint32 wp_transfered 12 | -------------------------------------------------------------------------------- /mavros_msgs/srv/WaypointSetCurrent.srv: -------------------------------------------------------------------------------- 1 | # Request set current waypoint 2 | # 3 | # wp_seq - index in waypoint array 4 | 5 | uint16 wp_seq 6 | --- 7 | bool success 8 | -------------------------------------------------------------------------------- /test_mavros/README.md: -------------------------------------------------------------------------------- 1 | MAVROS test package 2 | =================== 3 | 4 | This package consists hand-tests with FCU SITL environment. 5 | I hope later we will do automatic tests too. 6 | 7 | PX4 ROS SITL 8 | ------------ 9 | Follow the instructions presented on [PX4 ROS SITL Setup][px4-sitl-wiki]. 10 | 11 | To test the simulation environment all you have to do is launch the proper ROS launch file. Right now, the current available one is `iris_empty_world_offboard_ctl.launch`, which allows to test the offboard control routines of the Firmware, together with the MAVROS API. 12 | 13 | ### Available tests 14 | 15 | #### Offboard position and velocity control 16 | Note: acceleration control still not supported on PX4 Firmware side. 17 | 18 | ##### Tested in launch files 19 | 20 | - `iris_empty_world_offboard_ctl.launch` 21 | 22 | ##### Description 23 | 24 | Allows testing the offboard control routines of the PX4 firmware by issuing setpoint commands through MAVROS plugins. Current test implements code to send: 25 | 26 | - position setpoints 27 | - velocity setpoints 28 | 29 | The tests are implemented by issuing some kind of shaped path. Current shapes are: 30 | 31 | - square/rectangle 32 | - circle 33 | - eight 34 | - ellipse (3D) 35 | 36 | ##### How to use 37 | 38 | To test the different behaviors, edit `iris_empty_world_offboard_ctl.launch`. At the bottom of this file, you will find: 39 | 40 | ```xml 41 | 42 | 43 | 44 | ``` 45 | 46 | Just change the default value of them and issue `roslaunch test_mavros iris_empty_world_offboard_ctl.launch` 47 | 48 | Or, you can just issue the roslaunch passing the parameter values on the command line, p.e. `roslaunch test_mavros iris_empty_world_offboard_ctl.launch mode:=position shape:=square`. 49 | 50 | ##### TODO 51 | 52 | - Implement acceleration setpoint sending, when this is implemented on Firmware side 53 | - Give possibility to users to define the amplitude of movement 54 | - Implement a PID controller for velocity to avoid overshoots in the onboard controller 55 | 56 | 57 | 58 | APM SITL 59 | -------- 60 | 61 | All what you need described in [ardupilot wiki][apm-sitl-wiki]. 62 | 63 | 64 | ### Preparation 65 | 66 | ```sh 67 | # this is for zsh, but bash should be similar 68 | function add-dir-to-path() { 69 | PATH+=":$1" 70 | } 71 | 72 | # get sources 73 | cd ~/ros/src 74 | wstool set imu_tools --git https://github.com/ccny-ros-pkg/imu_tools.git -v indigo 75 | wstool update -j2 76 | catkin build 77 | 78 | cd ~/src/UAV 79 | git clone https://github.com/diydrones/ardupilot.git 80 | git checkout ArduPlane-3.3.0 -b ArduPlane-3.3.0 81 | git clone https://github.com/tridge/jsbsim.git 82 | 83 | # compile JSBSim 84 | cd jsbsim 85 | ./autogen.sh --enable-libraries 86 | make -j4 87 | 88 | # also look ardupliot wiki 89 | 90 | # add path to jsbsim binaries 91 | add-dir-to-path $PWD/src 92 | 93 | # path to sim_vehicle.sh 94 | cd ../ardupilot/Tools/autotest 95 | add-dir-to-path $PWD 96 | 97 | # build APM and load default params 98 | cd ../../ArduPlane 99 | sim_vehicle.sh -w 100 | ``` 101 | 102 | 103 | ### How to use 104 | 105 | ``` 106 | # shell 1: in ArduPlane folder 107 | sim_vehicle.sh --out udp:localhost:15550 --map 108 | 109 | # shell 2: imu test 110 | roslaunch test_mavros apm_imu_test.launch 111 | 112 | # shell 2: URDF model + local_position 113 | roslaunch test_mavros apm_local_position_test.launch 114 | ``` 115 | 116 | 117 | ### Screen capture for IMU test (youtube video) 118 | 119 | [![APM SITL imu test video](http://img.youtube.com/vi/mUIptiNbmS4/0.jpg)](http://www.youtube.com/watch?v=mUIptiNbmS4) 120 | 121 | 122 | ### Screen capture for URDF model test 123 | 124 | [![APM SITL urdf test video](http://img.youtube.com/vi/r_IOGkmy5ZY/0.jpg)](https://www.youtube.com/watch?v=r_IOGkmy5ZY) 125 | 126 | 127 | [apm-sitl-wiki]: http://dev.ardupilot.com/wiki/setting-up-sitl-on-linux/ 128 | [px4-sitl-wiki]: https://pixhawk.org/dev/ros/sitl 129 | -------------------------------------------------------------------------------- /test_mavros/include/test_mavros/sitl_test/sitl_test.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief SitlTest node implementation class 3 | * @file sitl_test.h 4 | * @author Nuno Marques 5 | * 6 | * @addtogroup sitl_test 7 | * @{ 8 | * @brief SITL test node implementation 9 | */ 10 | /* 11 | * Copyright 2015 Nuno Marques. 12 | * 13 | * This file is part of the mavros package and subject to the license terms 14 | * in the top-level LICENSE file of the mavros repository. 15 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 16 | */ 17 | 18 | #pragma once 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | namespace sitltest { 25 | /** 26 | * @brief SITL test node class 27 | * 28 | * This class implements sitl_test_node 29 | */ 30 | class SitlTest 31 | { 32 | public: 33 | SitlTest(); 34 | ~SitlTest() {}; 35 | 36 | static void spin(int argc, char *argv[]); 37 | }; 38 | }; // namespace sitltest 39 | -------------------------------------------------------------------------------- /test_mavros/include/test_mavros/sitl_test/test_setup.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief SITL tests setup 3 | * @file test_setup.h 4 | * @author Nuno Marques 5 | * 6 | * @addtogroup sitl_test 7 | * @{ 8 | * @brief SITL tests setup 9 | */ 10 | /* 11 | * Copyright 2015 Nuno Marques. 12 | * 13 | * This file is part of the mavros package and subject to the license terms 14 | * in the top-level LICENSE file of the mavros repository. 15 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 16 | */ 17 | 18 | #pragma once 19 | 20 | #include 21 | 22 | namespace testsetup { 23 | class TestSetup { 24 | public: 25 | TestSetup() : 26 | nh("~") 27 | { }; 28 | ~TestSetup() {}; 29 | 30 | ros::NodeHandle nh; 31 | 32 | bool use_pid; 33 | double rate; 34 | int num_of_tests; 35 | 36 | void setup(const ros::NodeHandle &nh){ 37 | nh.param("use_pid", use_pid, true); 38 | nh.param("rate", rate, 10.0); 39 | nh.param("num_of_tests", num_of_tests, 10); 40 | } 41 | }; 42 | }; // namespace testsetup 43 | -------------------------------------------------------------------------------- /test_mavros/include/test_mavros/utils/pid_controller.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief PID controller header 3 | * @file pid_controller.h 4 | * @author Nuno Marques 5 | * 6 | * @addtogroup test_utils 7 | * @{ 8 | */ 9 | /* 10 | * Copyright 2015 Nuno Marques. 11 | * 12 | * This file is part of the mavros package and subject to the license terms 13 | * in the top-level LICENSE file of the mavros repository. 14 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | namespace pidcontroller { 24 | class PIDController 25 | { 26 | public: 27 | PIDController(); 28 | ~PIDController() {}; 29 | 30 | /* *-* PID Setup *-* */ 31 | /** 32 | * @brief Sets up the PID values for computation of the linear velocities effort 33 | */ 34 | void setup_linvel_pid(double p_gain, double i_gain, double d_gain, double i_max, double i_min, const ros::NodeHandle &node); 35 | 36 | /** 37 | * @brief Sets up the PID values for computation of the yaw rate effort 38 | */ 39 | void setup_yawrate_pid(double p_gain, double i_gain, double d_gain, double i_max, double i_min, const ros::NodeHandle &node); 40 | 41 | /* *-* Effort computation *-* */ 42 | /** 43 | * @brief Computes the linear velocity effort to apply to each axis 44 | */ 45 | Eigen::Vector3d compute_linvel_effort(Eigen::Vector3d goal, Eigen::Vector3d current, ros::Time last_time); 46 | 47 | /** 48 | * @brief Computes the yaw rate effort to apply 49 | */ 50 | double compute_yawrate_effort(double goal, double current, ros::Time last_time); 51 | 52 | private: 53 | // Control toolbox PID controllers 54 | control_toolbox::Pid pid_linvel_x; 55 | control_toolbox::Pid pid_linvel_y; 56 | control_toolbox::Pid pid_linvel_z; 57 | control_toolbox::Pid pid_yaw_rate; 58 | 59 | }; 60 | }; // namespace pidcontroller 61 | -------------------------------------------------------------------------------- /test_mavros/launch/apm/apm_imu_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /test_mavros/launch/apm/apm_local_position_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | plugin_whitelist: 19 | - 'sys_*' 20 | - command 21 | - waypoint 22 | - local_position 23 | - global_position 24 | - rc_io 25 | - imu_pub 26 | 27 | fcu_url: udp://:15550@ 28 | 29 | local_position: 30 | tf: 31 | send: true 32 | frame_id: "map" # need to check that 33 | child_frame_id: "base_link" 34 | 35 | global_position: 36 | tf: 37 | send: false 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /test_mavros/launch/apm/schematic_plane_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /test_mavros/launch/base_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /test_mavros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | test_mavros 4 | 0.33.3 5 | Tests for MAVROS package 6 | 7 | Vladimir Ermakov 8 | Nuno Marques 9 | Vladimir Ermakov 10 | 11 | BSD 12 | GPLv3 13 | LGPLv3 14 | 15 | 16 | https://github.com/mavlink/mavros.git 17 | https://github.com/mavlink/mavros/issues 18 | 19 | catkin 20 | 21 | angles 22 | cmake_modules 23 | roscpp 24 | std_msgs 25 | geometry_msgs 26 | tf2_ros 27 | mavros 28 | mavros_extras 29 | eigen 30 | eigen_conversions 31 | control_toolbox 32 | 33 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /test_mavros/sitl_test/lib/pid_controller.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief PID controller class 3 | * @file pid_controller.cpp 4 | * @author Nuno Marques 5 | */ 6 | /* 7 | * Copyright 2015 Nuno Marques. 8 | * 9 | * This file is part of the mavros package and subject to the license terms 10 | * in the top-level LICENSE file of the mavros repository. 11 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 12 | */ 13 | 14 | #include 15 | 16 | using namespace pidcontroller; 17 | 18 | PIDController::PIDController() 19 | {}; 20 | 21 | void PIDController::setup_linvel_pid(double p_gain, double i_gain, double d_gain, double i_max, double i_min, const ros::NodeHandle &node){ 22 | // PID values 23 | std::array linvel_pid = { {p_gain, i_gain, d_gain} }; 24 | 25 | // Min/max bounds for the integral windup 26 | double linvel_imin = i_min; 27 | double linvel_imax = i_max; 28 | 29 | #ifdef CONTROL_TOOLBOX_PRE_1_14 30 | pid_linvel_x.initPid(linvel_pid[0], linvel_pid[1], linvel_pid[2], linvel_imax, linvel_imin, node); 31 | pid_linvel_y.initPid(linvel_pid[0], linvel_pid[1], linvel_pid[2], linvel_imax, linvel_imin, node); 32 | pid_linvel_z.initPid(linvel_pid[0], linvel_pid[1], linvel_pid[2], linvel_imax, linvel_imin, node); 33 | #else 34 | pid_linvel_x.initPid(linvel_pid[0], linvel_pid[1], linvel_pid[2], linvel_imax, linvel_imin, false, node); 35 | pid_linvel_y.initPid(linvel_pid[0], linvel_pid[1], linvel_pid[2], linvel_imax, linvel_imin, false, node); 36 | pid_linvel_z.initPid(linvel_pid[0], linvel_pid[1], linvel_pid[2], linvel_imax, linvel_imin, false, node); 37 | #endif 38 | } 39 | 40 | void PIDController::setup_yawrate_pid(double p_gain, double i_gain, double d_gain, double i_max, double i_min, const ros::NodeHandle &node){ 41 | // PID values 42 | std::array yawrate_pid = { {p_gain, i_gain, d_gain} }; 43 | 44 | // Min/max bounds for the integral windup 45 | double yawrate_imin = i_min; 46 | double yawrate_imax = i_max; 47 | 48 | #ifdef CONTROL_TOOLBOX_PRE_1_14 49 | pid_yaw_rate.initPid(yawrate_pid[0], yawrate_pid[1], yawrate_pid[2], yawrate_imax, yawrate_imin, node); 50 | #else 51 | pid_yaw_rate.initPid(yawrate_pid[0], yawrate_pid[1], yawrate_pid[2], yawrate_imax, yawrate_imin, false, node); 52 | #endif 53 | } 54 | 55 | Eigen::Vector3d PIDController::compute_linvel_effort(Eigen::Vector3d goal, Eigen::Vector3d current, ros::Time last_time){ 56 | double lin_vel_x = pid_linvel_x.computeCommand(goal.x() - current.x(), ros::Time::now() - last_time); 57 | double lin_vel_y = pid_linvel_y.computeCommand(goal.y() - current.y(), ros::Time::now() - last_time); 58 | double lin_vel_z = pid_linvel_z.computeCommand(goal.z() - current.z(), ros::Time::now() - last_time); 59 | 60 | return Eigen::Vector3d(lin_vel_x, lin_vel_y, lin_vel_z); 61 | } 62 | 63 | double PIDController::compute_yawrate_effort(double goal, double current, ros::Time last_time){ 64 | double yaw_rate = pid_yaw_rate.computeCommand(goal - current, ros::Time::now() - last_time); 65 | 66 | return yaw_rate; 67 | } 68 | -------------------------------------------------------------------------------- /test_mavros/sitl_test/lib/sitl_test.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief SITL test class 3 | * @file sitl_test.cpp 4 | * @author Nuno Marques 5 | */ 6 | /* 7 | * Copyright 2015 Nuno Marques. 8 | * 9 | * This file is part of the mavros package and subject to the license terms 10 | * in the top-level LICENSE file of the mavros repository. 11 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 12 | */ 13 | 14 | #include 15 | #include 16 | 17 | using namespace sitltest; 18 | using namespace testsetup; 19 | 20 | SitlTest::SitlTest() 21 | {} 22 | 23 | void SitlTest::spin(int argc, char *argv[]) 24 | { 25 | if (strcmp(argv[1],"offboard_control") == 0) 26 | { 27 | ros::init(argc, argv, "offboard_control"); 28 | testsetup::OffboardControl offboard_control; 29 | offboard_control.spin(argc, argv); 30 | } 31 | 32 | /** @todo add more testing structures */ 33 | } 34 | -------------------------------------------------------------------------------- /test_mavros/sitl_test/sitl_test_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief SITL test node 3 | * @file sitl_test_node.cpp 4 | * @author Nuno Marques 5 | */ 6 | /* 7 | * Copyright 2015 Nuno Marques. 8 | * 9 | * This file is part of the mavros package and subject to the license terms 10 | * in the top-level LICENSE file of the mavros repository. 11 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 12 | */ 13 | 14 | #include 15 | 16 | using namespace sitltest; 17 | 18 | int main(int argc, char *argv[]) 19 | { 20 | ROS_INFO("SITL Test node started..."); 21 | 22 | SitlTest::spin(argc, argv); 23 | 24 | return 0; 25 | } 26 | --------------------------------------------------------------------------------