├── mavlink ├── share │ └── pyshared │ │ └── pymavlink │ │ ├── generator │ │ ├── .gitignore │ │ ├── C │ │ │ ├── test │ │ │ │ ├── posix │ │ │ │ │ ├── .gitignore │ │ │ │ │ └── testmav.c │ │ │ │ └── windows │ │ │ │ │ ├── stdafx.cpp │ │ │ │ │ ├── targetver.h │ │ │ │ │ ├── stdafx.h │ │ │ │ │ └── testmav.cpp │ │ │ ├── include_v0.9 │ │ │ │ ├── test │ │ │ │ │ ├── version.h │ │ │ │ │ ├── mavlink.h │ │ │ │ │ ├── test.h │ │ │ │ │ └── testsuite.h │ │ │ │ └── checksum.h │ │ │ └── include_v1.0 │ │ │ │ ├── test │ │ │ │ ├── version.h │ │ │ │ ├── mavlink.h │ │ │ │ ├── test.h │ │ │ │ └── testsuite.h │ │ │ │ ├── checksum.h │ │ │ │ └── mavlink_types.h │ │ ├── gen_all.sh │ │ ├── gen_all.py │ │ ├── mavgen.py │ │ ├── gen_MatrixPilot.py │ │ ├── mavtestgen.py │ │ └── mavtemplate.py │ │ ├── tools │ │ └── images │ │ │ ├── gtk-quit.gif │ │ │ ├── player_end.gif │ │ │ ├── media-playback-stop.gif │ │ │ ├── media-seek-backward.gif │ │ │ ├── media-seek-forward.gif │ │ │ ├── media-playback-pause.gif │ │ │ └── media-playback-start.gif │ │ ├── README.txt │ │ ├── .gitignore │ │ ├── examples │ │ ├── mavtest.py │ │ ├── mavparms.py │ │ ├── mavtester.py │ │ ├── flightmodes.py │ │ ├── flighttime.py │ │ ├── wptogpx.py │ │ ├── bwtest.py │ │ ├── sigloss.py │ │ ├── gpslock.py │ │ ├── apmsetrate.py │ │ ├── mavtogpx.py │ │ ├── mavlogdump.py │ │ ├── magtest.py │ │ ├── magfit.py │ │ ├── magfit_gps.py │ │ ├── magfit_delta.py │ │ └── mavgraph.py │ │ ├── APM_Mavtest │ │ └── APM_Mavtest.pde │ │ └── mavextra.py ├── include │ └── mavlink │ │ ├── config.h │ │ └── v1.0 │ │ ├── common │ │ ├── version.h │ │ ├── mavlink.h │ │ ├── mavlink_msg_auth_key.h │ │ └── mavlink_msg_mission_current.h │ │ ├── autoquad │ │ ├── version.h │ │ ├── mavlink.h │ │ └── testsuite.h │ │ ├── pixhawk │ │ ├── version.h │ │ └── mavlink.h │ │ ├── matrixpilot │ │ ├── version.h │ │ └── mavlink.h │ │ ├── sensesoar │ │ ├── version.h │ │ ├── mavlink.h │ │ ├── mavlink_msg_obs_qff.h │ │ ├── mavlink_msg_obs_wind.h │ │ ├── mavlink_msg_obs_air_temp.h │ │ └── mavlink_msg_obs_velocity.h │ │ ├── ardupilotmega │ │ ├── version.h │ │ └── mavlink.h │ │ ├── checksum.h │ │ ├── mavlink_conversions.h │ │ └── mavlink_types.h └── lib │ └── pkgconfig │ └── mavlink.pc ├── drivers ├── px4flow │ ├── launch │ │ ├── px4flow_parameters.yaml │ │ └── px4flow.launch │ ├── package.xml │ ├── CMakeLists.txt │ ├── src │ │ └── px4flow_node.cc │ └── include │ │ └── px4flow │ │ └── SerialComm.h └── mavlink_serial_client │ ├── launch │ ├── mavlink_serial_client_parameters.yaml │ └── mavlink_serial_client.launch │ ├── CMakeLists.txt │ ├── package.xml │ ├── src │ └── mavlink_serial_client.cc │ └── include │ └── mavlink_serial_client │ └── SerialComm.h ├── px_comm ├── msg │ ├── Mavlink.msg │ ├── OpticalFlow.msg │ └── CameraInfo.msg ├── include │ └── px_comm │ │ ├── camera_models.h │ │ ├── DDSImageSupport.h │ │ └── DDSImage.h ├── dds_msg │ ├── DDSImage.idl │ └── DDSImageSupport.cxx ├── package.xml ├── srv │ └── SetCameraInfo.srv └── CMakeLists.txt ├── .gitignore └── README.md /mavlink/share/pyshared/pymavlink/generator/.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | -------------------------------------------------------------------------------- /mavlink/include/mavlink/config.h: -------------------------------------------------------------------------------- 1 | #define MAVLINK_VERSION "1.0.7" 2 | -------------------------------------------------------------------------------- /drivers/px4flow/launch/px4flow_parameters.yaml: -------------------------------------------------------------------------------- 1 | serial_port: /dev/ttyS0 2 | baudrate: 115200 3 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/generator/C/test/posix/.gitignore: -------------------------------------------------------------------------------- 1 | /testmav0.9 2 | /testmav1.0 3 | /testmav1.0_nonstrict 4 | -------------------------------------------------------------------------------- /drivers/mavlink_serial_client/launch/mavlink_serial_client_parameters.yaml: -------------------------------------------------------------------------------- 1 | serial_port: /dev/ttyS0 2 | baudrate: 115200 3 | frame_id: fcu 4 | -------------------------------------------------------------------------------- /mavlink/lib/pkgconfig/mavlink.pc: -------------------------------------------------------------------------------- 1 | prefix=/ 2 | exec_prefix=/ 3 | 4 | Name: mavlink 5 | Description: 6 | Version: 7 | Cflags: -I//include/mavlink 8 | -------------------------------------------------------------------------------- /px_comm/msg/Mavlink.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint8 len 3 | uint8 seq 4 | uint8 sysid 5 | uint8 compid 6 | uint8 msgid 7 | bool fromlcm 8 | uint64[] payload64 9 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/tools/images/gtk-quit.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cvg/px-ros-pkg/HEAD/mavlink/share/pyshared/pymavlink/tools/images/gtk-quit.gif -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/tools/images/player_end.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cvg/px-ros-pkg/HEAD/mavlink/share/pyshared/pymavlink/tools/images/player_end.gif -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/tools/images/media-playback-stop.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cvg/px-ros-pkg/HEAD/mavlink/share/pyshared/pymavlink/tools/images/media-playback-stop.gif -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/tools/images/media-seek-backward.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cvg/px-ros-pkg/HEAD/mavlink/share/pyshared/pymavlink/tools/images/media-seek-backward.gif -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/tools/images/media-seek-forward.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cvg/px-ros-pkg/HEAD/mavlink/share/pyshared/pymavlink/tools/images/media-seek-forward.gif -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | 6 | # Compiled Dynamic libraries 7 | *.so 8 | 9 | # Compiled Static libraries 10 | *.lai 11 | *.la 12 | *.a 13 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/tools/images/media-playback-pause.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cvg/px-ros-pkg/HEAD/mavlink/share/pyshared/pymavlink/tools/images/media-playback-pause.gif -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/tools/images/media-playback-start.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cvg/px-ros-pkg/HEAD/mavlink/share/pyshared/pymavlink/tools/images/media-playback-start.gif -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/README.txt: -------------------------------------------------------------------------------- 1 | This is a python implementation of the MAVLink protocol. 2 | 3 | Please see http://www.qgroundcontrol.org/mavlink/pymavlink for 4 | documentation 5 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | px-ros-pkg 2 | ---------- 3 | 4 | A repository for PIXHAWK open source code running on ROS. 5 | 6 | Notes: 7 | 8 | 1. The master branch uses catkin. If you wish to use rosbuild, please use the rosbuild branch. 9 | -------------------------------------------------------------------------------- /drivers/px4flow/launch/px4flow.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/.gitignore: -------------------------------------------------------------------------------- 1 | apidocs/ 2 | *.zip 3 | *.pyc 4 | send.sh 5 | generator/C/include/ardupilotmega 6 | generator/C/include/common 7 | generator/C/include/pixhawk 8 | generator/C/include/minimal 9 | generator/C/include/ualberta 10 | generator/C/include/slugs 11 | testmav0.9* 12 | testmav1.0* 13 | Debug/ 14 | -------------------------------------------------------------------------------- /drivers/mavlink_serial_client/launch/mavlink_serial_client.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/generator/C/test/windows/stdafx.cpp: -------------------------------------------------------------------------------- 1 | // stdafx.cpp : source file that includes just the standard includes 2 | // testmav.pch will be the pre-compiled header 3 | // stdafx.obj will contain the pre-compiled type information 4 | 5 | #include "stdafx.h" 6 | 7 | // TODO: reference any additional headers you need in STDAFX.H 8 | // and not in this file 9 | -------------------------------------------------------------------------------- /px_comm/include/px_comm/camera_models.h: -------------------------------------------------------------------------------- 1 | #ifndef CAMERAMODELS_H 2 | #define CAMERAMODELS_H 3 | 4 | #include 5 | 6 | namespace px 7 | { 8 | namespace camera_models 9 | { 10 | const std::string KANNALA_BRANDT = "KANNALA_BRANDT"; 11 | const std::string MEI = "MEI"; 12 | const std::string PINHOLE = "PINHOLE"; 13 | } 14 | } 15 | 16 | #endif 17 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/generator/C/test/windows/targetver.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Including SDKDDKVer.h defines the highest available Windows platform. 4 | 5 | // If you wish to build your application for a previous Windows platform, include WinSDKVer.h and 6 | // set the _WIN32_WINNT macro to the platform you wish to support before including SDKDDKVer.h. 7 | 8 | #include 9 | -------------------------------------------------------------------------------- /px_comm/dds_msg/DDSImage.idl: -------------------------------------------------------------------------------- 1 | // mirrors sensor_msgs/Image in ROS 2 | 3 | module px_comm 4 | { 5 | 6 | struct DDSImage { 7 | unsigned long seq; 8 | long stamp_sec; 9 | long stamp_nsec; 10 | string frame_id; 11 | unsigned long height; 12 | unsigned long width; 13 | string encoding; 14 | char is_bigendian; 15 | unsigned long step; 16 | sequence data; 17 | }; 18 | 19 | }; 20 | -------------------------------------------------------------------------------- /mavlink/include/mavlink/v1.0/common/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from common.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_VERSION_H 6 | #define MAVLINK_VERSION_H 7 | 8 | #define MAVLINK_BUILD_DATE "Tue Feb 4 15:28:29 2014" 9 | #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" 10 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 11 | 12 | #endif // MAVLINK_VERSION_H 13 | -------------------------------------------------------------------------------- /mavlink/include/mavlink/v1.0/autoquad/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from autoquad.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_VERSION_H 6 | #define MAVLINK_VERSION_H 7 | 8 | #define MAVLINK_BUILD_DATE "Tue Feb 4 15:27:43 2014" 9 | #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" 10 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 11 | 12 | #endif // MAVLINK_VERSION_H 13 | -------------------------------------------------------------------------------- /mavlink/include/mavlink/v1.0/pixhawk/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from pixhawk.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_VERSION_H 6 | #define MAVLINK_VERSION_H 7 | 8 | #define MAVLINK_BUILD_DATE "Tue Feb 4 15:28:16 2014" 9 | #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" 10 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 11 | 12 | #endif // MAVLINK_VERSION_H 13 | -------------------------------------------------------------------------------- /mavlink/include/mavlink/v1.0/matrixpilot/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from matrixpilot.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_VERSION_H 6 | #define MAVLINK_VERSION_H 7 | 8 | #define MAVLINK_BUILD_DATE "Tue Feb 4 15:28:04 2014" 9 | #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" 10 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 11 | 12 | #endif // MAVLINK_VERSION_H 13 | -------------------------------------------------------------------------------- /mavlink/include/mavlink/v1.0/sensesoar/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from sensesoar.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_VERSION_H 6 | #define MAVLINK_VERSION_H 7 | 8 | #define MAVLINK_BUILD_DATE "Tue Feb 4 15:28:27 2014" 9 | #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" 10 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 11 | 12 | #endif // MAVLINK_VERSION_H 13 | -------------------------------------------------------------------------------- /px_comm/msg/OpticalFlow.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | float32 ground_distance # distance to ground in meters 4 | int16 flow_x # x-component of optical flow in pixels 5 | int16 flow_y # y-component of optical flow in pixels 6 | float32 velocity_x # x-component of scaled optical flow in m/s 7 | float32 velocity_y # y-component of scaled optical flow in m/s 8 | uint8 quality # quality of optical flow estimate 9 | -------------------------------------------------------------------------------- /mavlink/include/mavlink/v1.0/ardupilotmega/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from ardupilotmega.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_VERSION_H 6 | #define MAVLINK_VERSION_H 7 | 8 | #define MAVLINK_BUILD_DATE "Tue Feb 4 15:27:33 2014" 9 | #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" 10 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 11 | 12 | #endif // MAVLINK_VERSION_H 13 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/generator/C/test/windows/stdafx.h: -------------------------------------------------------------------------------- 1 | // stdafx.h : include file for standard system include files, 2 | // or project specific include files that are used frequently, but 3 | // are changed infrequently 4 | // 5 | 6 | #pragma once 7 | 8 | #include "targetver.h" 9 | 10 | #include 11 | #include 12 | 13 | 14 | 15 | // TODO: reference additional headers your program requires here 16 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from test.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_VERSION_H 6 | #define MAVLINK_VERSION_H 7 | 8 | #define MAVLINK_BUILD_DATE "Thu Mar 1 15:11:54 2012" 9 | #define MAVLINK_WIRE_PROTOCOL_VERSION "0.9" 10 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 179 11 | 12 | #endif // MAVLINK_VERSION_H 13 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from test.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_VERSION_H 6 | #define MAVLINK_VERSION_H 7 | 8 | #define MAVLINK_BUILD_DATE "Thu Mar 1 15:11:58 2012" 9 | #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" 10 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 179 11 | 12 | #endif // MAVLINK_VERSION_H 13 | -------------------------------------------------------------------------------- /px_comm/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | px_comm 4 | 0.1.0 5 | The px_comm package 6 | 7 | Lionel Heng 8 | BSD 9 | http://ros.org/wiki/px_comm 10 | 11 | catkin 12 | 13 | message_generation 14 | 15 | message_runtime 16 | 17 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/generator/gen_all.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | for protocol in 0.9 1.0; do 4 | for xml in message_definitions/v$protocol/*.xml; do 5 | base=$(basename $xml .xml) 6 | ./mavgen.py --lang=C --wire-protocol=$protocol --output=C/include_v$protocol $xml || exit 1 7 | ./mavgen.py --lang=python --wire-protocol=$protocol --output=python/mavlink_${base}_v$protocol.py $xml || exit 1 8 | done 9 | done 10 | 11 | cp -f python/mavlink_ardupilotmega_v0.9.py ../mavlink.py 12 | cp -f python/mavlink_ardupilotmega_v1.0.py ../mavlinkv10.py 13 | -------------------------------------------------------------------------------- /px_comm/srv/SetCameraInfo.srv: -------------------------------------------------------------------------------- 1 | # This service requests that a camera stores the given CameraInfo 2 | # as that camera's calibration information. 3 | # 4 | # The width and height in the camera_info field should match what the 5 | # camera is currently outputting on its camera_info topic, and the camera 6 | # will assume that the region of the imager that is being referred to is 7 | # the region that the camera is currently capturing. 8 | 9 | CameraInfo camera_info # The camera_info to store 10 | --- 11 | bool success # True if the call succeeded 12 | string status_message # Used to give details about success -------------------------------------------------------------------------------- /mavlink/include/mavlink/v1.0/common/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from common.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_H 6 | #define MAVLINK_H 7 | 8 | #ifndef MAVLINK_STX 9 | #define MAVLINK_STX 254 10 | #endif 11 | 12 | #ifndef MAVLINK_ENDIAN 13 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 14 | #endif 15 | 16 | #ifndef MAVLINK_ALIGNED_FIELDS 17 | #define MAVLINK_ALIGNED_FIELDS 1 18 | #endif 19 | 20 | #ifndef MAVLINK_CRC_EXTRA 21 | #define MAVLINK_CRC_EXTRA 1 22 | #endif 23 | 24 | #include "version.h" 25 | #include "common.h" 26 | 27 | #endif // MAVLINK_H 28 | -------------------------------------------------------------------------------- /mavlink/include/mavlink/v1.0/pixhawk/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from pixhawk.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_H 6 | #define MAVLINK_H 7 | 8 | #ifndef MAVLINK_STX 9 | #define MAVLINK_STX 254 10 | #endif 11 | 12 | #ifndef MAVLINK_ENDIAN 13 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 14 | #endif 15 | 16 | #ifndef MAVLINK_ALIGNED_FIELDS 17 | #define MAVLINK_ALIGNED_FIELDS 1 18 | #endif 19 | 20 | #ifndef MAVLINK_CRC_EXTRA 21 | #define MAVLINK_CRC_EXTRA 1 22 | #endif 23 | 24 | #include "version.h" 25 | #include "pixhawk.h" 26 | 27 | #endif // MAVLINK_H 28 | -------------------------------------------------------------------------------- /mavlink/include/mavlink/v1.0/autoquad/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from autoquad.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_H 6 | #define MAVLINK_H 7 | 8 | #ifndef MAVLINK_STX 9 | #define MAVLINK_STX 254 10 | #endif 11 | 12 | #ifndef MAVLINK_ENDIAN 13 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 14 | #endif 15 | 16 | #ifndef MAVLINK_ALIGNED_FIELDS 17 | #define MAVLINK_ALIGNED_FIELDS 1 18 | #endif 19 | 20 | #ifndef MAVLINK_CRC_EXTRA 21 | #define MAVLINK_CRC_EXTRA 1 22 | #endif 23 | 24 | #include "version.h" 25 | #include "autoquad.h" 26 | 27 | #endif // MAVLINK_H 28 | -------------------------------------------------------------------------------- /mavlink/include/mavlink/v1.0/sensesoar/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from sensesoar.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_H 6 | #define MAVLINK_H 7 | 8 | #ifndef MAVLINK_STX 9 | #define MAVLINK_STX 254 10 | #endif 11 | 12 | #ifndef MAVLINK_ENDIAN 13 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 14 | #endif 15 | 16 | #ifndef MAVLINK_ALIGNED_FIELDS 17 | #define MAVLINK_ALIGNED_FIELDS 1 18 | #endif 19 | 20 | #ifndef MAVLINK_CRC_EXTRA 21 | #define MAVLINK_CRC_EXTRA 1 22 | #endif 23 | 24 | #include "version.h" 25 | #include "sensesoar.h" 26 | 27 | #endif // MAVLINK_H 28 | -------------------------------------------------------------------------------- /mavlink/include/mavlink/v1.0/matrixpilot/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from matrixpilot.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_H 6 | #define MAVLINK_H 7 | 8 | #ifndef MAVLINK_STX 9 | #define MAVLINK_STX 254 10 | #endif 11 | 12 | #ifndef MAVLINK_ENDIAN 13 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 14 | #endif 15 | 16 | #ifndef MAVLINK_ALIGNED_FIELDS 17 | #define MAVLINK_ALIGNED_FIELDS 1 18 | #endif 19 | 20 | #ifndef MAVLINK_CRC_EXTRA 21 | #define MAVLINK_CRC_EXTRA 1 22 | #endif 23 | 24 | #include "version.h" 25 | #include "matrixpilot.h" 26 | 27 | #endif // MAVLINK_H 28 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from test.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_H 6 | #define MAVLINK_H 7 | 8 | #ifndef MAVLINK_STX 9 | #define MAVLINK_STX 85 10 | #endif 11 | 12 | #ifndef MAVLINK_ENDIAN 13 | #define MAVLINK_ENDIAN MAVLINK_BIG_ENDIAN 14 | #endif 15 | 16 | #ifndef MAVLINK_ALIGNED_FIELDS 17 | #define MAVLINK_ALIGNED_FIELDS 0 18 | #endif 19 | 20 | #ifndef MAVLINK_CRC_EXTRA 21 | #define MAVLINK_CRC_EXTRA 0 22 | #endif 23 | 24 | #include "version.h" 25 | #include "test.h" 26 | 27 | #endif // MAVLINK_H 28 | -------------------------------------------------------------------------------- /mavlink/include/mavlink/v1.0/ardupilotmega/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from ardupilotmega.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_H 6 | #define MAVLINK_H 7 | 8 | #ifndef MAVLINK_STX 9 | #define MAVLINK_STX 254 10 | #endif 11 | 12 | #ifndef MAVLINK_ENDIAN 13 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 14 | #endif 15 | 16 | #ifndef MAVLINK_ALIGNED_FIELDS 17 | #define MAVLINK_ALIGNED_FIELDS 1 18 | #endif 19 | 20 | #ifndef MAVLINK_CRC_EXTRA 21 | #define MAVLINK_CRC_EXTRA 1 22 | #endif 23 | 24 | #include "version.h" 25 | #include "ardupilotmega.h" 26 | 27 | #endif // MAVLINK_H 28 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from test.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_H 6 | #define MAVLINK_H 7 | 8 | #ifndef MAVLINK_STX 9 | #define MAVLINK_STX 254 10 | #endif 11 | 12 | #ifndef MAVLINK_ENDIAN 13 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 14 | #endif 15 | 16 | #ifndef MAVLINK_ALIGNED_FIELDS 17 | #define MAVLINK_ALIGNED_FIELDS 1 18 | #endif 19 | 20 | #ifndef MAVLINK_CRC_EXTRA 21 | #define MAVLINK_CRC_EXTRA 1 22 | #endif 23 | 24 | #include "version.h" 25 | #include "test.h" 26 | 27 | #endif // MAVLINK_H 28 | -------------------------------------------------------------------------------- /drivers/px4flow/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | px4flow 4 | 0.1.0 5 | The px4flow package 6 | 7 | Lionel Heng 8 | BSD 9 | http://ros.org/wiki/px4flow 10 | 11 | catkin 12 | 13 | roscpp 14 | std_msgs 15 | sensor_msgs 16 | image_transport 17 | px_comm 18 | 19 | roscpp 20 | std_msgs 21 | sensor_msgs 22 | image_transport 23 | px_comm 24 | 25 | -------------------------------------------------------------------------------- /drivers/mavlink_serial_client/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mavlink_serial_client) 3 | 4 | find_package(catkin REQUIRED COMPONENTS cmake_modules roscpp std_msgs sensor_msgs image_transport px_comm) 5 | find_package(Boost REQUIRED COMPONENTS thread system signals) 6 | find_package(Eigen REQUIRED) 7 | 8 | catkin_package( 9 | INCLUDE_DIRS include 10 | CATKIN_DEPENDS roscpp std_msgs sensor_msgs image_transport px_comm 11 | DEPENDS boost eigen 12 | ) 13 | 14 | ########### 15 | ## Build ## 16 | ########### 17 | 18 | include_directories(include 19 | ../../mavlink/include/mavlink/v1.0 20 | ${catkin_INCLUDE_DIRS} 21 | ${Eigen_INCLUDE_DIRS} 22 | ) 23 | 24 | add_executable(mavlink_serial_client 25 | src/SerialComm.cc 26 | src/mavlink_serial_client.cc 27 | ) 28 | 29 | target_link_libraries(mavlink_serial_client 30 | ${catkin_LIBRARIES} 31 | ) 32 | 33 | add_dependencies(mavlink_serial_client px_comm_gencpp) 34 | -------------------------------------------------------------------------------- /drivers/mavlink_serial_client/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mavlink_serial_client 4 | 0.1.0 5 | The mavlink_serial_client package 6 | 7 | Lionel Heng 8 | BSD 9 | http://ros.org/wiki/mavlink_serial_client 10 | 11 | catkin 12 | 13 | cmake_modules 14 | roscpp 15 | std_msgs 16 | sensor_msgs 17 | image_transport 18 | px_comm 19 | 20 | roscpp 21 | std_msgs 22 | sensor_msgs 23 | image_transport 24 | px_comm 25 | 26 | -------------------------------------------------------------------------------- /drivers/px4flow/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(px4flow) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs image_transport px_comm) 5 | find_package(Boost REQUIRED COMPONENTS thread system signals) 6 | 7 | catkin_package( 8 | INCLUDE_DIRS include 9 | CATKIN_DEPENDS roscpp std_msgs sensor_msgs image_transport px_comm 10 | DEPENDS boost 11 | ) 12 | 13 | ########### 14 | ## Build ## 15 | ########### 16 | 17 | include_directories(include 18 | ../../mavlink/include/mavlink/v1.0 19 | ${catkin_INCLUDE_DIRS} 20 | ) 21 | 22 | add_executable(px4flow_node 23 | src/SerialComm.cc 24 | src/px4flow_node.cc 25 | ) 26 | 27 | target_link_libraries(px4flow_node 28 | ${catkin_LIBRARIES} 29 | ) 30 | 31 | add_dependencies(px4flow_node px_comm_gencpp) 32 | 33 | ############# 34 | ## Install ## 35 | ############# 36 | 37 | install(TARGETS px4flow_node 38 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 39 | ) 40 | 41 | install(DIRECTORY launch/ 42 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 43 | PATTERN ".svn" EXCLUDE 44 | ) 45 | -------------------------------------------------------------------------------- /px_comm/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(px_comm) 3 | 4 | find_package(catkin REQUIRED COMPONENTS geometry_msgs std_msgs message_generation) 5 | 6 | add_message_files( 7 | DIRECTORY msg 8 | FILES CameraInfo.msg Mavlink.msg OpticalFlow.msg 9 | ) 10 | 11 | add_service_files( 12 | DIRECTORY srv 13 | FILES SetCameraInfo.srv 14 | ) 15 | 16 | generate_messages( 17 | DEPENDENCIES 18 | geometry_msgs 19 | std_msgs 20 | ) 21 | 22 | # if RTI DDS is found 23 | if(DEFINED ENV{NDDSHOME}) 24 | catkin_package( 25 | INCLUDE_DIRS include 26 | LIBRARIES px_comm_dds 27 | CATKIN_DEPENDS message_runtime 28 | ) 29 | 30 | set(NDDSHOME $ENV{NDDSHOME}) 31 | 32 | include_directories( 33 | include 34 | include/px_comm 35 | ${NDDSHOME}/include 36 | ${NDDSHOME}/include/ndds 37 | ) 38 | 39 | add_definitions(-DRTI_UNIX) 40 | 41 | add_library(px_comm_dds 42 | dds_msg/DDSImage.cxx 43 | dds_msg/DDSImagePlugin.cxx 44 | dds_msg/DDSImageSupport.cxx 45 | ) 46 | else() 47 | catkin_package( 48 | INCLUDE_DIRS include 49 | CATKIN_DEPENDS message_runtime 50 | ) 51 | endif() 52 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/examples/mavtest.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys, os 4 | 5 | # allow import from the parent directory, where mavlink.py is 6 | sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) 7 | 8 | import mavlink 9 | 10 | class fifo(object): 11 | def __init__(self): 12 | self.buf = [] 13 | def write(self, data): 14 | self.buf += data 15 | return len(data) 16 | def read(self): 17 | return self.buf.pop(0) 18 | 19 | f = fifo() 20 | 21 | # create a mavlink instance, which will do IO on file object 'f' 22 | mav = mavlink.MAVLink(f) 23 | 24 | # set the WP_RADIUS parameter on the MAV at the end of the link 25 | mav.param_set_send(7, 1, "WP_RADIUS", 101) 26 | 27 | # alternatively, produce a MAVLink_param_set object 28 | # this can be sent via your own transport if you like 29 | m = mav.param_set_encode(7, 1, "WP_RADIUS", 101) 30 | 31 | # get the encoded message as a buffer 32 | b = m.get_msgbuf() 33 | 34 | # decode an incoming message 35 | m2 = mav.decode(b) 36 | 37 | # show what fields it has 38 | print("Got a message with id %u and fields %s" % (m2.get_msgId(), m2.get_fieldnames())) 39 | 40 | # print out the fields 41 | print(m2) 42 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/examples/mavparms.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | extract mavlink parameter values 5 | ''' 6 | 7 | import sys, time, os 8 | 9 | # allow import from the parent directory, where mavlink.py is 10 | sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) 11 | 12 | from optparse import OptionParser 13 | parser = OptionParser("mavparms.py [options]") 14 | parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") 15 | 16 | (opts, args) = parser.parse_args() 17 | 18 | if opts.mav10: 19 | os.environ['MAVLINK10'] = '1' 20 | import mavutil 21 | 22 | if len(args) < 1: 23 | print("Usage: mavparms.py [options] ") 24 | sys.exit(1) 25 | 26 | parms = {} 27 | 28 | def mavparms(logfile): 29 | '''extract mavlink parameters''' 30 | mlog = mavutil.mavlink_connection(filename) 31 | 32 | while True: 33 | m = mlog.recv_match(type='PARAM_VALUE') 34 | if m is None: 35 | return 36 | pname = str(m.param_id).strip() 37 | if len(pname) > 0: 38 | parms[pname] = m.param_value 39 | 40 | total = 0.0 41 | for filename in args: 42 | mavparms(filename) 43 | 44 | keys = parms.keys() 45 | keys.sort() 46 | for p in keys: 47 | print("%-15s %.6f" % (p, parms[p])) 48 | 49 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/APM_Mavtest/APM_Mavtest.pde: -------------------------------------------------------------------------------- 1 | /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- 2 | 3 | /* 4 | send all possible mavlink messages 5 | Andrew Tridgell July 2011 6 | */ 7 | 8 | // AVR runtime 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | // Libraries 15 | #include 16 | #include 17 | #include 18 | #include "mavtest.h" 19 | 20 | FastSerialPort0(Serial); // FTDI/console 21 | FastSerialPort1(Serial1); // GPS port 22 | FastSerialPort3(Serial3); // Telemetry port 23 | 24 | #define SERIAL0_BAUD 115200 25 | #define SERIAL3_BAUD 115200 26 | 27 | void setup() { 28 | Serial.begin(SERIAL0_BAUD, 128, 128); 29 | Serial3.begin(SERIAL3_BAUD, 128, 128); 30 | mavlink_comm_0_port = &Serial; 31 | mavlink_comm_1_port = &Serial3; 32 | } 33 | 34 | 35 | 36 | void loop() 37 | { 38 | Serial.println("Starting MAVLink test generator\n"); 39 | while (1) { 40 | mavlink_msg_heartbeat_send( 41 | MAVLINK_COMM_0, 42 | mavlink_system.type, 43 | MAV_AUTOPILOT_ARDUPILOTMEGA); 44 | 45 | mavlink_msg_heartbeat_send( 46 | MAVLINK_COMM_1, 47 | mavlink_system.type, 48 | MAV_AUTOPILOT_ARDUPILOTMEGA); 49 | 50 | mavtest_generate_outputs(MAVLINK_COMM_0); 51 | mavtest_generate_outputs(MAVLINK_COMM_1); 52 | delay(500); 53 | } 54 | } 55 | 56 | -------------------------------------------------------------------------------- /drivers/px4flow/src/px4flow_node.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "px4flow/SerialComm.h" 4 | 5 | void 6 | printTopicInfo(void) 7 | { 8 | // print published/subscribed topics 9 | std::string nodeName = ros::this_node::getName(); 10 | 11 | ros::V_string topics; 12 | ros::this_node::getSubscribedTopics(topics); 13 | 14 | std::string topicsStr = nodeName + ":\n\tsubscribed to topics:\n"; 15 | for (unsigned int i = 0; i < topics.size(); ++i) 16 | { 17 | topicsStr += ("\t\t" + topics.at(i) + "\n"); 18 | } 19 | 20 | topicsStr += "\tadvertised topics:\n"; 21 | ros::this_node::getAdvertisedTopics(topics); 22 | for (unsigned int i = 0; i < topics.size(); ++i) 23 | { 24 | topicsStr += ("\t\t" + topics.at(i) + "\n"); 25 | } 26 | 27 | ROS_INFO("%s", topicsStr.c_str()); 28 | } 29 | 30 | int 31 | main(int argc, char** argv) 32 | { 33 | ros::init(argc, argv, "px4flow"); 34 | ros::NodeHandle nh("px4flow"); 35 | 36 | ros::NodeHandle pnh("~"); 37 | 38 | std::string portStr; 39 | pnh.param("serial_port", portStr, std::string("/dev/ttyUSB0")); 40 | 41 | int baudrate; 42 | pnh.param("baudrate", baudrate, 115200); 43 | 44 | std::string frameId; 45 | pnh.param("frame_id", frameId, std::string("/px4flow")); 46 | 47 | px::SerialComm comm(frameId); 48 | if (!comm.open(portStr, baudrate)) 49 | { 50 | return -1; 51 | } 52 | 53 | printTopicInfo(); 54 | 55 | ros::spin(); 56 | 57 | return 0; 58 | } 59 | 60 | -------------------------------------------------------------------------------- /drivers/mavlink_serial_client/src/mavlink_serial_client.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "mavlink_serial_client/SerialComm.h" 4 | 5 | void 6 | printTopicInfo(void) 7 | { 8 | // print published/subscribed topics 9 | std::string nodeName = ros::this_node::getName(); 10 | 11 | ros::V_string topics; 12 | ros::this_node::getSubscribedTopics(topics); 13 | 14 | std::string topicsStr = nodeName + ":\n\tsubscribed to topics:\n"; 15 | for (unsigned int i = 0; i < topics.size(); ++i) 16 | { 17 | topicsStr += ("\t\t" + topics.at(i) + "\n"); 18 | } 19 | 20 | topicsStr += "\tadvertised topics:\n"; 21 | ros::this_node::getAdvertisedTopics(topics); 22 | for (unsigned int i = 0; i < topics.size(); ++i) 23 | { 24 | topicsStr += ("\t\t" + topics.at(i) + "\n"); 25 | } 26 | 27 | ROS_INFO("%s", topicsStr.c_str()); 28 | } 29 | 30 | int 31 | main(int argc, char** argv) 32 | { 33 | ros::init(argc, argv, "mavlink"); 34 | 35 | std::string nodeName = ros::this_node::getName(); 36 | ros::NodeHandle nh(nodeName); 37 | 38 | ros::NodeHandle pnh("~"); 39 | 40 | std::string portStr; 41 | pnh.param("serial_port", portStr, std::string("/dev/ttyS0")); 42 | 43 | int baudrate; 44 | pnh.param("baudrate", baudrate, 115200); 45 | 46 | std::string frameId; 47 | pnh.param("frame_id", frameId, std::string("fcu")); 48 | 49 | px::SerialComm comm(nh, frameId); 50 | if (!comm.open(portStr, baudrate)) 51 | { 52 | return -1; 53 | } 54 | 55 | printTopicInfo(); 56 | 57 | ros::spin(); 58 | 59 | return 0; 60 | } 61 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/examples/mavtester.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | test mavlink messages 5 | ''' 6 | 7 | import sys, struct, time, os 8 | from curses import ascii 9 | 10 | # allow import from the parent directory, where mavlink.py is 11 | sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) 12 | 13 | import mavlink, mavtest, mavutil 14 | 15 | from optparse import OptionParser 16 | parser = OptionParser("mavtester.py [options]") 17 | 18 | parser.add_option("--baudrate", dest="baudrate", type='int', 19 | help="master port baud rate", default=115200) 20 | parser.add_option("--device", dest="device", default=None, help="serial device") 21 | parser.add_option("--source-system", dest='SOURCE_SYSTEM', type='int', 22 | default=255, help='MAVLink source system for this GCS') 23 | (opts, args) = parser.parse_args() 24 | 25 | if opts.device is None: 26 | print("You must specify a serial device") 27 | sys.exit(1) 28 | 29 | def wait_heartbeat(m): 30 | '''wait for a heartbeat so we know the target system IDs''' 31 | print("Waiting for APM heartbeat") 32 | msg = m.recv_match(type='HEARTBEAT', blocking=True) 33 | print("Heartbeat from APM (system %u component %u)" % (m.target_system, m.target_system)) 34 | 35 | # create a mavlink serial instance 36 | master = mavutil.mavlink_connection(opts.device, baud=opts.baudrate, source_system=opts.SOURCE_SYSTEM) 37 | 38 | # wait for the heartbeat msg to find the system ID 39 | wait_heartbeat(master) 40 | 41 | print("Sending all message types") 42 | mavtest.generate_outputs(master.mav) 43 | 44 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/examples/flightmodes.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | show changes in flight modes 5 | ''' 6 | 7 | import sys, time, os 8 | 9 | # allow import from the parent directory, where mavlink.py is 10 | sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) 11 | 12 | from optparse import OptionParser 13 | parser = OptionParser("flightmodes.py [options]") 14 | parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") 15 | 16 | (opts, args) = parser.parse_args() 17 | 18 | if opts.mav10: 19 | os.environ['MAVLINK10'] = '1' 20 | import mavutil 21 | 22 | if len(args) < 1: 23 | print("Usage: flightmodes.py [options] ") 24 | sys.exit(1) 25 | 26 | def flight_modes(logfile): 27 | '''show flight modes for a log file''' 28 | print("Processing log %s" % filename) 29 | mlog = mavutil.mavlink_connection(filename) 30 | 31 | mode = -1 32 | nav_mode = -1 33 | 34 | filesize = os.path.getsize(filename) 35 | 36 | while True: 37 | m = mlog.recv_match(type='SYS_STATUS', 38 | condition='SYS_STATUS.mode != %u or SYS_STATUS.nav_mode != %u' % (mode, nav_mode)) 39 | if m is None: 40 | return 41 | mode = m.mode 42 | nav_mode = m.nav_mode 43 | pct = (100.0 * mlog.f.tell()) / filesize 44 | print('%s MAV.flightmode=%-12s mode=%u nav_mode=%u (MAV.timestamp=%u %u%%)' % ( 45 | time.asctime(time.localtime(m._timestamp)), 46 | mlog.flightmode, 47 | mode, nav_mode, m._timestamp, pct)) 48 | 49 | for filename in args: 50 | flight_modes(filename) 51 | 52 | 53 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/generator/gen_all.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | Use mavgen.py on all available MAVLink XML definitions to generate 5 | C and Python MAVLink routines for sending and parsing the protocol 6 | 7 | Copyright Pete Hollands 2011 8 | Released under GNU GPL version 3 or later 9 | ''' 10 | 11 | import os, sys, glob, re 12 | from mavgen import mavgen 13 | 14 | # allow import from the parent directory, where mavutil.py is 15 | sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) 16 | 17 | class options: 18 | """ a class to simulate the options of mavgen OptionsParser""" 19 | def __init__(self, lang, output, wire_protocol): 20 | self.language = lang 21 | self.wire_protocol = wire_protocol 22 | self.output = output 23 | 24 | protocols = [ '0.9', '1.0' ] 25 | 26 | for protocol in protocols : 27 | xml_directory = './message_definitions/v'+protocol 28 | print "xml_directory is", xml_directory 29 | xml_file_names = glob.glob(xml_directory+'/*.xml') 30 | 31 | for xml_file in xml_file_names: 32 | print "xml file is ", xml_file 33 | opts = options(lang = "C", output = "C/include_v"+protocol, \ 34 | wire_protocol=protocol) 35 | args = [] 36 | args.append(xml_file) 37 | mavgen(opts, args) 38 | xml_file_base = os.path.basename(xml_file) 39 | xml_file_base = re.sub("\.xml","", xml_file_base) 40 | print "xml_file_base is", xml_file_base 41 | opts = options(lang = "python", \ 42 | output="python/mavlink_"+xml_file_base+"_v"+protocol+".py", \ 43 | wire_protocol=protocol) 44 | mavgen(opts,args) 45 | -------------------------------------------------------------------------------- /drivers/px4flow/include/px4flow/SerialComm.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | // MAVLINK includes 5 | #include 6 | 7 | // ROS includes 8 | #include 9 | #include 10 | 11 | namespace px 12 | { 13 | 14 | class SerialComm 15 | { 16 | public: 17 | SerialComm(const std::string& frameId); 18 | ~SerialComm(); 19 | 20 | bool open(const std::string& portStr, int baudrate); 21 | void close(void); 22 | 23 | private: 24 | void readCallback(const boost::system::error_code& error, size_t bytesTransferred); 25 | void readStart(uint32_t timeout_ms); 26 | void syncCallback(const ros::TimerEvent& timerEvent); 27 | void timeoutCallback(const boost::system::error_code& error); 28 | 29 | boost::asio::io_service m_uartService; 30 | boost::asio::serial_port m_port; 31 | boost::asio::deadline_timer m_timer; 32 | boost::thread m_uartThread; 33 | 34 | size_t m_imageSize; ///< Image size being transmitted (bytes) 35 | size_t m_imagePackets; ///< Number of data packets being sent for this image 36 | int m_imagePayload; ///< Payload size per transmitted packet (bytes). Standard is 254, and decreases when image resolution increases. 37 | int m_imageWidth; ///< Width of the image stream 38 | int m_imageHeight; ///< Width of the image stream 39 | std::vector m_imageBuffer; 40 | 41 | int m_systemId; 42 | int m_compId; 43 | 44 | uint8_t m_buffer[MAVLINK_MAX_PACKET_LEN]; 45 | 46 | ros::Publisher m_optFlowPub; 47 | image_transport::Publisher m_imagePub; 48 | std::string m_frameId; 49 | 50 | ros::Timer m_syncTimer; 51 | 52 | bool m_timeout; 53 | int m_errorCount; 54 | 55 | bool m_connected; 56 | }; 57 | 58 | } 59 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/examples/flighttime.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | work out total flight time for a mavlink log 5 | ''' 6 | 7 | import sys, time, os 8 | 9 | # allow import from the parent directory, where mavlink.py is 10 | sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) 11 | 12 | from optparse import OptionParser 13 | parser = OptionParser("flighttime.py [options]") 14 | parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") 15 | 16 | (opts, args) = parser.parse_args() 17 | 18 | if opts.mav10: 19 | os.environ['MAVLINK10'] = '1' 20 | import mavutil 21 | 22 | if len(args) < 1: 23 | print("Usage: flighttime.py [options] ") 24 | sys.exit(1) 25 | 26 | def flight_time(logfile): 27 | '''work out flight time for a log file''' 28 | print("Processing log %s" % filename) 29 | mlog = mavutil.mavlink_connection(filename) 30 | 31 | in_air = False 32 | start_time = 0.0 33 | total_time = 0.0 34 | t = None 35 | 36 | while True: 37 | m = mlog.recv_match(type='VFR_HUD') 38 | if m is None: 39 | if in_air: 40 | total_time += time.mktime(t) - start_time 41 | if total_time > 0: 42 | print("Flight time : %u:%02u" % (int(total_time)/60, int(total_time)%60)) 43 | return total_time 44 | t = time.localtime(m._timestamp) 45 | if m.groundspeed > 3.0 and not in_air: 46 | print("In air at %s" % time.asctime(t)) 47 | in_air = True 48 | start_time = time.mktime(t) 49 | elif m.groundspeed < 3.0 and in_air: 50 | print("On ground at %s" % time.asctime(t)) 51 | in_air = False 52 | total_time += time.mktime(t) - start_time 53 | return total_time 54 | 55 | total = 0.0 56 | for filename in args: 57 | total += flight_time(filename) 58 | 59 | print("Total time in air: %u:%02u" % (int(total)/60, int(total)%60)) 60 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/examples/wptogpx.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | example program to extract GPS data from a waypoint file, and create a GPX 5 | file, for loading into google earth 6 | ''' 7 | 8 | import sys, struct, time, os 9 | 10 | # allow import from the parent directory, where mavlink.py is 11 | sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) 12 | 13 | from optparse import OptionParser 14 | parser = OptionParser("wptogpx.py [options]") 15 | (opts, args) = parser.parse_args() 16 | 17 | import mavutil, mavwp 18 | 19 | if len(args) < 1: 20 | print("Usage: wptogpx.py ") 21 | sys.exit(1) 22 | 23 | def wp_to_gpx(infilename, outfilename): 24 | '''convert a wp file to a GPX file''' 25 | 26 | wp = mavwp.MAVWPLoader() 27 | wp.load(infilename) 28 | outf = open(outfilename, mode='w') 29 | 30 | def process_wp(w, i): 31 | t = time.localtime(i) 32 | outf.write(''' 33 | %s 34 | WP %u 35 | 36 | ''' % (w.x, w.y, w.z, i)) 37 | 38 | def add_header(): 39 | outf.write(''' 40 | 46 | ''') 47 | 48 | def add_footer(): 49 | outf.write(''' 50 | 51 | ''') 52 | 53 | add_header() 54 | 55 | count = 0 56 | for i in range(wp.count()): 57 | w = wp.wp(i) 58 | if w.frame == 3: 59 | w.z += wp.wp(0).z 60 | if w.command == 16: 61 | process_wp(w, i) 62 | count += 1 63 | add_footer() 64 | print("Created %s with %u points" % (outfilename, count)) 65 | 66 | 67 | for infilename in args: 68 | outfilename = infilename + '.gpx' 69 | wp_to_gpx(infilename, outfilename) 70 | -------------------------------------------------------------------------------- /px_comm/msg/CameraInfo.msg: -------------------------------------------------------------------------------- 1 | ####################################################################### 2 | # Image acquisition info # 3 | ####################################################################### 4 | 5 | # Time of image acquisition, camera coordinate frame ID 6 | Header header # Header timestamp should be acquisition time of image 7 | # Header frame_id should be optical frame of camera 8 | # origin of frame should be optical center of camera 9 | # +x should point to the right in the image 10 | # +y should point down in the image 11 | # +z should point into the plane of the image 12 | 13 | 14 | ####################################################################### 15 | # Calibration Parameters # 16 | ####################################################################### 17 | # These are fixed during camera calibration. Their values will be the # 18 | # same in all messages until the camera is recalibrated. Note that # 19 | # self-calibrating systems may "recalibrate" frequently. # 20 | ####################################################################### 21 | 22 | # The camera model used. 23 | string camera_model 24 | 25 | # The name of the camera. 26 | string camera_name 27 | 28 | # The type of the camera. 29 | string camera_type 30 | 31 | # The image dimensions with which the camera was calibrated. Normally 32 | # this will be the full camera resolution in pixels. 33 | uint32 image_width 34 | uint32 image_height 35 | 36 | # The distortion parameters, size depending on the distortion model. 37 | float64[] D 38 | 39 | # The projection parameters, size depending on the projection model. 40 | float64[] P 41 | 42 | # Other parameters which are not defined by either the distortion or 43 | # projection model. 44 | float64[] M 45 | 46 | # Pose of camera with respect to a specific reference frame. 47 | geometry_msgs/Pose pose -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/examples/bwtest.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | check bandwidth of link 5 | ''' 6 | 7 | import sys, struct, time, os 8 | 9 | # allow import from the parent directory, where mavlink.py is 10 | sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) 11 | 12 | import mavutil 13 | 14 | from optparse import OptionParser 15 | parser = OptionParser("bwtest.py [options]") 16 | 17 | parser.add_option("--baudrate", dest="baudrate", type='int', 18 | help="master port baud rate", default=115200) 19 | parser.add_option("--device", dest="device", default=None, help="serial device") 20 | (opts, args) = parser.parse_args() 21 | 22 | if opts.device is None: 23 | print("You must specify a serial device") 24 | sys.exit(1) 25 | 26 | # create a mavlink serial instance 27 | master = mavutil.mavlink_connection(opts.device, baud=opts.baudrate) 28 | 29 | t1 = time.time() 30 | 31 | counts = {} 32 | 33 | bytes_sent = 0 34 | bytes_recv = 0 35 | 36 | while True: 37 | master.mav.heartbeat_send(1, 1) 38 | master.mav.sys_status_send(1, 2, 3, 4, 5, 6, 7) 39 | master.mav.gps_raw_send(1, 2, 3, 4, 5, 6, 7, 8, 9) 40 | master.mav.attitude_send(1, 2, 3, 4, 5, 6, 7) 41 | master.mav.vfr_hud_send(1, 2, 3, 4, 5, 6) 42 | while master.port.inWaiting() > 0: 43 | m = master.recv_msg() 44 | if m == None: break 45 | if m.get_type() not in counts: 46 | counts[m.get_type()] = 0 47 | counts[m.get_type()] += 1 48 | t2 = time.time() 49 | if t2 - t1 > 1.0: 50 | print("%u sent, %u received, %u errors bwin=%.1f kB/s bwout=%.1f kB/s" % ( 51 | master.mav.total_packets_sent, 52 | master.mav.total_packets_received, 53 | master.mav.total_receive_errors, 54 | 0.001*(master.mav.total_bytes_received-bytes_recv)/(t2-t1), 55 | 0.001*(master.mav.total_bytes_sent-bytes_sent)/(t2-t1))) 56 | bytes_sent = master.mav.total_bytes_sent 57 | bytes_recv = master.mav.total_bytes_received 58 | t1 = t2 59 | -------------------------------------------------------------------------------- /px_comm/include/px_comm/DDSImageSupport.h: -------------------------------------------------------------------------------- 1 | 2 | /* 3 | WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. 4 | 5 | This file was generated from DDSImage.idl using "rtiddsgen". 6 | The rtiddsgen tool is part of the RTI Connext distribution. 7 | For more information, type 'rtiddsgen -help' at a command shell 8 | or consult the RTI Connext manual. 9 | */ 10 | 11 | #ifndef DDSImageSupport_248003145_h 12 | #define DDSImageSupport_248003145_h 13 | 14 | /* Uses */ 15 | #include "DDSImage.h" 16 | 17 | #ifdef __cplusplus 18 | #ifndef ndds_cpp_h 19 | #include "ndds/ndds_cpp.h" 20 | #endif 21 | #else 22 | #ifndef ndds_c_h 23 | #include "ndds/ndds_c.h" 24 | #endif 25 | #endif 26 | 27 | 28 | namespace px_comm { 29 | /* ========================================================================= */ 30 | /** 31 | Uses: T 32 | 33 | Defines: TTypeSupport, TDataWriter, TDataReader 34 | 35 | Organized using the well-documented "Generics Pattern" for 36 | implementing generics in C and C++. 37 | */ 38 | 39 | #if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) 40 | /* If the code is building on Windows, start exporting symbols. 41 | */ 42 | #undef NDDSUSERDllExport 43 | #define NDDSUSERDllExport __declspec(dllexport) 44 | 45 | #endif 46 | 47 | #ifdef __cplusplus 48 | 49 | DDS_TYPESUPPORT_CPP(DDSImageTypeSupport, DDSImage); 50 | 51 | DDS_DATAWRITER_CPP(DDSImageDataWriter, DDSImage); 52 | DDS_DATAREADER_CPP(DDSImageDataReader, DDSImageSeq, DDSImage); 53 | 54 | #else 55 | 56 | DDS_TYPESUPPORT_C(DDSImageTypeSupport, DDSImage); 57 | DDS_DATAWRITER_C(DDSImageDataWriter, DDSImage); 58 | DDS_DATAREADER_C(DDSImageDataReader, DDSImageSeq, DDSImage); 59 | 60 | #endif 61 | 62 | #if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) 63 | /* If the code is building on Windows, stop exporting symbols. 64 | */ 65 | #undef NDDSUSERDllExport 66 | #define NDDSUSERDllExport 67 | #endif 68 | } /* namespace px_comm */ 69 | 70 | #endif /* DDSImageSupport_248003145_h */ 71 | 72 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/examples/sigloss.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | show times when signal is lost 5 | ''' 6 | 7 | import sys, time, os 8 | 9 | # allow import from the parent directory, where mavlink.py is 10 | sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) 11 | 12 | from optparse import OptionParser 13 | parser = OptionParser("sigloss.py [options]") 14 | parser.add_option("--no-timestamps",dest="notimestamps", action='store_true', help="Log doesn't have timestamps") 15 | parser.add_option("--planner",dest="planner", action='store_true', help="use planner file format") 16 | parser.add_option("--robust",dest="robust", action='store_true', help="Enable robust parsing (skip over bad data)") 17 | parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") 18 | parser.add_option("--deltat", type='float', default=1.0, help="loss threshold in seconds") 19 | 20 | (opts, args) = parser.parse_args() 21 | 22 | if opts.mav10: 23 | os.environ['MAVLINK10'] = '1' 24 | import mavutil 25 | 26 | if len(args) < 1: 27 | print("Usage: sigloss.py [options] ") 28 | sys.exit(1) 29 | 30 | def sigloss(logfile): 31 | '''work out signal loss times for a log file''' 32 | print("Processing log %s" % filename) 33 | mlog = mavutil.mavlink_connection(filename, 34 | planner_format=opts.planner, 35 | notimestamps=opts.notimestamps, 36 | robust_parsing=opts.robust) 37 | 38 | last_t = 0 39 | 40 | while True: 41 | m = mlog.recv_match() 42 | if m is None: 43 | return 44 | if opts.notimestamps: 45 | if not 'usec' in m._fieldnames: 46 | continue 47 | t = m.usec / 1.0e6 48 | else: 49 | t = m._timestamp 50 | if last_t != 0: 51 | if t - last_t > opts.deltat: 52 | print("Sig lost for %.1fs at %s" % (t-last_t, time.asctime(time.localtime(t)))) 53 | last_t = t 54 | 55 | total = 0.0 56 | for filename in args: 57 | sigloss(filename) 58 | -------------------------------------------------------------------------------- /drivers/mavlink_serial_client/include/mavlink_serial_client/SerialComm.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | // MAVLINK includes 5 | #include 6 | 7 | // ROS includes 8 | #include 9 | #include 10 | 11 | namespace px 12 | { 13 | 14 | class SerialComm 15 | { 16 | public: 17 | SerialComm(ros::NodeHandle& nh, const std::string& frameId); 18 | ~SerialComm(); 19 | 20 | bool open(const std::string& portStr, int baudrate); 21 | void close(void); 22 | 23 | private: 24 | void readCallback(const boost::system::error_code& error, size_t bytesTransferred); 25 | void readStart(uint32_t timeout_ms); 26 | void syncCallback(const ros::TimerEvent& timerEvent); 27 | void timeoutCallback(const boost::system::error_code& error); 28 | 29 | void rpyToQuaternion(const double& roll, const double& pitch, const double& yaw, 30 | double& w, double& x, double& y, double& z) const; 31 | void publishMAVLINKMessage(const mavlink_message_t& message); 32 | 33 | boost::asio::io_service m_uartService; 34 | boost::asio::serial_port m_port; 35 | boost::asio::deadline_timer m_timer; 36 | boost::thread m_uartThread; 37 | 38 | size_t m_imageSize; ///< Image size being transmitted (bytes) 39 | size_t m_imagePackets; ///< Number of data packets being sent for this image 40 | int m_imagePayload; ///< Payload size per transmitted packet (bytes). Standard is 254, and decreases when image resolution increases. 41 | int m_imageWidth; ///< Width of the image stream 42 | int m_imageHeight; ///< Width of the image stream 43 | std::vector m_imageBuffer; 44 | 45 | int m_systemId; 46 | int m_compId; 47 | 48 | uint8_t m_buffer[MAVLINK_MAX_PACKET_LEN]; 49 | 50 | ros::NodeHandle m_nh; 51 | image_transport::Publisher m_imagePub; 52 | ros::Publisher m_imuPub; 53 | ros::Publisher m_imuRawPub; 54 | ros::Publisher m_magPub; 55 | ros::Publisher m_mavlinkPub; 56 | ros::Publisher m_optFlowPub; 57 | ros::Publisher m_viconPub; 58 | 59 | mavlink_highres_imu_t m_imuRaw; 60 | 61 | std::string m_frameId; 62 | 63 | bool m_timeout; 64 | int m_errorCount; 65 | 66 | bool m_connected; 67 | }; 68 | 69 | } 70 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/checksum.h: -------------------------------------------------------------------------------- 1 | #ifdef __cplusplus 2 | extern "C" { 3 | #endif 4 | 5 | #ifndef _CHECKSUM_H_ 6 | #define _CHECKSUM_H_ 7 | 8 | 9 | /** 10 | * 11 | * CALCULATE THE CHECKSUM 12 | * 13 | */ 14 | 15 | #define X25_INIT_CRC 0xffff 16 | #define X25_VALIDATE_CRC 0xf0b8 17 | 18 | /** 19 | * @brief Accumulate the X.25 CRC by adding one char at a time. 20 | * 21 | * The checksum function adds the hash of one char at a time to the 22 | * 16 bit checksum (uint16_t). 23 | * 24 | * @param data new char to hash 25 | * @param crcAccum the already accumulated checksum 26 | **/ 27 | static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) 28 | { 29 | /*Accumulate one byte of data into the CRC*/ 30 | uint8_t tmp; 31 | 32 | tmp = data ^ (uint8_t)(*crcAccum &0xff); 33 | tmp ^= (tmp<<4); 34 | *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); 35 | } 36 | 37 | /** 38 | * @brief Initiliaze the buffer for the X.25 CRC 39 | * 40 | * @param crcAccum the 16 bit X.25 CRC 41 | */ 42 | static inline void crc_init(uint16_t* crcAccum) 43 | { 44 | *crcAccum = X25_INIT_CRC; 45 | } 46 | 47 | 48 | /** 49 | * @brief Calculates the X.25 checksum on a byte buffer 50 | * 51 | * @param pBuffer buffer containing the byte array to hash 52 | * @param length length of the byte array 53 | * @return the checksum over the buffer bytes 54 | **/ 55 | static inline uint16_t crc_calculate(uint8_t* pBuffer, uint16_t length) 56 | { 57 | uint16_t crcTmp; 58 | crc_init(&crcTmp); 59 | while (length--) { 60 | crc_accumulate(*pBuffer++, &crcTmp); 61 | } 62 | return crcTmp; 63 | } 64 | 65 | /** 66 | * @brief Accumulate the X.25 CRC by adding an array of bytes 67 | * 68 | * The checksum function adds the hash of one char at a time to the 69 | * 16 bit checksum (uint16_t). 70 | * 71 | * @param data new bytes to hash 72 | * @param crcAccum the already accumulated checksum 73 | **/ 74 | static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint8_t length) 75 | { 76 | const uint8_t *p = (const uint8_t *)pBuffer; 77 | while (length--) { 78 | crc_accumulate(*p++, crcAccum); 79 | } 80 | } 81 | 82 | 83 | 84 | 85 | #endif /* _CHECKSUM_H_ */ 86 | 87 | #ifdef __cplusplus 88 | } 89 | #endif 90 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/checksum.h: -------------------------------------------------------------------------------- 1 | #ifdef __cplusplus 2 | extern "C" { 3 | #endif 4 | 5 | #ifndef _CHECKSUM_H_ 6 | #define _CHECKSUM_H_ 7 | 8 | 9 | /** 10 | * 11 | * CALCULATE THE CHECKSUM 12 | * 13 | */ 14 | 15 | #define X25_INIT_CRC 0xffff 16 | #define X25_VALIDATE_CRC 0xf0b8 17 | 18 | /** 19 | * @brief Accumulate the X.25 CRC by adding one char at a time. 20 | * 21 | * The checksum function adds the hash of one char at a time to the 22 | * 16 bit checksum (uint16_t). 23 | * 24 | * @param data new char to hash 25 | * @param crcAccum the already accumulated checksum 26 | **/ 27 | static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) 28 | { 29 | /*Accumulate one byte of data into the CRC*/ 30 | uint8_t tmp; 31 | 32 | tmp = data ^ (uint8_t)(*crcAccum &0xff); 33 | tmp ^= (tmp<<4); 34 | *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); 35 | } 36 | 37 | /** 38 | * @brief Initiliaze the buffer for the X.25 CRC 39 | * 40 | * @param crcAccum the 16 bit X.25 CRC 41 | */ 42 | static inline void crc_init(uint16_t* crcAccum) 43 | { 44 | *crcAccum = X25_INIT_CRC; 45 | } 46 | 47 | 48 | /** 49 | * @brief Calculates the X.25 checksum on a byte buffer 50 | * 51 | * @param pBuffer buffer containing the byte array to hash 52 | * @param length length of the byte array 53 | * @return the checksum over the buffer bytes 54 | **/ 55 | static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length) 56 | { 57 | uint16_t crcTmp; 58 | crc_init(&crcTmp); 59 | while (length--) { 60 | crc_accumulate(*pBuffer++, &crcTmp); 61 | } 62 | return crcTmp; 63 | } 64 | 65 | /** 66 | * @brief Accumulate the X.25 CRC by adding an array of bytes 67 | * 68 | * The checksum function adds the hash of one char at a time to the 69 | * 16 bit checksum (uint16_t). 70 | * 71 | * @param data new bytes to hash 72 | * @param crcAccum the already accumulated checksum 73 | **/ 74 | static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint8_t length) 75 | { 76 | const uint8_t *p = (const uint8_t *)pBuffer; 77 | while (length--) { 78 | crc_accumulate(*p++, crcAccum); 79 | } 80 | } 81 | 82 | 83 | 84 | 85 | #endif /* _CHECKSUM_H_ */ 86 | 87 | #ifdef __cplusplus 88 | } 89 | #endif 90 | -------------------------------------------------------------------------------- /mavlink/include/mavlink/v1.0/checksum.h: -------------------------------------------------------------------------------- 1 | #ifdef __cplusplus 2 | extern "C" { 3 | #endif 4 | 5 | #ifndef _CHECKSUM_H_ 6 | #define _CHECKSUM_H_ 7 | 8 | 9 | /** 10 | * 11 | * CALCULATE THE CHECKSUM 12 | * 13 | */ 14 | 15 | #define X25_INIT_CRC 0xffff 16 | #define X25_VALIDATE_CRC 0xf0b8 17 | 18 | #ifndef HAVE_CRC_ACCUMULATE 19 | /** 20 | * @brief Accumulate the X.25 CRC by adding one char at a time. 21 | * 22 | * The checksum function adds the hash of one char at a time to the 23 | * 16 bit checksum (uint16_t). 24 | * 25 | * @param data new char to hash 26 | * @param crcAccum the already accumulated checksum 27 | **/ 28 | static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) 29 | { 30 | /*Accumulate one byte of data into the CRC*/ 31 | uint8_t tmp; 32 | 33 | tmp = data ^ (uint8_t)(*crcAccum &0xff); 34 | tmp ^= (tmp<<4); 35 | *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); 36 | } 37 | #endif 38 | 39 | /** 40 | * @brief Initiliaze the buffer for the X.25 CRC 41 | * 42 | * @param crcAccum the 16 bit X.25 CRC 43 | */ 44 | static inline void crc_init(uint16_t* crcAccum) 45 | { 46 | *crcAccum = X25_INIT_CRC; 47 | } 48 | 49 | 50 | /** 51 | * @brief Calculates the X.25 checksum on a byte buffer 52 | * 53 | * @param pBuffer buffer containing the byte array to hash 54 | * @param length length of the byte array 55 | * @return the checksum over the buffer bytes 56 | **/ 57 | static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length) 58 | { 59 | uint16_t crcTmp; 60 | crc_init(&crcTmp); 61 | while (length--) { 62 | crc_accumulate(*pBuffer++, &crcTmp); 63 | } 64 | return crcTmp; 65 | } 66 | 67 | /** 68 | * @brief Accumulate the X.25 CRC by adding an array of bytes 69 | * 70 | * The checksum function adds the hash of one char at a time to the 71 | * 16 bit checksum (uint16_t). 72 | * 73 | * @param data new bytes to hash 74 | * @param crcAccum the already accumulated checksum 75 | **/ 76 | static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint8_t length) 77 | { 78 | const uint8_t *p = (const uint8_t *)pBuffer; 79 | while (length--) { 80 | crc_accumulate(*p++, crcAccum); 81 | } 82 | } 83 | 84 | 85 | 86 | 87 | #endif /* _CHECKSUM_H_ */ 88 | 89 | #ifdef __cplusplus 90 | } 91 | #endif 92 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/examples/gpslock.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | show GPS lock events in a MAVLink log 5 | ''' 6 | 7 | import sys, time, os 8 | 9 | # allow import from the parent directory, where mavlink.py is 10 | sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) 11 | 12 | from optparse import OptionParser 13 | parser = OptionParser("gpslock.py [options]") 14 | parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") 15 | 16 | (opts, args) = parser.parse_args() 17 | 18 | if opts.mav10: 19 | os.environ['MAVLINK10'] = '1' 20 | import mavutil 21 | 22 | if len(args) < 1: 23 | print("Usage: gpslock.py [options] ") 24 | sys.exit(1) 25 | 26 | def lock_time(logfile): 27 | '''work out gps lock times for a log file''' 28 | print("Processing log %s" % filename) 29 | mlog = mavutil.mavlink_connection(filename) 30 | 31 | locked = False 32 | start_time = 0.0 33 | total_time = 0.0 34 | t = None 35 | m = mlog.recv_match(type='GPS_RAW') 36 | unlock_time = time.mktime(time.localtime(m._timestamp)) 37 | 38 | while True: 39 | m = mlog.recv_match(type='GPS_RAW') 40 | if m is None: 41 | if locked: 42 | total_time += time.mktime(t) - start_time 43 | if total_time > 0: 44 | print("Lock time : %u:%02u" % (int(total_time)/60, int(total_time)%60)) 45 | return total_time 46 | t = time.localtime(m._timestamp) 47 | if m.fix_type == 2 and not locked: 48 | print("Locked at %s after %u seconds" % (time.asctime(t), 49 | time.mktime(t) - unlock_time)) 50 | locked = True 51 | start_time = time.mktime(t) 52 | elif m.fix_type == 1 and locked: 53 | print("Lost GPS lock at %s" % time.asctime(t)) 54 | locked = False 55 | total_time += time.mktime(t) - start_time 56 | unlock_time = time.mktime(t) 57 | elif m.fix_type == 0 and locked: 58 | print("Lost protocol lock at %s" % time.asctime(t)) 59 | locked = False 60 | total_time += time.mktime(t) - start_time 61 | unlock_time = time.mktime(t) 62 | return total_time 63 | 64 | total = 0.0 65 | for filename in args: 66 | total += lock_time(filename) 67 | 68 | print("Total time locked: %u:%02u" % (int(total)/60, int(total)%60)) 69 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/examples/apmsetrate.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | set stream rate on an APM 5 | ''' 6 | 7 | import sys, struct, time, os 8 | 9 | # allow import from the parent directory, where mavlink.py is 10 | sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) 11 | 12 | from optparse import OptionParser 13 | parser = OptionParser("apmsetrate.py [options]") 14 | 15 | parser.add_option("--baudrate", dest="baudrate", type='int', 16 | help="master port baud rate", default=115200) 17 | parser.add_option("--device", dest="device", default=None, help="serial device") 18 | parser.add_option("--rate", dest="rate", default=4, type='int', help="requested stream rate") 19 | parser.add_option("--source-system", dest='SOURCE_SYSTEM', type='int', 20 | default=255, help='MAVLink source system for this GCS') 21 | parser.add_option("--showmessages", dest="showmessages", action='store_true', 22 | help="show incoming messages", default=False) 23 | parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") 24 | (opts, args) = parser.parse_args() 25 | 26 | if opts.mav10: 27 | os.environ['MAVLINK10'] = '1' 28 | import mavlink10 as mavlink 29 | else: 30 | import mavlink 31 | import mavutil 32 | 33 | if opts.device is None: 34 | print("You must specify a serial device") 35 | sys.exit(1) 36 | 37 | def wait_heartbeat(m): 38 | '''wait for a heartbeat so we know the target system IDs''' 39 | print("Waiting for APM heartbeat") 40 | m.wait_heartbeat() 41 | print("Heartbeat from APM (system %u component %u)" % (m.target_system, m.target_system)) 42 | 43 | def show_messages(m): 44 | '''show incoming mavlink messages''' 45 | while True: 46 | msg = m.recv_match(blocking=True) 47 | if not msg: 48 | return 49 | if msg.get_type() == "BAD_DATA": 50 | if mavutil.all_printable(msg.data): 51 | sys.stdout.write(msg.data) 52 | sys.stdout.flush() 53 | else: 54 | print(msg) 55 | 56 | # create a mavlink serial instance 57 | master = mavutil.mavlink_connection(opts.device, baud=opts.baudrate) 58 | 59 | # wait for the heartbeat msg to find the system ID 60 | wait_heartbeat(master) 61 | 62 | print("Sending all stream request for rate %u" % opts.rate) 63 | for i in range(0, 3): 64 | master.mav.request_data_stream_send(master.target_system, master.target_component, 65 | mavlink.MAV_DATA_STREAM_ALL, opts.rate, 1) 66 | if opts.showmessages: 67 | show_messages(master) 68 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/examples/mavtogpx.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | example program to extract GPS data from a mavlink log, and create a GPX 5 | file, for loading into google earth 6 | ''' 7 | 8 | import sys, struct, time, os 9 | 10 | # allow import from the parent directory, where mavlink.py is 11 | sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) 12 | 13 | from optparse import OptionParser 14 | parser = OptionParser("mavtogpx.py [options]") 15 | parser.add_option("--condition",dest="condition", default=None, help="select packets by a condition") 16 | parser.add_option("--nofixcheck", default=False, action='store_true', help="don't check for GPS fix") 17 | parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") 18 | (opts, args) = parser.parse_args() 19 | 20 | if opts.mav10: 21 | os.environ['MAVLINK10'] = '1' 22 | import mavutil 23 | 24 | if len(args) < 1: 25 | print("Usage: mavtogpx.py ") 26 | sys.exit(1) 27 | 28 | def mav_to_gpx(infilename, outfilename): 29 | '''convert a mavlink log file to a GPX file''' 30 | 31 | mlog = mavutil.mavlink_connection(infilename) 32 | outf = open(outfilename, mode='w') 33 | 34 | def process_packet(m): 35 | t = time.localtime(m._timestamp) 36 | outf.write(''' 37 | %s 38 | 39 | %s 40 | %s 41 | 3d 42 | 43 | ''' % (m.lat, m.lon, m.alt, 44 | time.strftime("%Y-%m-%dT%H:%M:%SZ", t), 45 | m.hdg, m.v)) 46 | 47 | def add_header(): 48 | outf.write(''' 49 | 55 | 56 | 57 | ''') 58 | 59 | def add_footer(): 60 | outf.write(''' 61 | 62 | 63 | ''') 64 | 65 | add_header() 66 | 67 | count=0 68 | while True: 69 | m = mlog.recv_match(type='GPS_RAW', condition=opts.condition) 70 | if m is None: break 71 | if m.fix_type != 2 and not opts.nofixcheck: 72 | continue 73 | if m.lat == 0.0 or m.lon == 0.0: 74 | continue 75 | process_packet(m) 76 | count += 1 77 | add_footer() 78 | print("Created %s with %u points" % (outfilename, count)) 79 | 80 | 81 | for infilename in args: 82 | outfilename = infilename + '.gpx' 83 | mav_to_gpx(infilename, outfilename) 84 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/examples/mavlogdump.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | example program that dumps a Mavlink log file. The log file is 5 | assumed to be in the format that qgroundcontrol uses, which consists 6 | of a series of MAVLink packets, each with a 64 bit timestamp 7 | header. The timestamp is in microseconds since 1970 (unix epoch) 8 | ''' 9 | 10 | import sys, time, os, struct 11 | 12 | # allow import from the parent directory, where mavlink.py is 13 | sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) 14 | 15 | from optparse import OptionParser 16 | parser = OptionParser("mavlogdump.py [options]") 17 | 18 | parser.add_option("--no-timestamps",dest="notimestamps", action='store_true', help="Log doesn't have timestamps") 19 | parser.add_option("--planner",dest="planner", action='store_true', help="use planner file format") 20 | parser.add_option("--robust",dest="robust", action='store_true', help="Enable robust parsing (skip over bad data)") 21 | parser.add_option("-f", "--follow",dest="follow", action='store_true', help="keep waiting for more data at end of file") 22 | parser.add_option("--condition",dest="condition", default=None, help="select packets by condition") 23 | parser.add_option("-q", "--quiet", dest="quiet", action='store_true', help="don't display packets") 24 | parser.add_option("-o", "--output", default=None, help="output matching packets to give file") 25 | parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") 26 | parser.add_option("--types", default=None, help="types of messages (comma separated)") 27 | (opts, args) = parser.parse_args() 28 | 29 | if opts.mav10: 30 | os.environ['MAVLINK10'] = '1' 31 | import mavutil 32 | 33 | if len(args) < 1: 34 | print("Usage: mavlogdump.py [options] ") 35 | sys.exit(1) 36 | 37 | filename = args[0] 38 | mlog = mavutil.mavlink_connection(filename, planner_format=opts.planner, 39 | notimestamps=opts.notimestamps, 40 | robust_parsing=opts.robust) 41 | 42 | output = None 43 | if opts.output: 44 | output = mavutil.mavlogfile(opts.output, write=True) 45 | 46 | types = opts.types 47 | if types is not None: 48 | types = types.split(',') 49 | 50 | while True: 51 | m = mlog.recv_match(condition=opts.condition, blocking=opts.follow) 52 | if m is None: 53 | break 54 | if types is not None and m.get_type() not in types: 55 | continue 56 | if output: 57 | timestamp = getattr(m, '_timestamp', None) 58 | if timestamp: 59 | output.write(struct.pack('>Q', timestamp*1.0e6)) 60 | output.write(m.get_msgbuf().tostring()) 61 | if opts.quiet: 62 | continue 63 | print("%s.%02u: %s" % ( 64 | time.strftime("%Y-%m-%d %H:%M:%S", 65 | time.localtime(m._timestamp)), 66 | int(m._timestamp*100.0)%100, m)) 67 | 68 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/generator/mavgen.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | ''' 3 | parse a MAVLink protocol XML file and generate a python implementation 4 | 5 | Copyright Andrew Tridgell 2011 6 | Released under GNU GPL version 3 or later 7 | ''' 8 | 9 | def mavgen(opts, args) : 10 | """Generate mavlink message formatters and parsers (C and Python ) using options 11 | and args where args are a list of xml files. This function allows python 12 | scripts under Windows to control mavgen using the same interface as 13 | shell scripts under Unix""" 14 | import sys, textwrap, os 15 | 16 | import mavparse 17 | import mavgen_python 18 | import mavgen_c 19 | 20 | xml = [] 21 | 22 | for fname in args: 23 | print("Parsing %s" % fname) 24 | xml.append(mavparse.MAVXML(fname, opts.wire_protocol)) 25 | 26 | # expand includes 27 | for x in xml[:]: 28 | for i in x.include: 29 | fname = os.path.join(os.path.dirname(x.filename), i) 30 | print("Parsing %s" % fname) 31 | xml.append(mavparse.MAVXML(fname, opts.wire_protocol)) 32 | 33 | # include message lengths and CRCs too 34 | for idx in range(0, 256): 35 | if x.message_lengths[idx] == 0: 36 | x.message_lengths[idx] = xml[-1].message_lengths[idx] 37 | x.message_crcs[idx] = xml[-1].message_crcs[idx] 38 | x.message_names[idx] = xml[-1].message_names[idx] 39 | 40 | # work out max payload size across all includes 41 | largest_payload = 0 42 | for x in xml: 43 | if x.largest_payload > largest_payload: 44 | largest_payload = x.largest_payload 45 | for x in xml: 46 | x.largest_payload = largest_payload 47 | 48 | if mavparse.check_duplicates(xml): 49 | sys.exit(1) 50 | 51 | print("Found %u MAVLink message types in %u XML files" % ( 52 | mavparse.total_msgs(xml), len(xml))) 53 | 54 | if opts.language == 'python': 55 | mavgen_python.generate(opts.output, xml) 56 | elif opts.language == 'C': 57 | mavgen_c.generate(opts.output, xml) 58 | else: 59 | print("Unsupported language %s" % opts.language) 60 | 61 | 62 | if __name__=="__main__": 63 | import sys, textwrap, os 64 | 65 | from optparse import OptionParser 66 | 67 | # allow import from the parent directory, where mavutil.py is 68 | sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) 69 | 70 | import mavparse 71 | import mavgen_python 72 | import mavgen_c 73 | 74 | parser = OptionParser("%prog [options] ") 75 | parser.add_option("-o", "--output", dest="output", default="mavlink", help="output directory.") 76 | parser.add_option("--lang", dest="language", default="python", help="language of generated code: 'Python' or 'C' [default: %default]") 77 | parser.add_option("--wire-protocol", dest="wire_protocol", default=mavparse.PROTOCOL_0_9, help="MAVLink protocol version: '0.9' or '1.0'. [default: %default]") 78 | (opts, args) = parser.parse_args() 79 | 80 | if len(args) < 1: 81 | parser.error("You must supply at least one MAVLink XML protocol definition") 82 | mavgen(opts, args) 83 | -------------------------------------------------------------------------------- /px_comm/dds_msg/DDSImageSupport.cxx: -------------------------------------------------------------------------------- 1 | 2 | /* 3 | WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. 4 | 5 | This file was generated from DDSImage.idl using "rtiddsgen". 6 | The rtiddsgen tool is part of the RTI Connext distribution. 7 | For more information, type 'rtiddsgen -help' at a command shell 8 | or consult the RTI Connext manual. 9 | */ 10 | 11 | #include "DDSImageSupport.h" 12 | #include "DDSImagePlugin.h" 13 | 14 | #ifdef __cplusplus 15 | #ifndef dds_c_log_impl_h 16 | #include "dds_c/dds_c_log_impl.h" 17 | #endif 18 | #endif 19 | 20 | namespace px_comm { 21 | 22 | /* ========================================================================= */ 23 | /** 24 | <> 25 | 26 | Defines: TData, 27 | TDataWriter, 28 | TDataReader, 29 | TTypeSupport 30 | 31 | Configure and implement 'DDSImage' support classes. 32 | 33 | Note: Only the #defined classes get defined 34 | */ 35 | 36 | /* ----------------------------------------------------------------- */ 37 | /* DDSDataWriter 38 | */ 39 | 40 | /** 41 | <> 42 | 43 | Defines: TDataWriter, TData 44 | */ 45 | 46 | /* Requires */ 47 | #define TTYPENAME DDSImageTYPENAME 48 | 49 | /* Defines */ 50 | #define TDataWriter DDSImageDataWriter 51 | #define TData px_comm::DDSImage 52 | 53 | #ifdef __cplusplus 54 | #include "dds_cpp/generic/dds_cpp_data_TDataWriter.gen" 55 | #else 56 | #include "dds_c/generic/dds_c_data_TDataWriter.gen" 57 | #endif 58 | 59 | #undef TDataWriter 60 | #undef TData 61 | 62 | #undef TTYPENAME 63 | 64 | /* ----------------------------------------------------------------- */ 65 | /* DDSDataReader 66 | */ 67 | 68 | /** 69 | <> 70 | 71 | Defines: TDataReader, TDataSeq, TData 72 | */ 73 | 74 | /* Requires */ 75 | #define TTYPENAME DDSImageTYPENAME 76 | 77 | /* Defines */ 78 | #define TDataReader DDSImageDataReader 79 | #define TDataSeq DDSImageSeq 80 | #define TData px_comm::DDSImage 81 | 82 | #ifdef __cplusplus 83 | #include "dds_cpp/generic/dds_cpp_data_TDataReader.gen" 84 | #else 85 | #include "dds_c/generic/dds_c_data_TDataReader.gen" 86 | #endif 87 | 88 | #undef TDataReader 89 | #undef TDataSeq 90 | #undef TData 91 | 92 | #undef TTYPENAME 93 | 94 | /* ----------------------------------------------------------------- */ 95 | /* TypeSupport 96 | 97 | <> 98 | 99 | Requires: TTYPENAME, 100 | TPlugin_new 101 | TPlugin_delete 102 | Defines: TTypeSupport, TData, TDataReader, TDataWriter 103 | */ 104 | 105 | /* Requires */ 106 | #define TTYPENAME DDSImageTYPENAME 107 | #define TPlugin_new px_comm::DDSImagePlugin_new 108 | #define TPlugin_delete px_comm::DDSImagePlugin_delete 109 | 110 | /* Defines */ 111 | #define TTypeSupport DDSImageTypeSupport 112 | #define TData px_comm::DDSImage 113 | #define TDataReader DDSImageDataReader 114 | #define TDataWriter DDSImageDataWriter 115 | #ifdef __cplusplus 116 | 117 | #include "dds_cpp/generic/dds_cpp_data_TTypeSupport.gen" 118 | 119 | #else 120 | #include "dds_c/generic/dds_c_data_TTypeSupport.gen" 121 | #endif 122 | #undef TTypeSupport 123 | #undef TData 124 | #undef TDataReader 125 | #undef TDataWriter 126 | 127 | #undef TTYPENAME 128 | #undef TPlugin_new 129 | #undef TPlugin_delete 130 | 131 | } /* namespace px_comm */ 132 | 133 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/generator/gen_MatrixPilot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | ''' 3 | Use mavgen.py matrixpilot.xml definitions to generate 4 | C and Python MAVLink routines for sending and parsing the protocol 5 | This python script is soley for MatrixPilot MAVLink impoementation 6 | 7 | Copyright Pete Hollands 2011 8 | Released under GNU GPL version 3 or later 9 | ''' 10 | 11 | import os, sys, glob, re 12 | from shutil import copy 13 | from mavgen import mavgen 14 | 15 | # allow import from the parent directory, where mavutil.py is 16 | # Under Windows, this script must be run from a DOS command window 17 | sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) 18 | 19 | class options: 20 | """ a class to simulate the options of mavgen OptionsParser""" 21 | def __init__(self, lang, output, wire_protocol): 22 | self.language = lang 23 | self.wire_protocol = wire_protocol 24 | self.output = output 25 | 26 | def remove_include_files(target_directory): 27 | search_pattern = target_directory+'/*.h' 28 | print "search pattern is", search_pattern 29 | files_to_remove = glob.glob(search_pattern) 30 | for afile in files_to_remove : 31 | try: 32 | print "removing", afile 33 | os.remove(afile) 34 | except: 35 | print "error while trying to remove", afile 36 | 37 | def copy_include_files(source_directory,target_directory): 38 | search_pattern = source_directory+'/*.h' 39 | files_to_copy = glob.glob(search_pattern) 40 | for afile in files_to_copy: 41 | basename = os.path.basename(afile) 42 | print "Copying ...", basename 43 | copy(afile, target_directory) 44 | 45 | protocol = "1.0" 46 | 47 | xml_directory = './message_definitions/v'+protocol 48 | print "xml_directory is", xml_directory 49 | xml_file_names = [] 50 | xml_file_names.append(xml_directory+"/"+"matrixpilot.xml") 51 | 52 | for xml_file in xml_file_names: 53 | print "xml file is ", xml_file 54 | opts = options(lang = "C", output = "C/include_v"+protocol, \ 55 | wire_protocol=protocol) 56 | args = [] 57 | args.append(xml_file) 58 | mavgen(opts, args) 59 | xml_file_base = os.path.basename(xml_file) 60 | xml_file_base = re.sub("\.xml","", xml_file_base) 61 | print "xml_file_base is", xml_file_base 62 | opts = options(lang = "python", \ 63 | output="python/mavlink_"+xml_file_base+"_v"+protocol+".py", \ 64 | wire_protocol=protocol) 65 | mavgen(opts,args) 66 | 67 | mavlink_directory_list = ["common","matrixpilot"] 68 | for mavlink_directory in mavlink_directory_list : 69 | # Look specifically for MatrixPilot directory structure 70 | target_directory = "../../../../MAVLink/include/"+mavlink_directory 71 | source_directory = "C/include_v"+protocol+"/"+mavlink_directory 72 | if os.access(source_directory, os.R_OK): 73 | if os.access(target_directory, os.W_OK): 74 | print "Preparing to copy over files..." 75 | print "About to remove all files in",target_directory 76 | print "OK to continue ?[Yes / No]: ", 77 | line = sys.stdin.readline() 78 | if line == "Yes\n" or line == "yes\n" \ 79 | or line == "Y\n" or line == "y\n": 80 | print "passed" 81 | remove_include_files(target_directory) 82 | copy_include_files(source_directory,target_directory) 83 | print "Finished copying over include files" 84 | else : 85 | print "Your answer is No. Exiting Program" 86 | sys.exit() 87 | else : 88 | print "Cannot find " + target_directory + "in MatrixPilot" 89 | sys.exit() 90 | else: 91 | print "Could not find files to copy at", source_directory 92 | print "Exiting Program." 93 | sys.exit() 94 | -------------------------------------------------------------------------------- /px_comm/include/px_comm/DDSImage.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | /* 4 | WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. 5 | 6 | This file was generated from DDSImage.idl using "rtiddsgen". 7 | The rtiddsgen tool is part of the RTI Connext distribution. 8 | For more information, type 'rtiddsgen -help' at a command shell 9 | or consult the RTI Connext manual. 10 | */ 11 | 12 | #ifndef DDSImage_248003145_h 13 | #define DDSImage_248003145_h 14 | 15 | #ifndef NDDS_STANDALONE_TYPE 16 | #ifdef __cplusplus 17 | #ifndef ndds_cpp_h 18 | #include "ndds/ndds_cpp.h" 19 | #endif 20 | #else 21 | #ifndef ndds_c_h 22 | #include "ndds/ndds_c.h" 23 | #endif 24 | #endif 25 | #else 26 | #include "ndds_standalone_type.h" 27 | #endif 28 | 29 | namespace px_comm { 30 | 31 | extern const char *DDSImageTYPENAME; 32 | 33 | #ifdef __cplusplus 34 | struct DDSImageSeq; 35 | #ifndef NDDS_STANDALONE_TYPE 36 | class DDSImageTypeSupport; 37 | class DDSImageDataWriter; 38 | class DDSImageDataReader; 39 | #endif 40 | #endif 41 | 42 | class DDSImage 43 | { 44 | public: 45 | #ifdef __cplusplus 46 | typedef struct DDSImageSeq Seq; 47 | #ifndef NDDS_STANDALONE_TYPE 48 | typedef DDSImageTypeSupport TypeSupport; 49 | typedef DDSImageDataWriter DataWriter; 50 | typedef DDSImageDataReader DataReader; 51 | #endif 52 | 53 | #endif 54 | 55 | DDS_UnsignedLong seq ; 56 | DDS_Long stamp_sec ; 57 | DDS_Long stamp_nsec ; 58 | DDS_Char * frame_id ; 59 | DDS_UnsignedLong height ; 60 | DDS_UnsignedLong width ; 61 | DDS_Char * encoding ; 62 | DDS_Char is_bigendian ; 63 | DDS_UnsignedLong step ; 64 | DDS_CharSeq data ; 65 | }; 66 | #if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) 67 | /* If the code is building on Windows, start exporting symbols. 68 | */ 69 | #undef NDDSUSERDllExport 70 | #define NDDSUSERDllExport __declspec(dllexport) 71 | #endif 72 | 73 | NDDSUSERDllExport DDS_TypeCode* DDSImage_get_typecode(void); /* Type code */ 74 | 75 | DDS_SEQUENCE(DDSImageSeq, DDSImage); 76 | 77 | NDDSUSERDllExport 78 | RTIBool DDSImage_initialize( 79 | DDSImage* self); 80 | 81 | NDDSUSERDllExport 82 | RTIBool DDSImage_initialize_ex( 83 | DDSImage* self,RTIBool allocatePointers,RTIBool allocateMemory); 84 | 85 | NDDSUSERDllExport 86 | RTIBool DDSImage_initialize_w_params( 87 | DDSImage* self, 88 | const struct DDS_TypeAllocationParams_t * allocParams); 89 | 90 | NDDSUSERDllExport 91 | void DDSImage_finalize( 92 | DDSImage* self); 93 | 94 | NDDSUSERDllExport 95 | void DDSImage_finalize_ex( 96 | DDSImage* self,RTIBool deletePointers); 97 | 98 | NDDSUSERDllExport 99 | void DDSImage_finalize_w_params( 100 | DDSImage* self, 101 | const struct DDS_TypeDeallocationParams_t * deallocParams); 102 | 103 | NDDSUSERDllExport 104 | void DDSImage_finalize_optional_members( 105 | DDSImage* self, RTIBool deletePointers); 106 | 107 | NDDSUSERDllExport 108 | RTIBool DDSImage_copy( 109 | DDSImage* dst, 110 | const DDSImage* src); 111 | 112 | #if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) 113 | /* If the code is building on Windows, stop exporting symbols. 114 | */ 115 | #undef NDDSUSERDllExport 116 | #define NDDSUSERDllExport 117 | #endif 118 | } /* namespace px_comm */ 119 | 120 | #endif /* DDSImage */ 121 | 122 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/generator/mavtestgen.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | ''' 3 | generate a MAVLink test suite 4 | 5 | Copyright Andrew Tridgell 2011 6 | Released under GNU GPL version 3 or later 7 | ''' 8 | 9 | import sys, textwrap 10 | from optparse import OptionParser 11 | 12 | # mavparse is up a directory level 13 | sys.path.append('..') 14 | import mavparse 15 | 16 | def gen_value(f, i, language): 17 | '''generate a test value for the ith field of a message''' 18 | type = f.type 19 | 20 | # could be an array 21 | if type.find("[") != -1: 22 | aidx = type.find("[") 23 | basetype = type[0:aidx] 24 | if basetype == "array": 25 | basetype = "int8_t" 26 | if language == 'C': 27 | return '(const %s *)"%s%u"' % (basetype, f.name, i) 28 | return '"%s%u"' % (f.name, i) 29 | 30 | if type == 'float': 31 | return 17.0 + i*7 32 | if type == 'char': 33 | return 'A' + i 34 | if type == 'int8_t': 35 | return 5 + i 36 | if type in ['int8_t', 'uint8_t']: 37 | return 5 + i 38 | if type in ['uint8_t_mavlink_version']: 39 | return 2 40 | if type in ['int16_t', 'uint16_t']: 41 | return 17235 + i*52 42 | if type in ['int32_t', 'uint32_t']: 43 | v = 963497464 + i*52 44 | if language == 'C': 45 | return "%sL" % v 46 | return v 47 | if type in ['int64_t', 'uint64_t']: 48 | v = 9223372036854775807 + i*63 49 | if language == 'C': 50 | return "%sLL" % v 51 | return v 52 | 53 | 54 | 55 | def generate_methods_python(outf, msgs): 56 | outf.write(""" 57 | ''' 58 | MAVLink protocol test implementation (auto-generated by mavtestgen.py) 59 | 60 | Generated from: %s 61 | 62 | Note: this file has been auto-generated. DO NOT EDIT 63 | ''' 64 | 65 | import mavlink 66 | 67 | def generate_outputs(mav): 68 | '''generate all message types as outputs''' 69 | """) 70 | for m in msgs: 71 | if m.name == "HEARTBEAT": continue 72 | outf.write("\tmav.%s_send(" % m.name.lower()) 73 | for i in range(0, len(m.fields)): 74 | f = m.fields[i] 75 | outf.write("%s=%s" % (f.name, gen_value(f, i, 'py'))) 76 | if i != len(m.fields)-1: 77 | outf.write(",") 78 | outf.write(")\n") 79 | 80 | 81 | def generate_methods_C(outf, msgs): 82 | outf.write(""" 83 | /* 84 | MAVLink protocol test implementation (auto-generated by mavtestgen.py) 85 | 86 | Generated from: %s 87 | 88 | Note: this file has been auto-generated. DO NOT EDIT 89 | */ 90 | 91 | static void mavtest_generate_outputs(mavlink_channel_t chan) 92 | { 93 | """) 94 | for m in msgs: 95 | if m.name == "HEARTBEAT": continue 96 | outf.write("\tmavlink_msg_%s_send(chan," % m.name.lower()) 97 | for i in range(0, len(m.fields)): 98 | f = m.fields[i] 99 | outf.write("%s" % gen_value(f, i, 'C')) 100 | if i != len(m.fields)-1: 101 | outf.write(",") 102 | outf.write(");\n") 103 | outf.write("}\n") 104 | 105 | 106 | 107 | ###################################################################### 108 | '''main program''' 109 | 110 | parser = OptionParser("%prog [options] ") 111 | parser.add_option("-o", "--output", dest="output", default="mavtest", help="output folder [default: %default]") 112 | (opts, args) = parser.parse_args() 113 | 114 | if len(args) < 1: 115 | parser.error("You must supply at least one MAVLink XML protocol definition") 116 | 117 | 118 | msgs = [] 119 | enums = [] 120 | 121 | for fname in args: 122 | (m, e) = mavparse.parse_mavlink_xml(fname) 123 | msgs.extend(m) 124 | enums.extend(e) 125 | 126 | 127 | if mavparse.check_duplicates(msgs): 128 | sys.exit(1) 129 | 130 | print("Found %u MAVLink message types" % len(msgs)) 131 | 132 | print("Generating python %s" % (opts.output+'.py')) 133 | outf = open(opts.output + '.py', "w") 134 | generate_methods_python(outf, msgs) 135 | outf.close() 136 | 137 | print("Generating C %s" % (opts.output+'.h')) 138 | outf = open(opts.output + '.h', "w") 139 | generate_methods_C(outf, msgs) 140 | outf.close() 141 | 142 | print("Generated %s OK" % opts.output) 143 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/examples/magtest.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | rotate APMs on bench to test magnetometers 5 | 6 | ''' 7 | 8 | import sys, os, time 9 | from math import radians 10 | 11 | # allow import from the parent directory, where mavlink.py is 12 | sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) 13 | 14 | import mavlink, mavutil 15 | 16 | from optparse import OptionParser 17 | parser = OptionParser("rotate.py [options]") 18 | 19 | parser.add_option("--device1", dest="device1", default=None, help="mavlink device1") 20 | parser.add_option("--device2", dest="device2", default=None, help="mavlink device2") 21 | parser.add_option("--baudrate", dest="baudrate", type='int', 22 | help="master port baud rate", default=115200) 23 | (opts, args) = parser.parse_args() 24 | 25 | if opts.device1 is None or opts.device2 is None: 26 | print("You must specify a mavlink device") 27 | sys.exit(1) 28 | 29 | def set_attitude(rc3, rc4): 30 | global mav1, mav2 31 | values = [ 65535 ] * 8 32 | values[2] = rc3 33 | values[3] = rc4 34 | mav1.mav.rc_channels_override_send(mav1.target_system, mav1.target_component, *values) 35 | mav2.mav.rc_channels_override_send(mav2.target_system, mav2.target_component, *values) 36 | 37 | 38 | # create a mavlink instance 39 | mav1 = mavutil.mavlink_connection(opts.device1, baud=opts.baudrate) 40 | 41 | # create a mavlink instance 42 | mav2 = mavutil.mavlink_connection(opts.device2, baud=opts.baudrate) 43 | 44 | print("Waiting for HEARTBEAT") 45 | mav1.wait_heartbeat() 46 | mav2.wait_heartbeat() 47 | print("Heartbeat from APM (system %u component %u)" % (mav1.target_system, mav1.target_system)) 48 | print("Heartbeat from APM (system %u component %u)" % (mav2.target_system, mav2.target_system)) 49 | 50 | print("Waiting for MANUAL mode") 51 | mav1.recv_match(type='SYS_STATUS', condition='SYS_STATUS.mode==2 and SYS_STATUS.nav_mode==4', blocking=True) 52 | mav2.recv_match(type='SYS_STATUS', condition='SYS_STATUS.mode==2 and SYS_STATUS.nav_mode==4', blocking=True) 53 | 54 | print("Setting declination") 55 | mav1.mav.param_set_send(mav1.target_system, mav1.target_component, 56 | 'COMPASS_DEC', radians(12.33)) 57 | mav2.mav.param_set_send(mav2.target_system, mav2.target_component, 58 | 'COMPASS_DEC', radians(12.33)) 59 | 60 | 61 | set_attitude(1060, 1160) 62 | 63 | event = mavutil.periodic_event(30) 64 | pevent = mavutil.periodic_event(0.3) 65 | rc3_min = 1060 66 | rc3_max = 1850 67 | rc4_min = 1080 68 | rc4_max = 1500 69 | rc3 = rc3_min 70 | rc4 = 1160 71 | delta3 = 2 72 | delta4 = 1 73 | use_pitch = 1 74 | 75 | MAV_ACTION_CALIBRATE_GYRO = 17 76 | mav1.mav.action_send(mav1.target_system, mav1.target_component, MAV_ACTION_CALIBRATE_GYRO) 77 | mav2.mav.action_send(mav2.target_system, mav2.target_component, MAV_ACTION_CALIBRATE_GYRO) 78 | 79 | print("Waiting for gyro calibration") 80 | mav1.recv_match(type='ACTION_ACK') 81 | mav2.recv_match(type='ACTION_ACK') 82 | 83 | print("Resetting mag offsets") 84 | mav1.mav.set_mag_offsets_send(mav1.target_system, mav1.target_component, 0, 0, 0) 85 | mav2.mav.set_mag_offsets_send(mav2.target_system, mav2.target_component, 0, 0, 0) 86 | 87 | def TrueHeading(SERVO_OUTPUT_RAW): 88 | p = float(SERVO_OUTPUT_RAW.servo3_raw - rc3_min) / (rc3_max - rc3_min) 89 | return 172 + p*(326 - 172) 90 | 91 | while True: 92 | mav1.recv_msg() 93 | mav2.recv_msg() 94 | if event.trigger(): 95 | if not use_pitch: 96 | rc4 = 1160 97 | set_attitude(rc3, rc4) 98 | rc3 += delta3 99 | if rc3 > rc3_max or rc3 < rc3_min: 100 | delta3 = -delta3 101 | use_pitch ^= 1 102 | rc4 += delta4 103 | if rc4 > rc4_max or rc4 < rc4_min: 104 | delta4 = -delta4 105 | if pevent.trigger(): 106 | print "hdg1: %3u hdg2: %3u ofs1: %4u, %4u, %4u ofs2: %4u, %4u, %4u" % ( 107 | mav1.messages['VFR_HUD'].heading, 108 | mav2.messages['VFR_HUD'].heading, 109 | mav1.messages['SENSOR_OFFSETS'].mag_ofs_x, 110 | mav1.messages['SENSOR_OFFSETS'].mag_ofs_y, 111 | mav1.messages['SENSOR_OFFSETS'].mag_ofs_z, 112 | mav2.messages['SENSOR_OFFSETS'].mag_ofs_x, 113 | mav2.messages['SENSOR_OFFSETS'].mag_ofs_y, 114 | mav2.messages['SENSOR_OFFSETS'].mag_ofs_z, 115 | ) 116 | time.sleep(0.01) 117 | 118 | # 314M 326G 119 | # 160M 172G 120 | 121 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/test.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol generated from test.xml 3 | * @see http://qgroundcontrol.org/mavlink/ 4 | */ 5 | #ifndef TEST_H 6 | #define TEST_H 7 | 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | // MESSAGE LENGTHS AND CRCS 13 | 14 | #ifndef MAVLINK_MESSAGE_LENGTHS 15 | #define MAVLINK_MESSAGE_LENGTHS {179, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} 16 | #endif 17 | 18 | #ifndef MAVLINK_MESSAGE_CRCS 19 | #define MAVLINK_MESSAGE_CRCS {91, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} 20 | #endif 21 | 22 | #ifndef MAVLINK_MESSAGE_INFO 23 | #define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_TEST_TYPES, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}} 24 | #endif 25 | 26 | #include "../protocol.h" 27 | 28 | #define MAVLINK_ENABLED_TEST 29 | 30 | 31 | 32 | // MAVLINK VERSION 33 | 34 | #ifndef MAVLINK_VERSION 35 | #define MAVLINK_VERSION 3 36 | #endif 37 | 38 | #if (MAVLINK_VERSION == 0) 39 | #undef MAVLINK_VERSION 40 | #define MAVLINK_VERSION 3 41 | #endif 42 | 43 | // ENUM DEFINITIONS 44 | 45 | 46 | 47 | // MESSAGE DEFINITIONS 48 | #include "./mavlink_msg_test_types.h" 49 | 50 | #ifdef __cplusplus 51 | } 52 | #endif // __cplusplus 53 | #endif // TEST_H 54 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/test.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol generated from test.xml 3 | * @see http://qgroundcontrol.org/mavlink/ 4 | */ 5 | #ifndef TEST_H 6 | #define TEST_H 7 | 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | // MESSAGE LENGTHS AND CRCS 13 | 14 | #ifndef MAVLINK_MESSAGE_LENGTHS 15 | #define MAVLINK_MESSAGE_LENGTHS {179, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} 16 | #endif 17 | 18 | #ifndef MAVLINK_MESSAGE_CRCS 19 | #define MAVLINK_MESSAGE_CRCS {103, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} 20 | #endif 21 | 22 | #ifndef MAVLINK_MESSAGE_INFO 23 | #define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_TEST_TYPES, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}} 24 | #endif 25 | 26 | #include "../protocol.h" 27 | 28 | #define MAVLINK_ENABLED_TEST 29 | 30 | 31 | 32 | // MAVLINK VERSION 33 | 34 | #ifndef MAVLINK_VERSION 35 | #define MAVLINK_VERSION 3 36 | #endif 37 | 38 | #if (MAVLINK_VERSION == 0) 39 | #undef MAVLINK_VERSION 40 | #define MAVLINK_VERSION 3 41 | #endif 42 | 43 | // ENUM DEFINITIONS 44 | 45 | 46 | 47 | // MESSAGE DEFINITIONS 48 | #include "./mavlink_msg_test_types.h" 49 | 50 | #ifdef __cplusplus 51 | } 52 | #endif // __cplusplus 53 | #endif // TEST_H 54 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/examples/magfit.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | fit best estimate of magnetometer offsets 5 | ''' 6 | 7 | import sys, time, os, math 8 | 9 | # allow import from the parent directory, where mavlink.py is 10 | sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) 11 | 12 | from optparse import OptionParser 13 | parser = OptionParser("magfit.py [options]") 14 | parser.add_option("--no-timestamps",dest="notimestamps", action='store_true', help="Log doesn't have timestamps") 15 | parser.add_option("--condition",dest="condition", default=None, help="select packets by condition") 16 | parser.add_option("--noise", type='float', default=0, help="noise to add") 17 | parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") 18 | 19 | (opts, args) = parser.parse_args() 20 | 21 | if opts.mav10: 22 | os.environ['MAVLINK10'] = '1' 23 | import mavutil 24 | from rotmat import Vector3 25 | 26 | if len(args) < 1: 27 | print("Usage: magfit.py [options] ") 28 | sys.exit(1) 29 | 30 | def noise(): 31 | '''a noise vector''' 32 | from random import gauss 33 | v = Vector3(gauss(0, 1), gauss(0, 1), gauss(0, 1)) 34 | v.normalize() 35 | return v * opts.noise 36 | 37 | def select_data(data): 38 | ret = [] 39 | counts = {} 40 | for d in data: 41 | mag = d 42 | key = "%u:%u:%u" % (mag.x/20,mag.y/20,mag.z/20) 43 | if key in counts: 44 | counts[key] += 1 45 | else: 46 | counts[key] = 1 47 | if counts[key] < 3: 48 | ret.append(d) 49 | print(len(data), len(ret)) 50 | return ret 51 | 52 | def radius(mag, offsets): 53 | '''return radius give data point and offsets''' 54 | return (mag + offsets).length() 55 | 56 | def radius_cmp(a, b, offsets): 57 | '''return radius give data point and offsets''' 58 | diff = radius(a, offsets) - radius(b, offsets) 59 | if diff > 0: 60 | return 1 61 | if diff < 0: 62 | return -1 63 | return 0 64 | 65 | def sphere_error(p, data): 66 | from scipy import sqrt 67 | x,y,z,r = p 68 | ofs = Vector3(x,y,z) 69 | ret = [] 70 | for d in data: 71 | mag = d 72 | err = r - radius(mag, ofs) 73 | ret.append(err) 74 | return ret 75 | 76 | def fit_data(data): 77 | import numpy, scipy 78 | from scipy import optimize 79 | 80 | p0 = [0.0, 0.0, 0.0, 0.0] 81 | p1, ier = optimize.leastsq(sphere_error, p0[:], args=(data)) 82 | if not ier in [1, 2, 3, 4]: 83 | raise RuntimeError("Unable to find solution") 84 | return (Vector3(p1[0], p1[1], p1[2]), p1[3]) 85 | 86 | def magfit(logfile): 87 | '''find best magnetometer offset fit to a log file''' 88 | 89 | print("Processing log %s" % filename) 90 | mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps) 91 | 92 | data = [] 93 | 94 | last_t = 0 95 | offsets = Vector3(0,0,0) 96 | 97 | # now gather all the data 98 | while True: 99 | m = mlog.recv_match(condition=opts.condition) 100 | if m is None: 101 | break 102 | if m.get_type() == "SENSOR_OFFSETS": 103 | # update current offsets 104 | offsets = Vector3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z) 105 | if m.get_type() == "RAW_IMU": 106 | mag = Vector3(m.xmag, m.ymag, m.zmag) 107 | # add data point after subtracting the current offsets 108 | data.append(mag - offsets + noise()) 109 | 110 | print("Extracted %u data points" % len(data)) 111 | print("Current offsets: %s" % offsets) 112 | 113 | data = select_data(data) 114 | 115 | # do an initial fit with all data 116 | (offsets, field_strength) = fit_data(data) 117 | 118 | for count in range(3): 119 | # sort the data by the radius 120 | data.sort(lambda a,b : radius_cmp(a,b,offsets)) 121 | 122 | print("Fit %u : %s field_strength=%6.1f to %6.1f" % ( 123 | count, offsets, 124 | radius(data[0], offsets), radius(data[-1], offsets))) 125 | 126 | # discard outliers, keep the middle 3/4 127 | data = data[len(data)/8:-len(data)/8] 128 | 129 | # fit again 130 | (offsets, field_strength) = fit_data(data) 131 | 132 | print("Final : %s field_strength=%6.1f to %6.1f" % ( 133 | offsets, 134 | radius(data[0], offsets), radius(data[-1], offsets))) 135 | 136 | total = 0.0 137 | for filename in args: 138 | magfit(filename) 139 | -------------------------------------------------------------------------------- /mavlink/include/mavlink/v1.0/mavlink_conversions.h: -------------------------------------------------------------------------------- 1 | #ifndef _MAVLINK_CONVERSIONS_H_ 2 | #define _MAVLINK_CONVERSIONS_H_ 3 | 4 | /* enable math defines on Windows */ 5 | #ifdef _MSC_VER 6 | #ifndef _USE_MATH_DEFINES 7 | #define _USE_MATH_DEFINES 8 | #endif 9 | #endif 10 | #include 11 | 12 | #ifndef M_PI_2 13 | #define M_PI_2 ((float)asin(1)) 14 | #endif 15 | 16 | /** 17 | * @file mavlink_conversions.h 18 | * 19 | * These conversion functions follow the NASA rotation standards definition file 20 | * available online. 21 | * 22 | * Their intent is to lower the barrier for MAVLink adopters to use gimbal-lock free 23 | * (both rotation matrices, sometimes called DCM, and quaternions are gimbal-lock free) 24 | * rotation representations. Euler angles (roll, pitch, yaw) will be phased out of the 25 | * protocol as widely as possible. 26 | * 27 | * @author James Goppert 28 | */ 29 | 30 | MAVLINK_HELPER void mavlink_quaternion_to_dcm(const float quaternion[4], float dcm[3][3]) 31 | { 32 | double a = quaternion[0]; 33 | double b = quaternion[1]; 34 | double c = quaternion[2]; 35 | double d = quaternion[3]; 36 | double aSq = a * a; 37 | double bSq = b * b; 38 | double cSq = c * c; 39 | double dSq = d * d; 40 | dcm[0][0] = aSq + bSq - cSq - dSq; 41 | dcm[0][1] = 2.0 * (b * c - a * d); 42 | dcm[0][2] = 2.0 * (a * c + b * d); 43 | dcm[1][0] = 2.0 * (b * c + a * d); 44 | dcm[1][1] = aSq - bSq + cSq - dSq; 45 | dcm[1][2] = 2.0 * (c * d - a * b); 46 | dcm[2][0] = 2.0 * (b * d - a * c); 47 | dcm[2][1] = 2.0 * (a * b + c * d); 48 | dcm[2][2] = aSq - bSq - cSq + dSq; 49 | } 50 | 51 | MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, float* pitch, float* yaw) 52 | { 53 | float phi, theta, psi; 54 | theta = asin(-dcm[2][0]); 55 | 56 | if (fabsf(theta - (float)M_PI_2) < 1.0e-3f) { 57 | phi = 0.0f; 58 | psi = (atan2f(dcm[1][2] - dcm[0][1], 59 | dcm[0][2] + dcm[1][1]) + phi); 60 | 61 | } else if (fabsf(theta + (float)M_PI_2) < 1.0e-3f) { 62 | phi = 0.0f; 63 | psi = atan2f(dcm[1][2] - dcm[0][1], 64 | dcm[0][2] + dcm[1][1] - phi); 65 | 66 | } else { 67 | phi = atan2f(dcm[2][1], dcm[2][2]); 68 | psi = atan2f(dcm[1][0], dcm[0][0]); 69 | } 70 | 71 | *roll = phi; 72 | *pitch = theta; 73 | *yaw = psi; 74 | } 75 | 76 | MAVLINK_HELPER void mavlink_quaternion_to_euler(const float quaternion[4], float* roll, float* pitch, float* yaw) 77 | { 78 | float dcm[3][3]; 79 | mavlink_quaternion_to_dcm(quaternion, dcm); 80 | mavlink_dcm_to_euler(dcm, roll, pitch, yaw); 81 | } 82 | 83 | MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float yaw, float quaternion[4]) 84 | { 85 | double cosPhi_2 = cos((double)roll / 2.0); 86 | double sinPhi_2 = sin((double)roll / 2.0); 87 | double cosTheta_2 = cos((double)pitch / 2.0); 88 | double sinTheta_2 = sin((double)pitch / 2.0); 89 | double cosPsi_2 = cos((double)yaw / 2.0); 90 | double sinPsi_2 = sin((double)yaw / 2.0); 91 | quaternion[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 + 92 | sinPhi_2 * sinTheta_2 * sinPsi_2); 93 | quaternion[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 - 94 | cosPhi_2 * sinTheta_2 * sinPsi_2); 95 | quaternion[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 + 96 | sinPhi_2 * cosTheta_2 * sinPsi_2); 97 | quaternion[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 - 98 | sinPhi_2 * sinTheta_2 * cosPsi_2); 99 | } 100 | 101 | MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quaternion[4]) 102 | { 103 | quaternion[0] = (0.5 * sqrt(1.0 + 104 | (double)(dcm[0][0] + dcm[1][1] + dcm[2][2]))); 105 | quaternion[1] = (0.5 * sqrt(1.0 + 106 | (double)(dcm[0][0] - dcm[1][1] - dcm[2][2]))); 107 | quaternion[2] = (0.5 * sqrt(1.0 + 108 | (double)(-dcm[0][0] + dcm[1][1] - dcm[2][2]))); 109 | quaternion[3] = (0.5 * sqrt(1.0 + 110 | (double)(-dcm[0][0] - dcm[1][1] + dcm[2][2]))); 111 | } 112 | 113 | MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, float dcm[3][3]) 114 | { 115 | double cosPhi = cos(roll); 116 | double sinPhi = sin(roll); 117 | double cosThe = cos(pitch); 118 | double sinThe = sin(pitch); 119 | double cosPsi = cos(yaw); 120 | double sinPsi = sin(yaw); 121 | 122 | dcm[0][0] = cosThe * cosPsi; 123 | dcm[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; 124 | dcm[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi; 125 | 126 | dcm[1][0] = cosThe * sinPsi; 127 | dcm[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi; 128 | dcm[1][2] = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi; 129 | 130 | dcm[2][0] = -sinThe; 131 | dcm[2][1] = sinPhi * cosThe; 132 | dcm[2][2] = cosPhi * cosThe; 133 | } 134 | 135 | #endif 136 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/mavextra.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | ''' 3 | useful extra functions for use by mavlink clients 4 | 5 | Copyright Andrew Tridgell 2011 6 | Released under GNU GPL version 3 or later 7 | ''' 8 | 9 | from math import * 10 | 11 | 12 | def kmh(mps): 13 | '''convert m/s to Km/h''' 14 | return mps*3.6 15 | 16 | def altitude(press_abs, ground_press=955.0, ground_temp=30): 17 | '''calculate barometric altitude''' 18 | return log(ground_press/press_abs)*(ground_temp+273.15)*29271.267*0.001 19 | 20 | 21 | def mag_heading(RAW_IMU, ATTITUDE, declination=0, SENSOR_OFFSETS=None, ofs=None): 22 | '''calculate heading from raw magnetometer''' 23 | mag_x = RAW_IMU.xmag 24 | mag_y = RAW_IMU.ymag 25 | mag_z = RAW_IMU.zmag 26 | if SENSOR_OFFSETS is not None and ofs is not None: 27 | mag_x += ofs[0] - SENSOR_OFFSETS.mag_ofs_x 28 | mag_y += ofs[1] - SENSOR_OFFSETS.mag_ofs_y 29 | mag_z += ofs[2] - SENSOR_OFFSETS.mag_ofs_z 30 | 31 | headX = mag_x*cos(ATTITUDE.pitch) + mag_y*sin(ATTITUDE.roll)*sin(ATTITUDE.pitch) + mag_z*cos(ATTITUDE.roll)*sin(ATTITUDE.pitch) 32 | headY = mag_y*cos(ATTITUDE.roll) - mag_z*sin(ATTITUDE.roll) 33 | heading = degrees(atan2(-headY,headX)) + declination 34 | if heading < 0: 35 | heading += 360 36 | return heading 37 | 38 | def mag_field(RAW_IMU, SENSOR_OFFSETS=None, ofs=None): 39 | '''calculate magnetic field strength from raw magnetometer''' 40 | mag_x = RAW_IMU.xmag 41 | mag_y = RAW_IMU.ymag 42 | mag_z = RAW_IMU.zmag 43 | if SENSOR_OFFSETS is not None and ofs is not None: 44 | mag_x += ofs[0] - SENSOR_OFFSETS.mag_ofs_x 45 | mag_y += ofs[1] - SENSOR_OFFSETS.mag_ofs_y 46 | mag_z += ofs[2] - SENSOR_OFFSETS.mag_ofs_z 47 | return sqrt(mag_x**2 + mag_y**2 + mag_z**2) 48 | 49 | def angle_diff(angle1, angle2): 50 | '''show the difference between two angles in degrees''' 51 | ret = angle1 - angle2 52 | if ret > 180: 53 | ret -= 360; 54 | if ret < -180: 55 | ret += 360 56 | return ret 57 | 58 | 59 | lowpass_data = {} 60 | 61 | def lowpass(var, key, factor): 62 | '''a simple lowpass filter''' 63 | global lowpass_data 64 | if not key in lowpass_data: 65 | lowpass_data[key] = var 66 | else: 67 | lowpass_data[key] = factor*lowpass_data[key] + (1.0 - factor)*var 68 | return lowpass_data[key] 69 | 70 | last_delta = {} 71 | 72 | def delta(var, key): 73 | '''calculate slope''' 74 | global last_delta 75 | dv = 0 76 | if key in last_delta: 77 | dv = var - last_delta[key] 78 | last_delta[key] = var 79 | return dv 80 | 81 | def delta_angle(var, key): 82 | '''calculate slope of an angle''' 83 | global last_delta 84 | dv = 0 85 | if key in last_delta: 86 | dv = var - last_delta[key] 87 | last_delta[key] = var 88 | if dv > 180: 89 | dv -= 360 90 | if dv < -180: 91 | dv += 360 92 | return dv 93 | 94 | def roll_estimate(RAW_IMU,smooth=0.7): 95 | '''estimate roll from accelerometer''' 96 | rx = lowpass(RAW_IMU.xacc,'rx',smooth) 97 | ry = lowpass(RAW_IMU.yacc,'ry',smooth) 98 | rz = lowpass(RAW_IMU.zacc,'rz',smooth) 99 | return degrees(-asin(ry/sqrt(rx**2+ry**2+rz**2))) 100 | 101 | def pitch_estimate(RAW_IMU, smooth=0.7): 102 | '''estimate pitch from accelerometer''' 103 | rx = lowpass(RAW_IMU.xacc,'rx',smooth) 104 | ry = lowpass(RAW_IMU.yacc,'ry',smooth) 105 | rz = lowpass(RAW_IMU.zacc,'rz',smooth) 106 | return degrees(asin(rx/sqrt(rx**2+ry**2+rz**2))) 107 | 108 | def gravity(RAW_IMU, SENSOR_OFFSETS=None, ofs=None, smooth=0.7): 109 | '''estimate pitch from accelerometer''' 110 | rx = RAW_IMU.xacc 111 | ry = RAW_IMU.yacc 112 | rz = RAW_IMU.zacc+45 113 | if SENSOR_OFFSETS is not None and ofs is not None: 114 | rx += ofs[0] - SENSOR_OFFSETS.accel_cal_x 115 | ry += ofs[1] - SENSOR_OFFSETS.accel_cal_y 116 | rz += ofs[2] - SENSOR_OFFSETS.accel_cal_z 117 | return lowpass(sqrt(rx**2+ry**2+rz**2)*0.01,'_gravity',smooth) 118 | 119 | 120 | 121 | def pitch_sim(SIMSTATE, GPS_RAW): 122 | '''estimate pitch from SIMSTATE accels''' 123 | xacc = SIMSTATE.xacc - lowpass(delta(GPS_RAW.v,"v")*6.6, "v", 0.9) 124 | zacc = SIMSTATE.zacc 125 | zacc += SIMSTATE.ygyro * GPS_RAW.v; 126 | if xacc/zacc >= 1: 127 | return 0 128 | if xacc/zacc <= -1: 129 | return -0 130 | return degrees(-asin(xacc/zacc)) 131 | 132 | def distance_two(GPS_RAW1, GPS_RAW2): 133 | '''distance between two points''' 134 | lat1 = radians(GPS_RAW1.lat) 135 | lat2 = radians(GPS_RAW2.lat) 136 | lon1 = radians(GPS_RAW1.lon) 137 | lon2 = radians(GPS_RAW2.lon) 138 | dLat = lat2 - lat1 139 | dLon = lon2 - lon1 140 | 141 | a = sin(0.5*dLat) * sin(0.5*dLat) + sin(0.5*dLon) * sin(0.5*dLon) * cos(lat1) * cos(lat2) 142 | c = 2.0 * atan2(sqrt(a), sqrt(1.0-a)) 143 | return 6371 * 1000 * c 144 | 145 | 146 | first_fix = None 147 | 148 | def distance_home(GPS_RAW): 149 | '''distance from first fix point''' 150 | global first_fix 151 | if first_fix == None or first_fix.fix_type < 2: 152 | first_fix = GPS_RAW 153 | return 0 154 | return distance_two(GPS_RAW, first_fix) 155 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/examples/magfit_gps.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | fit best estimate of magnetometer offsets 5 | ''' 6 | 7 | import sys, time, os, math 8 | 9 | # allow import from the parent directory, where mavlink.py is 10 | sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) 11 | 12 | from optparse import OptionParser 13 | parser = OptionParser("magfit.py [options]") 14 | parser.add_option("--no-timestamps",dest="notimestamps", action='store_true', help="Log doesn't have timestamps") 15 | parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") 16 | parser.add_option("--minspeed", type='float', default=5.0, help="minimum ground speed to use") 17 | 18 | (opts, args) = parser.parse_args() 19 | 20 | if opts.mav10: 21 | os.environ['MAVLINK10'] = '1' 22 | import mavutil 23 | 24 | if len(args) < 1: 25 | print("Usage: magfit.py [options] ") 26 | sys.exit(1) 27 | 28 | class vec3(object): 29 | def __init__(self, x, y, z): 30 | self.x = x 31 | self.y = y 32 | self.z = z 33 | def __str__(self): 34 | return "%.1f %.1f %.1f" % (self.x, self.y, self.z) 35 | 36 | def heading_error1(parm, data): 37 | from math import sin, cos, atan2, degrees 38 | from numpy import dot 39 | xofs,yofs,zofs,a1,a2,a3,a4,a5,a6,a7,a8,a9,declination = parm 40 | 41 | ret = [] 42 | for d in data: 43 | x = d[0] + xofs 44 | y = d[1] + yofs 45 | z = d[2] + zofs 46 | r = d[3] 47 | p = d[4] 48 | h = d[5] 49 | 50 | headX = x*cos(p) + y*sin(r)*sin(p) + z*cos(r)*sin(p) 51 | headY = y*cos(r) - z*sin(r) 52 | heading = degrees(atan2(-headY,headX)) + declination 53 | if heading < 0: 54 | heading += 360 55 | herror = h - heading 56 | if herror > 180: 57 | herror -= 360 58 | if herror < -180: 59 | herror += 360 60 | ret.append(herror) 61 | return ret 62 | 63 | def heading_error(parm, data): 64 | from math import sin, cos, atan2, degrees 65 | from numpy import dot 66 | xofs,yofs,zofs,a1,a2,a3,a4,a5,a6,a7,a8,a9,declination = parm 67 | 68 | a = [[1.0,a2,a3],[a4,a5,a6],[a7,a8,a9]] 69 | 70 | ret = [] 71 | for d in data: 72 | x = d[0] + xofs 73 | y = d[1] + yofs 74 | z = d[2] + zofs 75 | r = d[3] 76 | p = d[4] 77 | h = d[5] 78 | mv = [x, y, z] 79 | mv2 = dot(a, mv) 80 | x = mv2[0] 81 | y = mv2[1] 82 | z = mv2[2] 83 | 84 | headX = x*cos(p) + y*sin(r)*sin(p) + z*cos(r)*sin(p) 85 | headY = y*cos(r) - z*sin(r) 86 | heading = degrees(atan2(-headY,headX)) + declination 87 | if heading < 0: 88 | heading += 360 89 | herror = h - heading 90 | if herror > 180: 91 | herror -= 360 92 | if herror < -180: 93 | herror += 360 94 | ret.append(herror) 95 | return ret 96 | 97 | def fit_data(data): 98 | import numpy, scipy 99 | from scipy import optimize 100 | 101 | p0 = [0.0, 0.0, 0.0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0] 102 | p1, ier = optimize.leastsq(heading_error1, p0[:], args=(data)) 103 | 104 | # p0 = p1[:] 105 | # p1, ier = optimize.leastsq(heading_error, p0[:], args=(data)) 106 | 107 | print(p1) 108 | if not ier in [1, 2, 3, 4]: 109 | raise RuntimeError("Unable to find solution") 110 | return p1 111 | 112 | def magfit(logfile): 113 | '''find best magnetometer offset fit to a log file''' 114 | print("Processing log %s" % filename) 115 | mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps) 116 | 117 | flying = False 118 | gps_heading = 0.0 119 | 120 | data = [] 121 | 122 | # get the current mag offsets 123 | m = mlog.recv_match(type='SENSOR_OFFSETS') 124 | offsets = vec3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z) 125 | 126 | attitude = mlog.recv_match(type='ATTITUDE') 127 | 128 | # now gather all the data 129 | while True: 130 | m = mlog.recv_match() 131 | if m is None: 132 | break 133 | if m.get_type() == "GPS_RAW": 134 | # flying if groundspeed more than 5 m/s 135 | flying = (m.v > opts.minspeed and m.fix_type == 2) 136 | gps_heading = m.hdg 137 | if m.get_type() == "ATTITUDE": 138 | attitude = m 139 | if m.get_type() == "SENSOR_OFFSETS": 140 | # update current offsets 141 | offsets = vec3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z) 142 | if not flying: 143 | continue 144 | if m.get_type() == "RAW_IMU": 145 | data.append((m.xmag - offsets.x, m.ymag - offsets.y, m.zmag - offsets.z, attitude.roll, attitude.pitch, gps_heading)) 146 | print("Extracted %u data points" % len(data)) 147 | print("Current offsets: %s" % offsets) 148 | ofs2 = fit_data(data) 149 | print("Declination estimate: %.1f" % ofs2[-1]) 150 | new_offsets = vec3(ofs2[0], ofs2[1], ofs2[2]) 151 | a = [[ofs2[3], ofs2[4], ofs2[5]], 152 | [ofs2[6], ofs2[7], ofs2[8]], 153 | [ofs2[9], ofs2[10], ofs2[11]]] 154 | print(a) 155 | print("New offsets : %s" % new_offsets) 156 | 157 | total = 0.0 158 | for filename in args: 159 | magfit(filename) 160 | -------------------------------------------------------------------------------- /mavlink/include/mavlink/v1.0/autoquad/testsuite.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol testsuite generated from autoquad.xml 3 | * @see http://qgroundcontrol.org/mavlink/ 4 | */ 5 | #ifndef AUTOQUAD_TESTSUITE_H 6 | #define AUTOQUAD_TESTSUITE_H 7 | 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | #ifndef MAVLINK_TEST_ALL 13 | #define MAVLINK_TEST_ALL 14 | static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); 15 | static void mavlink_test_autoquad(uint8_t, uint8_t, mavlink_message_t *last_msg); 16 | 17 | static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) 18 | { 19 | mavlink_test_common(system_id, component_id, last_msg); 20 | mavlink_test_autoquad(system_id, component_id, last_msg); 21 | } 22 | #endif 23 | 24 | #include "../common/testsuite.h" 25 | 26 | 27 | static void mavlink_test_aq_telemetry_f(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) 28 | { 29 | mavlink_message_t msg; 30 | uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; 31 | uint16_t i; 32 | mavlink_aq_telemetry_f_t packet_in = { 33 | 17.0, 34 | }45.0, 35 | }73.0, 36 | }101.0, 37 | }129.0, 38 | }157.0, 39 | }185.0, 40 | }213.0, 41 | }241.0, 42 | }269.0, 43 | }297.0, 44 | }325.0, 45 | }353.0, 46 | }381.0, 47 | }409.0, 48 | }437.0, 49 | }465.0, 50 | }493.0, 51 | }521.0, 52 | }549.0, 53 | }21395, 54 | }; 55 | mavlink_aq_telemetry_f_t packet1, packet2; 56 | memset(&packet1, 0, sizeof(packet1)); 57 | packet1.value1 = packet_in.value1; 58 | packet1.value2 = packet_in.value2; 59 | packet1.value3 = packet_in.value3; 60 | packet1.value4 = packet_in.value4; 61 | packet1.value5 = packet_in.value5; 62 | packet1.value6 = packet_in.value6; 63 | packet1.value7 = packet_in.value7; 64 | packet1.value8 = packet_in.value8; 65 | packet1.value9 = packet_in.value9; 66 | packet1.value10 = packet_in.value10; 67 | packet1.value11 = packet_in.value11; 68 | packet1.value12 = packet_in.value12; 69 | packet1.value13 = packet_in.value13; 70 | packet1.value14 = packet_in.value14; 71 | packet1.value15 = packet_in.value15; 72 | packet1.value16 = packet_in.value16; 73 | packet1.value17 = packet_in.value17; 74 | packet1.value18 = packet_in.value18; 75 | packet1.value19 = packet_in.value19; 76 | packet1.value20 = packet_in.value20; 77 | packet1.Index = packet_in.Index; 78 | 79 | 80 | 81 | memset(&packet2, 0, sizeof(packet2)); 82 | mavlink_msg_aq_telemetry_f_encode(system_id, component_id, &msg, &packet1); 83 | mavlink_msg_aq_telemetry_f_decode(&msg, &packet2); 84 | MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); 85 | 86 | memset(&packet2, 0, sizeof(packet2)); 87 | mavlink_msg_aq_telemetry_f_pack(system_id, component_id, &msg , packet1.Index , packet1.value1 , packet1.value2 , packet1.value3 , packet1.value4 , packet1.value5 , packet1.value6 , packet1.value7 , packet1.value8 , packet1.value9 , packet1.value10 , packet1.value11 , packet1.value12 , packet1.value13 , packet1.value14 , packet1.value15 , packet1.value16 , packet1.value17 , packet1.value18 , packet1.value19 , packet1.value20 ); 88 | mavlink_msg_aq_telemetry_f_decode(&msg, &packet2); 89 | MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); 90 | 91 | memset(&packet2, 0, sizeof(packet2)); 92 | mavlink_msg_aq_telemetry_f_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.Index , packet1.value1 , packet1.value2 , packet1.value3 , packet1.value4 , packet1.value5 , packet1.value6 , packet1.value7 , packet1.value8 , packet1.value9 , packet1.value10 , packet1.value11 , packet1.value12 , packet1.value13 , packet1.value14 , packet1.value15 , packet1.value16 , packet1.value17 , packet1.value18 , packet1.value19 , packet1.value20 ); 93 | mavlink_msg_aq_telemetry_f_decode(&msg, &packet2); 94 | MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); 95 | 96 | memset(&packet2, 0, sizeof(packet2)); 97 | mavlink_msg_to_send_buffer(buffer, &msg); 98 | for (i=0; i") 36 | sys.exit(1) 37 | 38 | def noise(): 39 | '''a noise vector''' 40 | from random import gauss 41 | v = Vector3(gauss(0, 1), gauss(0, 1), gauss(0, 1)) 42 | v.normalize() 43 | return v * opts.noise 44 | 45 | def find_offsets(data, ofs): 46 | '''find mag offsets by applying Bills "offsets revisited" algorithm 47 | on the data 48 | 49 | This is an implementation of the algorithm from: 50 | http://gentlenav.googlecode.com/files/MagnetometerOffsetNullingRevisited.pdf 51 | ''' 52 | 53 | # a limit on the maximum change in each step 54 | max_change = opts.max_change 55 | 56 | # the gain factor for the algorithm 57 | gain = opts.gain 58 | 59 | data2 = [] 60 | for d in data: 61 | d = d.copy() + noise() 62 | d.x = float(int(d.x + 0.5)) 63 | d.y = float(int(d.y + 0.5)) 64 | d.z = float(int(d.z + 0.5)) 65 | data2.append(d) 66 | data = data2 67 | 68 | history_idx = 0 69 | mag_history = data[0:opts.history] 70 | 71 | for i in range(opts.history, len(data)): 72 | B1 = mag_history[history_idx] + ofs 73 | B2 = data[i] + ofs 74 | 75 | diff = B2 - B1 76 | diff_length = diff.length() 77 | if diff_length <= opts.min_diff: 78 | # the mag vector hasn't changed enough - we don't get any 79 | # information from this 80 | history_idx = (history_idx+1) % opts.history 81 | continue 82 | 83 | mag_history[history_idx] = data[i] 84 | history_idx = (history_idx+1) % opts.history 85 | 86 | # equation 6 of Bills paper 87 | delta = diff * (gain * (B2.length() - B1.length()) / diff_length) 88 | 89 | # limit the change from any one reading. This is to prevent 90 | # single crazy readings from throwing off the offsets for a long 91 | # time 92 | delta_length = delta.length() 93 | if max_change != 0 and delta_length > max_change: 94 | delta *= max_change / delta_length 95 | 96 | # set the new offsets 97 | ofs = ofs - delta 98 | 99 | if opts.verbose: 100 | print ofs 101 | return ofs 102 | 103 | 104 | def magfit(logfile): 105 | '''find best magnetometer offset fit to a log file''' 106 | 107 | print("Processing log %s" % filename) 108 | 109 | # open the log file 110 | mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps) 111 | 112 | data = [] 113 | mag = None 114 | offsets = Vector3(0,0,0) 115 | 116 | # now gather all the data 117 | while True: 118 | # get the next MAVLink message in the log 119 | m = mlog.recv_match(condition=opts.condition) 120 | if m is None: 121 | break 122 | if m.get_type() == "SENSOR_OFFSETS": 123 | # update offsets that were used during this flight 124 | offsets = Vector3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z) 125 | if m.get_type() == "RAW_IMU" and offsets != None: 126 | # extract one mag vector, removing the offsets that were 127 | # used during that flight to get the raw sensor values 128 | mag = Vector3(m.xmag, m.ymag, m.zmag) - offsets 129 | data.append(mag) 130 | 131 | print("Extracted %u data points" % len(data)) 132 | print("Current offsets: %s" % offsets) 133 | 134 | # run the fitting algorithm 135 | ofs = offsets 136 | ofs = Vector3(0,0,0) 137 | for r in range(opts.repeat): 138 | ofs = find_offsets(data, ofs) 139 | print('Loop %u offsets %s' % (r, ofs)) 140 | sys.stdout.flush() 141 | print("New offsets: %s" % ofs) 142 | 143 | total = 0.0 144 | for filename in args: 145 | magfit(filename) 146 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/generator/mavtemplate.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | ''' 3 | simple templating system for mavlink generator 4 | 5 | Copyright Andrew Tridgell 2011 6 | Released under GNU GPL version 3 or later 7 | ''' 8 | 9 | from mavparse import MAVParseError 10 | 11 | class MAVTemplate(object): 12 | '''simple templating system''' 13 | def __init__(self, 14 | start_var_token="${", 15 | end_var_token="}", 16 | start_rep_token="${{", 17 | end_rep_token="}}", 18 | trim_leading_lf=True, 19 | checkmissing=True): 20 | self.start_var_token = start_var_token 21 | self.end_var_token = end_var_token 22 | self.start_rep_token = start_rep_token 23 | self.end_rep_token = end_rep_token 24 | self.trim_leading_lf = trim_leading_lf 25 | self.checkmissing = checkmissing 26 | 27 | def find_end(self, text, start_token, end_token): 28 | '''find the of a token. 29 | Returns the offset in the string immediately after the matching end_token''' 30 | if not text.startswith(start_token): 31 | raise MAVParseError("invalid token start") 32 | offset = len(start_token) 33 | nesting = 1 34 | while nesting > 0: 35 | idx1 = text[offset:].find(start_token) 36 | idx2 = text[offset:].find(end_token) 37 | if idx1 == -1 and idx2 == -1: 38 | raise MAVParseError("token nesting error") 39 | if idx1 == -1 or idx1 > idx2: 40 | offset += idx2 + len(end_token) 41 | nesting -= 1 42 | else: 43 | offset += idx1 + len(start_token) 44 | nesting += 1 45 | return offset 46 | 47 | def find_var_end(self, text): 48 | '''find the of a variable''' 49 | return self.find_end(text, self.start_var_token, self.end_var_token) 50 | 51 | def find_rep_end(self, text): 52 | '''find the of a repitition''' 53 | return self.find_end(text, self.start_rep_token, self.end_rep_token) 54 | 55 | def substitute(self, text, subvars={}, 56 | trim_leading_lf=None, checkmissing=None): 57 | '''substitute variables in a string''' 58 | 59 | if trim_leading_lf is None: 60 | trim_leading_lf = self.trim_leading_lf 61 | if checkmissing is None: 62 | checkmissing = self.checkmissing 63 | 64 | # handle repititions 65 | while True: 66 | subidx = text.find(self.start_rep_token) 67 | if subidx == -1: 68 | break 69 | endidx = self.find_rep_end(text[subidx:]) 70 | if endidx == -1: 71 | raise MAVParseError("missing end macro in %s" % text[subidx:]) 72 | part1 = text[0:subidx] 73 | part2 = text[subidx+len(self.start_rep_token):subidx+(endidx-len(self.end_rep_token))] 74 | part3 = text[subidx+endidx:] 75 | a = part2.split(':') 76 | field_name = a[0] 77 | rest = ':'.join(a[1:]) 78 | v = getattr(subvars, field_name, None) 79 | if v is None: 80 | raise MAVParseError('unable to find field %s' % field_name) 81 | t1 = part1 82 | for f in v: 83 | t1 += self.substitute(rest, f, trim_leading_lf=False, checkmissing=False) 84 | if len(v) != 0 and t1[-1] in ["\n", ","]: 85 | t1 = t1[:-1] 86 | t1 += part3 87 | text = t1 88 | 89 | if trim_leading_lf: 90 | if text[0] == '\n': 91 | text = text[1:] 92 | while True: 93 | idx = text.find(self.start_var_token) 94 | if idx == -1: 95 | return text 96 | endidx = text[idx:].find(self.end_var_token) 97 | if endidx == -1: 98 | raise MAVParseError('missing end of variable: %s' % text[idx:idx+10]) 99 | varname = text[idx+2:idx+endidx] 100 | if isinstance(subvars, dict): 101 | if not varname in subvars: 102 | if checkmissing: 103 | raise MAVParseError("unknown variable in '%s%s%s'" % ( 104 | self.start_var_token, varname, self.end_var_token)) 105 | return text[0:idx+endidx] + self.substitute(text[idx+endidx:], subvars, 106 | trim_leading_lf=False, checkmissing=False) 107 | value = subvars[varname] 108 | else: 109 | value = getattr(subvars, varname, None) 110 | if value is None: 111 | if checkmissing: 112 | raise MAVParseError("unknown variable in '%s%s%s'" % ( 113 | self.start_var_token, varname, self.end_var_token)) 114 | return text[0:idx+endidx] + self.substitute(text[idx+endidx:], subvars, 115 | trim_leading_lf=False, checkmissing=False) 116 | text = text.replace("%s%s%s" % (self.start_var_token, varname, self.end_var_token), str(value)) 117 | return text 118 | 119 | def write(self, file, text, subvars={}, trim_leading_lf=True): 120 | '''write to a file with variable substitution''' 121 | file.write(self.substitute(text, subvars=subvars, trim_leading_lf=trim_leading_lf)) 122 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/generator/C/test/windows/testmav.cpp: -------------------------------------------------------------------------------- 1 | // testmav.cpp : Defines the entry point for the console application. 2 | // 3 | 4 | #include "stdafx.h" 5 | #include "stdio.h" 6 | #include "stdint.h" 7 | #include "stddef.h" 8 | #include "assert.h" 9 | 10 | 11 | #define MAVLINK_USE_CONVENIENCE_FUNCTIONS 12 | #define MAVLINK_COMM_NUM_BUFFERS 2 13 | 14 | #include 15 | static mavlink_system_t mavlink_system = {42,11,}; 16 | 17 | #define MAVLINK_ASSERT(x) assert(x) 18 | static void comm_send_ch(mavlink_channel_t chan, uint8_t c); 19 | 20 | static mavlink_message_t last_msg; 21 | 22 | #include 23 | #include 24 | 25 | static unsigned chan_counts[MAVLINK_COMM_NUM_BUFFERS]; 26 | 27 | static const unsigned message_lengths[] = MAVLINK_MESSAGE_LENGTHS; 28 | static unsigned error_count; 29 | 30 | static const mavlink_message_info_t message_info[256] = MAVLINK_MESSAGE_INFO; 31 | 32 | static void print_one_field(mavlink_message_t *msg, const mavlink_field_info_t *f, int idx) 33 | { 34 | #define PRINT_FORMAT(f, def) (f->print_format?f->print_format:def) 35 | switch (f->type) { 36 | case MAVLINK_TYPE_CHAR: 37 | printf(PRINT_FORMAT(f, "%c"), _MAV_RETURN_char(msg, f->wire_offset+idx*1)); 38 | break; 39 | case MAVLINK_TYPE_UINT8_T: 40 | printf(PRINT_FORMAT(f, "%u"), _MAV_RETURN_uint8_t(msg, f->wire_offset+idx*1)); 41 | break; 42 | case MAVLINK_TYPE_INT8_T: 43 | printf(PRINT_FORMAT(f, "%d"), _MAV_RETURN_int8_t(msg, f->wire_offset+idx*1)); 44 | break; 45 | case MAVLINK_TYPE_UINT16_T: 46 | printf(PRINT_FORMAT(f, "%u"), _MAV_RETURN_uint16_t(msg, f->wire_offset+idx*2)); 47 | break; 48 | case MAVLINK_TYPE_INT16_T: 49 | printf(PRINT_FORMAT(f, "%d"), _MAV_RETURN_int16_t(msg, f->wire_offset+idx*2)); 50 | break; 51 | case MAVLINK_TYPE_UINT32_T: 52 | printf(PRINT_FORMAT(f, "%lu"), (unsigned long)_MAV_RETURN_uint32_t(msg, f->wire_offset+idx*4)); 53 | break; 54 | case MAVLINK_TYPE_INT32_T: 55 | printf(PRINT_FORMAT(f, "%ld"), (long)_MAV_RETURN_int32_t(msg, f->wire_offset+idx*4)); 56 | break; 57 | case MAVLINK_TYPE_UINT64_T: 58 | printf(PRINT_FORMAT(f, "%llu"), (unsigned long long)_MAV_RETURN_uint64_t(msg, f->wire_offset+idx*8)); 59 | break; 60 | case MAVLINK_TYPE_INT64_T: 61 | printf(PRINT_FORMAT(f, "%lld"), (long long)_MAV_RETURN_int64_t(msg, f->wire_offset+idx*8)); 62 | break; 63 | case MAVLINK_TYPE_FLOAT: 64 | printf(PRINT_FORMAT(f, "%f"), (double)_MAV_RETURN_float(msg, f->wire_offset+idx*4)); 65 | break; 66 | case MAVLINK_TYPE_DOUBLE: 67 | printf(PRINT_FORMAT(f, "%f"), _MAV_RETURN_double(msg, f->wire_offset+idx*8)); 68 | break; 69 | } 70 | } 71 | 72 | static void print_field(mavlink_message_t *msg, const mavlink_field_info_t *f) 73 | { 74 | printf("%s: ", f->name); 75 | if (f->array_length == 0) { 76 | print_one_field(msg, f, 0); 77 | printf(" "); 78 | } else { 79 | unsigned i; 80 | /* print an array */ 81 | if (f->type == MAVLINK_TYPE_CHAR) { 82 | printf("'%.*s'", f->array_length, 83 | f->wire_offset+(const char *)_MAV_PAYLOAD(msg)); 84 | 85 | } else { 86 | printf("[ "); 87 | for (i=0; iarray_length; i++) { 88 | print_one_field(msg, f, i); 89 | if (i < f->array_length) { 90 | printf(", "); 91 | } 92 | } 93 | printf("]"); 94 | } 95 | } 96 | printf(" "); 97 | } 98 | 99 | static void print_message(mavlink_message_t *msg) 100 | { 101 | const mavlink_message_info_t *m = &message_info[msg->msgid]; 102 | const mavlink_field_info_t *f = m->fields; 103 | unsigned i; 104 | printf("%s { ", m->name); 105 | for (i=0; inum_fields; i++) { 106 | print_field(msg, &f[i]); 107 | } 108 | printf("}\n"); 109 | } 110 | 111 | static void comm_send_ch(mavlink_channel_t chan, uint8_t c) 112 | { 113 | mavlink_status_t status; 114 | if (mavlink_parse_char(chan, c, &last_msg, &status)) { 115 | print_message(&last_msg); 116 | chan_counts[chan]++; 117 | /* channel 0 gets 3 messages per message, because of 118 | the channel defaults for _pack() and _encode() */ 119 | if (chan == MAVLINK_COMM_0 && status.current_rx_seq != (uint8_t)(chan_counts[chan]*3)) { 120 | printf("Channel 0 sequence mismatch error at packet %u (rx_seq=%u)\n", 121 | chan_counts[chan], status.current_rx_seq); 122 | error_count++; 123 | } else if (chan > MAVLINK_COMM_0 && status.current_rx_seq != (uint8_t)chan_counts[chan]) { 124 | printf("Channel %u sequence mismatch error at packet %u (rx_seq=%u)\n", 125 | (unsigned)chan, chan_counts[chan], status.current_rx_seq); 126 | error_count++; 127 | } 128 | if (message_lengths[last_msg.msgid] != last_msg.len) { 129 | printf("Incorrect message length %u for message %u - expected %u\n", 130 | (unsigned)last_msg.len, (unsigned)last_msg.msgid, message_lengths[last_msg.msgid]); 131 | error_count++; 132 | } 133 | } 134 | if (status.packet_rx_drop_count != 0) { 135 | printf("Parse error at packet %u\n", chan_counts[chan]); 136 | error_count++; 137 | } 138 | } 139 | 140 | int _tmain(int argc, _TCHAR* argv[]) 141 | { 142 | int chan; 143 | mavlink_test_all(11, 10, &last_msg); 144 | for (chan=MAVLINK_COMM_0; chan<=MAVLINK_COMM_1; chan++) { 145 | printf("Received %u messages on channel %u OK\n", 146 | chan_counts[chan], (unsigned)chan); 147 | } 148 | if (error_count != 0) { 149 | printf("Error count %u\n", error_count); 150 | return(1); 151 | } 152 | printf("No errors detected\n"); 153 | return 0; 154 | } 155 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/generator/C/test/posix/testmav.c: -------------------------------------------------------------------------------- 1 | /* 2 | simple MAVLink testsuite for C 3 | */ 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #define MAVLINK_USE_CONVENIENCE_FUNCTIONS 11 | #define MAVLINK_COMM_NUM_BUFFERS 2 12 | 13 | // this trick allows us to make mavlink_message_t as small as possible 14 | // for this dialect, which saves some memory 15 | #include 16 | #define MAVLINK_MAX_PAYLOAD_LEN MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 17 | 18 | #include 19 | static mavlink_system_t mavlink_system = {42,11,}; 20 | 21 | #define MAVLINK_ASSERT(x) assert(x) 22 | static void comm_send_ch(mavlink_channel_t chan, uint8_t c); 23 | 24 | static mavlink_message_t last_msg; 25 | 26 | #include 27 | #include 28 | 29 | static unsigned chan_counts[MAVLINK_COMM_NUM_BUFFERS]; 30 | 31 | static const unsigned message_lengths[] = MAVLINK_MESSAGE_LENGTHS; 32 | static unsigned error_count; 33 | 34 | static const mavlink_message_info_t message_info[256] = MAVLINK_MESSAGE_INFO; 35 | 36 | static void print_one_field(mavlink_message_t *msg, const mavlink_field_info_t *f, int idx) 37 | { 38 | #define PRINT_FORMAT(f, def) (f->print_format?f->print_format:def) 39 | switch (f->type) { 40 | case MAVLINK_TYPE_CHAR: 41 | printf(PRINT_FORMAT(f, "%c"), _MAV_RETURN_char(msg, f->wire_offset+idx*1)); 42 | break; 43 | case MAVLINK_TYPE_UINT8_T: 44 | printf(PRINT_FORMAT(f, "%u"), _MAV_RETURN_uint8_t(msg, f->wire_offset+idx*1)); 45 | break; 46 | case MAVLINK_TYPE_INT8_T: 47 | printf(PRINT_FORMAT(f, "%d"), _MAV_RETURN_int8_t(msg, f->wire_offset+idx*1)); 48 | break; 49 | case MAVLINK_TYPE_UINT16_T: 50 | printf(PRINT_FORMAT(f, "%u"), _MAV_RETURN_uint16_t(msg, f->wire_offset+idx*2)); 51 | break; 52 | case MAVLINK_TYPE_INT16_T: 53 | printf(PRINT_FORMAT(f, "%d"), _MAV_RETURN_int16_t(msg, f->wire_offset+idx*2)); 54 | break; 55 | case MAVLINK_TYPE_UINT32_T: 56 | printf(PRINT_FORMAT(f, "%lu"), (unsigned long)_MAV_RETURN_uint32_t(msg, f->wire_offset+idx*4)); 57 | break; 58 | case MAVLINK_TYPE_INT32_T: 59 | printf(PRINT_FORMAT(f, "%ld"), (long)_MAV_RETURN_int32_t(msg, f->wire_offset+idx*4)); 60 | break; 61 | case MAVLINK_TYPE_UINT64_T: 62 | printf(PRINT_FORMAT(f, "%llu"), (unsigned long long)_MAV_RETURN_uint64_t(msg, f->wire_offset+idx*8)); 63 | break; 64 | case MAVLINK_TYPE_INT64_T: 65 | printf(PRINT_FORMAT(f, "%lld"), (long long)_MAV_RETURN_int64_t(msg, f->wire_offset+idx*8)); 66 | break; 67 | case MAVLINK_TYPE_FLOAT: 68 | printf(PRINT_FORMAT(f, "%f"), (double)_MAV_RETURN_float(msg, f->wire_offset+idx*4)); 69 | break; 70 | case MAVLINK_TYPE_DOUBLE: 71 | printf(PRINT_FORMAT(f, "%f"), _MAV_RETURN_double(msg, f->wire_offset+idx*8)); 72 | break; 73 | } 74 | } 75 | 76 | static void print_field(mavlink_message_t *msg, const mavlink_field_info_t *f) 77 | { 78 | printf("%s: ", f->name); 79 | if (f->array_length == 0) { 80 | print_one_field(msg, f, 0); 81 | printf(" "); 82 | } else { 83 | unsigned i; 84 | /* print an array */ 85 | if (f->type == MAVLINK_TYPE_CHAR) { 86 | printf("'%.*s'", f->array_length, 87 | f->wire_offset+(const char *)_MAV_PAYLOAD(msg)); 88 | 89 | } else { 90 | printf("[ "); 91 | for (i=0; iarray_length; i++) { 92 | print_one_field(msg, f, i); 93 | if (i < f->array_length) { 94 | printf(", "); 95 | } 96 | } 97 | printf("]"); 98 | } 99 | } 100 | printf(" "); 101 | } 102 | 103 | static void print_message(mavlink_message_t *msg) 104 | { 105 | const mavlink_message_info_t *m = &message_info[msg->msgid]; 106 | const mavlink_field_info_t *f = m->fields; 107 | unsigned i; 108 | printf("%s { ", m->name); 109 | for (i=0; inum_fields; i++) { 110 | print_field(msg, &f[i]); 111 | } 112 | printf("}\n"); 113 | } 114 | 115 | static void comm_send_ch(mavlink_channel_t chan, uint8_t c) 116 | { 117 | mavlink_status_t status; 118 | if (mavlink_parse_char(chan, c, &last_msg, &status)) { 119 | print_message(&last_msg); 120 | chan_counts[chan]++; 121 | /* channel 0 gets 3 messages per message, because of 122 | the channel defaults for _pack() and _encode() */ 123 | if (chan == MAVLINK_COMM_0 && status.current_rx_seq != (uint8_t)(chan_counts[chan]*3)) { 124 | printf("Channel 0 sequence mismatch error at packet %u (rx_seq=%u)\n", 125 | chan_counts[chan], status.current_rx_seq); 126 | error_count++; 127 | } else if (chan > MAVLINK_COMM_0 && status.current_rx_seq != (uint8_t)chan_counts[chan]) { 128 | printf("Channel %u sequence mismatch error at packet %u (rx_seq=%u)\n", 129 | (unsigned)chan, chan_counts[chan], status.current_rx_seq); 130 | error_count++; 131 | } 132 | if (message_lengths[last_msg.msgid] != last_msg.len) { 133 | printf("Incorrect message length %u for message %u - expected %u\n", 134 | (unsigned)last_msg.len, (unsigned)last_msg.msgid, message_lengths[last_msg.msgid]); 135 | error_count++; 136 | } 137 | } 138 | if (status.packet_rx_drop_count != 0) { 139 | printf("Parse error at packet %u\n", chan_counts[chan]); 140 | error_count++; 141 | } 142 | } 143 | 144 | int main(void) 145 | { 146 | mavlink_channel_t chan; 147 | mavlink_test_all(11, 10, &last_msg); 148 | for (chan=MAVLINK_COMM_0; chan<=MAVLINK_COMM_1; chan++) { 149 | printf("Received %u messages on channel %u OK\n", 150 | chan_counts[chan], (unsigned)chan); 151 | } 152 | if (error_count != 0) { 153 | printf("Error count %u\n", error_count); 154 | exit(1); 155 | } 156 | printf("No errors detected\n"); 157 | return 0; 158 | } 159 | 160 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/testsuite.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol testsuite generated from test.xml 3 | * @see http://qgroundcontrol.org/mavlink/ 4 | */ 5 | #ifndef TEST_TESTSUITE_H 6 | #define TEST_TESTSUITE_H 7 | 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | #ifndef MAVLINK_TEST_ALL 13 | #define MAVLINK_TEST_ALL 14 | 15 | static void mavlink_test_test(uint8_t, uint8_t, mavlink_message_t *last_msg); 16 | 17 | static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) 18 | { 19 | 20 | mavlink_test_test(system_id, component_id, last_msg); 21 | } 22 | #endif 23 | 24 | 25 | 26 | 27 | static void mavlink_test_test_types(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) 28 | { 29 | mavlink_message_t msg; 30 | uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; 31 | uint16_t i; 32 | mavlink_test_types_t packet_in = { 33 | 93372036854775807ULL, 34 | 93372036854776311LL, 35 | 235.0, 36 | { 93372036854777319, 93372036854777320, 93372036854777321 }, 37 | { 93372036854778831, 93372036854778832, 93372036854778833 }, 38 | { 627.0, 628.0, 629.0 }, 39 | 963502456, 40 | 963502664, 41 | 745.0, 42 | { 963503080, 963503081, 963503082 }, 43 | { 963503704, 963503705, 963503706 }, 44 | { 941.0, 942.0, 943.0 }, 45 | 24723, 46 | 24827, 47 | { 24931, 24932, 24933 }, 48 | { 25243, 25244, 25245 }, 49 | 'E', 50 | "FGHIJKLMN", 51 | 198, 52 | 9, 53 | { 76, 77, 78 }, 54 | { 21, 22, 23 }, 55 | }; 56 | mavlink_test_types_t packet1, packet2; 57 | memset(&packet1, 0, sizeof(packet1)); 58 | packet1.u64 = packet_in.u64; 59 | packet1.s64 = packet_in.s64; 60 | packet1.d = packet_in.d; 61 | packet1.u32 = packet_in.u32; 62 | packet1.s32 = packet_in.s32; 63 | packet1.f = packet_in.f; 64 | packet1.u16 = packet_in.u16; 65 | packet1.s16 = packet_in.s16; 66 | packet1.c = packet_in.c; 67 | packet1.u8 = packet_in.u8; 68 | packet1.s8 = packet_in.s8; 69 | 70 | mav_array_memcpy(packet1.u64_array, packet_in.u64_array, sizeof(uint64_t)*3); 71 | mav_array_memcpy(packet1.s64_array, packet_in.s64_array, sizeof(int64_t)*3); 72 | mav_array_memcpy(packet1.d_array, packet_in.d_array, sizeof(double)*3); 73 | mav_array_memcpy(packet1.u32_array, packet_in.u32_array, sizeof(uint32_t)*3); 74 | mav_array_memcpy(packet1.s32_array, packet_in.s32_array, sizeof(int32_t)*3); 75 | mav_array_memcpy(packet1.f_array, packet_in.f_array, sizeof(float)*3); 76 | mav_array_memcpy(packet1.u16_array, packet_in.u16_array, sizeof(uint16_t)*3); 77 | mav_array_memcpy(packet1.s16_array, packet_in.s16_array, sizeof(int16_t)*3); 78 | mav_array_memcpy(packet1.s, packet_in.s, sizeof(char)*10); 79 | mav_array_memcpy(packet1.u8_array, packet_in.u8_array, sizeof(uint8_t)*3); 80 | mav_array_memcpy(packet1.s8_array, packet_in.s8_array, sizeof(int8_t)*3); 81 | 82 | 83 | memset(&packet2, 0, sizeof(packet2)); 84 | mavlink_msg_test_types_encode(system_id, component_id, &msg, &packet1); 85 | mavlink_msg_test_types_decode(&msg, &packet2); 86 | MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); 87 | 88 | memset(&packet2, 0, sizeof(packet2)); 89 | mavlink_msg_test_types_pack(system_id, component_id, &msg , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array ); 90 | mavlink_msg_test_types_decode(&msg, &packet2); 91 | MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); 92 | 93 | memset(&packet2, 0, sizeof(packet2)); 94 | mavlink_msg_test_types_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array ); 95 | mavlink_msg_test_types_decode(&msg, &packet2); 96 | MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); 97 | 98 | memset(&packet2, 0, sizeof(packet2)); 99 | mavlink_msg_to_send_buffer(buffer, &msg); 100 | for (i=0; imsgid = MAVLINK_MSG_ID_OBS_QFF; 55 | #if MAVLINK_CRC_EXTRA 56 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_QFF_LEN, MAVLINK_MSG_ID_OBS_QFF_CRC); 57 | #else 58 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_QFF_LEN); 59 | #endif 60 | } 61 | 62 | /** 63 | * @brief Pack a obs_qff message on a channel 64 | * @param system_id ID of this system 65 | * @param component_id ID of this component (e.g. 200 for IMU) 66 | * @param chan The MAVLink channel this message will be sent over 67 | * @param msg The MAVLink message to compress the data into 68 | * @param qff 69 | 70 | 71 | * @return length of the message in bytes (excluding serial stream start sign) 72 | */ 73 | static inline uint16_t mavlink_msg_obs_qff_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 74 | mavlink_message_t* msg, 75 | float qff) 76 | { 77 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 78 | char buf[MAVLINK_MSG_ID_OBS_QFF_LEN]; 79 | _mav_put_float(buf, 0, qff); 80 | 81 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_QFF_LEN); 82 | #else 83 | mavlink_obs_qff_t packet; 84 | packet.qff = qff; 85 | 86 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_QFF_LEN); 87 | #endif 88 | 89 | msg->msgid = MAVLINK_MSG_ID_OBS_QFF; 90 | #if MAVLINK_CRC_EXTRA 91 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_QFF_LEN, MAVLINK_MSG_ID_OBS_QFF_CRC); 92 | #else 93 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_QFF_LEN); 94 | #endif 95 | } 96 | 97 | /** 98 | * @brief Encode a obs_qff struct 99 | * 100 | * @param system_id ID of this system 101 | * @param component_id ID of this component (e.g. 200 for IMU) 102 | * @param msg The MAVLink message to compress the data into 103 | * @param obs_qff C-struct to read the message contents from 104 | */ 105 | static inline uint16_t mavlink_msg_obs_qff_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_qff_t* obs_qff) 106 | { 107 | return mavlink_msg_obs_qff_pack(system_id, component_id, msg, obs_qff->qff); 108 | } 109 | 110 | /** 111 | * @brief Encode a obs_qff struct on a channel 112 | * 113 | * @param system_id ID of this system 114 | * @param component_id ID of this component (e.g. 200 for IMU) 115 | * @param chan The MAVLink channel this message will be sent over 116 | * @param msg The MAVLink message to compress the data into 117 | * @param obs_qff C-struct to read the message contents from 118 | */ 119 | static inline uint16_t mavlink_msg_obs_qff_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_qff_t* obs_qff) 120 | { 121 | return mavlink_msg_obs_qff_pack_chan(system_id, component_id, chan, msg, obs_qff->qff); 122 | } 123 | 124 | /** 125 | * @brief Send a obs_qff message 126 | * @param chan MAVLink channel to send the message 127 | * 128 | * @param qff 129 | 130 | 131 | */ 132 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 133 | 134 | static inline void mavlink_msg_obs_qff_send(mavlink_channel_t chan, float qff) 135 | { 136 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 137 | char buf[MAVLINK_MSG_ID_OBS_QFF_LEN]; 138 | _mav_put_float(buf, 0, qff); 139 | 140 | #if MAVLINK_CRC_EXTRA 141 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, buf, MAVLINK_MSG_ID_OBS_QFF_LEN, MAVLINK_MSG_ID_OBS_QFF_CRC); 142 | #else 143 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, buf, MAVLINK_MSG_ID_OBS_QFF_LEN); 144 | #endif 145 | #else 146 | mavlink_obs_qff_t packet; 147 | packet.qff = qff; 148 | 149 | #if MAVLINK_CRC_EXTRA 150 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, (const char *)&packet, MAVLINK_MSG_ID_OBS_QFF_LEN, MAVLINK_MSG_ID_OBS_QFF_CRC); 151 | #else 152 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, (const char *)&packet, MAVLINK_MSG_ID_OBS_QFF_LEN); 153 | #endif 154 | #endif 155 | } 156 | 157 | #endif 158 | 159 | // MESSAGE OBS_QFF UNPACKING 160 | 161 | 162 | /** 163 | * @brief Get field qff from obs_qff message 164 | * 165 | * @return 166 | 167 | 168 | */ 169 | static inline float mavlink_msg_obs_qff_get_qff(const mavlink_message_t* msg) 170 | { 171 | return _MAV_RETURN_float(msg, 0); 172 | } 173 | 174 | /** 175 | * @brief Decode a obs_qff message into a struct 176 | * 177 | * @param msg The message to decode 178 | * @param obs_qff C-struct to decode the message contents into 179 | */ 180 | static inline void mavlink_msg_obs_qff_decode(const mavlink_message_t* msg, mavlink_obs_qff_t* obs_qff) 181 | { 182 | #if MAVLINK_NEED_BYTE_SWAP 183 | obs_qff->qff = mavlink_msg_obs_qff_get_qff(msg); 184 | #else 185 | memcpy(obs_qff, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_QFF_LEN); 186 | #endif 187 | } 188 | -------------------------------------------------------------------------------- /mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h: -------------------------------------------------------------------------------- 1 | // MESSAGE AUTH_KEY PACKING 2 | 3 | #define MAVLINK_MSG_ID_AUTH_KEY 7 4 | 5 | typedef struct __mavlink_auth_key_t 6 | { 7 | char key[32]; ///< key 8 | } mavlink_auth_key_t; 9 | 10 | #define MAVLINK_MSG_ID_AUTH_KEY_LEN 32 11 | #define MAVLINK_MSG_ID_7_LEN 32 12 | 13 | #define MAVLINK_MSG_ID_AUTH_KEY_CRC 119 14 | #define MAVLINK_MSG_ID_7_CRC 119 15 | 16 | #define MAVLINK_MSG_AUTH_KEY_FIELD_KEY_LEN 32 17 | 18 | #define MAVLINK_MESSAGE_INFO_AUTH_KEY { \ 19 | "AUTH_KEY", \ 20 | 1, \ 21 | { { "key", NULL, MAVLINK_TYPE_CHAR, 32, 0, offsetof(mavlink_auth_key_t, key) }, \ 22 | } \ 23 | } 24 | 25 | 26 | /** 27 | * @brief Pack a auth_key message 28 | * @param system_id ID of this system 29 | * @param component_id ID of this component (e.g. 200 for IMU) 30 | * @param msg The MAVLink message to compress the data into 31 | * 32 | * @param key key 33 | * @return length of the message in bytes (excluding serial stream start sign) 34 | */ 35 | static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 36 | const char *key) 37 | { 38 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 39 | char buf[MAVLINK_MSG_ID_AUTH_KEY_LEN]; 40 | 41 | _mav_put_char_array(buf, 0, key, 32); 42 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTH_KEY_LEN); 43 | #else 44 | mavlink_auth_key_t packet; 45 | 46 | mav_array_memcpy(packet.key, key, sizeof(char)*32); 47 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTH_KEY_LEN); 48 | #endif 49 | 50 | msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; 51 | #if MAVLINK_CRC_EXTRA 52 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); 53 | #else 54 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTH_KEY_LEN); 55 | #endif 56 | } 57 | 58 | /** 59 | * @brief Pack a auth_key message on a channel 60 | * @param system_id ID of this system 61 | * @param component_id ID of this component (e.g. 200 for IMU) 62 | * @param chan The MAVLink channel this message will be sent over 63 | * @param msg The MAVLink message to compress the data into 64 | * @param key key 65 | * @return length of the message in bytes (excluding serial stream start sign) 66 | */ 67 | static inline uint16_t mavlink_msg_auth_key_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 68 | mavlink_message_t* msg, 69 | const char *key) 70 | { 71 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 72 | char buf[MAVLINK_MSG_ID_AUTH_KEY_LEN]; 73 | 74 | _mav_put_char_array(buf, 0, key, 32); 75 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTH_KEY_LEN); 76 | #else 77 | mavlink_auth_key_t packet; 78 | 79 | mav_array_memcpy(packet.key, key, sizeof(char)*32); 80 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTH_KEY_LEN); 81 | #endif 82 | 83 | msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; 84 | #if MAVLINK_CRC_EXTRA 85 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); 86 | #else 87 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUTH_KEY_LEN); 88 | #endif 89 | } 90 | 91 | /** 92 | * @brief Encode a auth_key struct 93 | * 94 | * @param system_id ID of this system 95 | * @param component_id ID of this component (e.g. 200 for IMU) 96 | * @param msg The MAVLink message to compress the data into 97 | * @param auth_key C-struct to read the message contents from 98 | */ 99 | static inline uint16_t mavlink_msg_auth_key_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_auth_key_t* auth_key) 100 | { 101 | return mavlink_msg_auth_key_pack(system_id, component_id, msg, auth_key->key); 102 | } 103 | 104 | /** 105 | * @brief Encode a auth_key struct on a channel 106 | * 107 | * @param system_id ID of this system 108 | * @param component_id ID of this component (e.g. 200 for IMU) 109 | * @param chan The MAVLink channel this message will be sent over 110 | * @param msg The MAVLink message to compress the data into 111 | * @param auth_key C-struct to read the message contents from 112 | */ 113 | static inline uint16_t mavlink_msg_auth_key_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_auth_key_t* auth_key) 114 | { 115 | return mavlink_msg_auth_key_pack_chan(system_id, component_id, chan, msg, auth_key->key); 116 | } 117 | 118 | /** 119 | * @brief Send a auth_key message 120 | * @param chan MAVLink channel to send the message 121 | * 122 | * @param key key 123 | */ 124 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 125 | 126 | static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char *key) 127 | { 128 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 129 | char buf[MAVLINK_MSG_ID_AUTH_KEY_LEN]; 130 | 131 | _mav_put_char_array(buf, 0, key, 32); 132 | #if MAVLINK_CRC_EXTRA 133 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); 134 | #else 135 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, MAVLINK_MSG_ID_AUTH_KEY_LEN); 136 | #endif 137 | #else 138 | mavlink_auth_key_t packet; 139 | 140 | mav_array_memcpy(packet.key, key, sizeof(char)*32); 141 | #if MAVLINK_CRC_EXTRA 142 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)&packet, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); 143 | #else 144 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)&packet, MAVLINK_MSG_ID_AUTH_KEY_LEN); 145 | #endif 146 | #endif 147 | } 148 | 149 | #endif 150 | 151 | // MESSAGE AUTH_KEY UNPACKING 152 | 153 | 154 | /** 155 | * @brief Get field key from auth_key message 156 | * 157 | * @return key 158 | */ 159 | static inline uint16_t mavlink_msg_auth_key_get_key(const mavlink_message_t* msg, char *key) 160 | { 161 | return _MAV_RETURN_char_array(msg, key, 32, 0); 162 | } 163 | 164 | /** 165 | * @brief Decode a auth_key message into a struct 166 | * 167 | * @param msg The message to decode 168 | * @param auth_key C-struct to decode the message contents into 169 | */ 170 | static inline void mavlink_msg_auth_key_decode(const mavlink_message_t* msg, mavlink_auth_key_t* auth_key) 171 | { 172 | #if MAVLINK_NEED_BYTE_SWAP 173 | mavlink_msg_auth_key_get_key(msg, auth_key->key); 174 | #else 175 | memcpy(auth_key, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AUTH_KEY_LEN); 176 | #endif 177 | } 178 | -------------------------------------------------------------------------------- /mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h: -------------------------------------------------------------------------------- 1 | // MESSAGE OBS_WIND PACKING 2 | 3 | #define MAVLINK_MSG_ID_OBS_WIND 176 4 | 5 | typedef struct __mavlink_obs_wind_t 6 | { 7 | float wind[3]; ///< 8 | 9 | 10 | } mavlink_obs_wind_t; 11 | 12 | #define MAVLINK_MSG_ID_OBS_WIND_LEN 12 13 | #define MAVLINK_MSG_ID_176_LEN 12 14 | 15 | #define MAVLINK_MSG_ID_OBS_WIND_CRC 16 16 | #define MAVLINK_MSG_ID_176_CRC 16 17 | 18 | #define MAVLINK_MSG_OBS_WIND_FIELD_WIND_LEN 3 19 | 20 | #define MAVLINK_MESSAGE_INFO_OBS_WIND { \ 21 | "OBS_WIND", \ 22 | 1, \ 23 | { { "wind", NULL, MAVLINK_TYPE_FLOAT, 3, 0, offsetof(mavlink_obs_wind_t, wind) }, \ 24 | } \ 25 | } 26 | 27 | 28 | /** 29 | * @brief Pack a obs_wind message 30 | * @param system_id ID of this system 31 | * @param component_id ID of this component (e.g. 200 for IMU) 32 | * @param msg The MAVLink message to compress the data into 33 | * 34 | * @param wind 35 | 36 | 37 | * @return length of the message in bytes (excluding serial stream start sign) 38 | */ 39 | static inline uint16_t mavlink_msg_obs_wind_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 40 | const float *wind) 41 | { 42 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 43 | char buf[MAVLINK_MSG_ID_OBS_WIND_LEN]; 44 | 45 | _mav_put_float_array(buf, 0, wind, 3); 46 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_WIND_LEN); 47 | #else 48 | mavlink_obs_wind_t packet; 49 | 50 | mav_array_memcpy(packet.wind, wind, sizeof(float)*3); 51 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_WIND_LEN); 52 | #endif 53 | 54 | msg->msgid = MAVLINK_MSG_ID_OBS_WIND; 55 | #if MAVLINK_CRC_EXTRA 56 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_WIND_LEN, MAVLINK_MSG_ID_OBS_WIND_CRC); 57 | #else 58 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_WIND_LEN); 59 | #endif 60 | } 61 | 62 | /** 63 | * @brief Pack a obs_wind message on a channel 64 | * @param system_id ID of this system 65 | * @param component_id ID of this component (e.g. 200 for IMU) 66 | * @param chan The MAVLink channel this message will be sent over 67 | * @param msg The MAVLink message to compress the data into 68 | * @param wind 69 | 70 | 71 | * @return length of the message in bytes (excluding serial stream start sign) 72 | */ 73 | static inline uint16_t mavlink_msg_obs_wind_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 74 | mavlink_message_t* msg, 75 | const float *wind) 76 | { 77 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 78 | char buf[MAVLINK_MSG_ID_OBS_WIND_LEN]; 79 | 80 | _mav_put_float_array(buf, 0, wind, 3); 81 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_WIND_LEN); 82 | #else 83 | mavlink_obs_wind_t packet; 84 | 85 | mav_array_memcpy(packet.wind, wind, sizeof(float)*3); 86 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_WIND_LEN); 87 | #endif 88 | 89 | msg->msgid = MAVLINK_MSG_ID_OBS_WIND; 90 | #if MAVLINK_CRC_EXTRA 91 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_WIND_LEN, MAVLINK_MSG_ID_OBS_WIND_CRC); 92 | #else 93 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_WIND_LEN); 94 | #endif 95 | } 96 | 97 | /** 98 | * @brief Encode a obs_wind struct 99 | * 100 | * @param system_id ID of this system 101 | * @param component_id ID of this component (e.g. 200 for IMU) 102 | * @param msg The MAVLink message to compress the data into 103 | * @param obs_wind C-struct to read the message contents from 104 | */ 105 | static inline uint16_t mavlink_msg_obs_wind_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_wind_t* obs_wind) 106 | { 107 | return mavlink_msg_obs_wind_pack(system_id, component_id, msg, obs_wind->wind); 108 | } 109 | 110 | /** 111 | * @brief Encode a obs_wind struct on a channel 112 | * 113 | * @param system_id ID of this system 114 | * @param component_id ID of this component (e.g. 200 for IMU) 115 | * @param chan The MAVLink channel this message will be sent over 116 | * @param msg The MAVLink message to compress the data into 117 | * @param obs_wind C-struct to read the message contents from 118 | */ 119 | static inline uint16_t mavlink_msg_obs_wind_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_wind_t* obs_wind) 120 | { 121 | return mavlink_msg_obs_wind_pack_chan(system_id, component_id, chan, msg, obs_wind->wind); 122 | } 123 | 124 | /** 125 | * @brief Send a obs_wind message 126 | * @param chan MAVLink channel to send the message 127 | * 128 | * @param wind 129 | 130 | 131 | */ 132 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 133 | 134 | static inline void mavlink_msg_obs_wind_send(mavlink_channel_t chan, const float *wind) 135 | { 136 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 137 | char buf[MAVLINK_MSG_ID_OBS_WIND_LEN]; 138 | 139 | _mav_put_float_array(buf, 0, wind, 3); 140 | #if MAVLINK_CRC_EXTRA 141 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, buf, MAVLINK_MSG_ID_OBS_WIND_LEN, MAVLINK_MSG_ID_OBS_WIND_CRC); 142 | #else 143 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, buf, MAVLINK_MSG_ID_OBS_WIND_LEN); 144 | #endif 145 | #else 146 | mavlink_obs_wind_t packet; 147 | 148 | mav_array_memcpy(packet.wind, wind, sizeof(float)*3); 149 | #if MAVLINK_CRC_EXTRA 150 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, (const char *)&packet, MAVLINK_MSG_ID_OBS_WIND_LEN, MAVLINK_MSG_ID_OBS_WIND_CRC); 151 | #else 152 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, (const char *)&packet, MAVLINK_MSG_ID_OBS_WIND_LEN); 153 | #endif 154 | #endif 155 | } 156 | 157 | #endif 158 | 159 | // MESSAGE OBS_WIND UNPACKING 160 | 161 | 162 | /** 163 | * @brief Get field wind from obs_wind message 164 | * 165 | * @return 166 | 167 | 168 | */ 169 | static inline uint16_t mavlink_msg_obs_wind_get_wind(const mavlink_message_t* msg, float *wind) 170 | { 171 | return _MAV_RETURN_float_array(msg, wind, 3, 0); 172 | } 173 | 174 | /** 175 | * @brief Decode a obs_wind message into a struct 176 | * 177 | * @param msg The message to decode 178 | * @param obs_wind C-struct to decode the message contents into 179 | */ 180 | static inline void mavlink_msg_obs_wind_decode(const mavlink_message_t* msg, mavlink_obs_wind_t* obs_wind) 181 | { 182 | #if MAVLINK_NEED_BYTE_SWAP 183 | mavlink_msg_obs_wind_get_wind(msg, obs_wind->wind); 184 | #else 185 | memcpy(obs_wind, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_WIND_LEN); 186 | #endif 187 | } 188 | -------------------------------------------------------------------------------- /mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h: -------------------------------------------------------------------------------- 1 | // MESSAGE OBS_AIR_TEMP PACKING 2 | 3 | #define MAVLINK_MSG_ID_OBS_AIR_TEMP 183 4 | 5 | typedef struct __mavlink_obs_air_temp_t 6 | { 7 | float airT; ///< 8 | 9 | 10 | } mavlink_obs_air_temp_t; 11 | 12 | #define MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN 4 13 | #define MAVLINK_MSG_ID_183_LEN 4 14 | 15 | #define MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC 248 16 | #define MAVLINK_MSG_ID_183_CRC 248 17 | 18 | 19 | 20 | #define MAVLINK_MESSAGE_INFO_OBS_AIR_TEMP { \ 21 | "OBS_AIR_TEMP", \ 22 | 1, \ 23 | { { "airT", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_obs_air_temp_t, airT) }, \ 24 | } \ 25 | } 26 | 27 | 28 | /** 29 | * @brief Pack a obs_air_temp message 30 | * @param system_id ID of this system 31 | * @param component_id ID of this component (e.g. 200 for IMU) 32 | * @param msg The MAVLink message to compress the data into 33 | * 34 | * @param airT 35 | 36 | 37 | * @return length of the message in bytes (excluding serial stream start sign) 38 | */ 39 | static inline uint16_t mavlink_msg_obs_air_temp_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 40 | float airT) 41 | { 42 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 43 | char buf[MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN]; 44 | _mav_put_float(buf, 0, airT); 45 | 46 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); 47 | #else 48 | mavlink_obs_air_temp_t packet; 49 | packet.airT = airT; 50 | 51 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); 52 | #endif 53 | 54 | msg->msgid = MAVLINK_MSG_ID_OBS_AIR_TEMP; 55 | #if MAVLINK_CRC_EXTRA 56 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN, MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC); 57 | #else 58 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); 59 | #endif 60 | } 61 | 62 | /** 63 | * @brief Pack a obs_air_temp message on a channel 64 | * @param system_id ID of this system 65 | * @param component_id ID of this component (e.g. 200 for IMU) 66 | * @param chan The MAVLink channel this message will be sent over 67 | * @param msg The MAVLink message to compress the data into 68 | * @param airT 69 | 70 | 71 | * @return length of the message in bytes (excluding serial stream start sign) 72 | */ 73 | static inline uint16_t mavlink_msg_obs_air_temp_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 74 | mavlink_message_t* msg, 75 | float airT) 76 | { 77 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 78 | char buf[MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN]; 79 | _mav_put_float(buf, 0, airT); 80 | 81 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); 82 | #else 83 | mavlink_obs_air_temp_t packet; 84 | packet.airT = airT; 85 | 86 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); 87 | #endif 88 | 89 | msg->msgid = MAVLINK_MSG_ID_OBS_AIR_TEMP; 90 | #if MAVLINK_CRC_EXTRA 91 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN, MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC); 92 | #else 93 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); 94 | #endif 95 | } 96 | 97 | /** 98 | * @brief Encode a obs_air_temp struct 99 | * 100 | * @param system_id ID of this system 101 | * @param component_id ID of this component (e.g. 200 for IMU) 102 | * @param msg The MAVLink message to compress the data into 103 | * @param obs_air_temp C-struct to read the message contents from 104 | */ 105 | static inline uint16_t mavlink_msg_obs_air_temp_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_air_temp_t* obs_air_temp) 106 | { 107 | return mavlink_msg_obs_air_temp_pack(system_id, component_id, msg, obs_air_temp->airT); 108 | } 109 | 110 | /** 111 | * @brief Encode a obs_air_temp struct on a channel 112 | * 113 | * @param system_id ID of this system 114 | * @param component_id ID of this component (e.g. 200 for IMU) 115 | * @param chan The MAVLink channel this message will be sent over 116 | * @param msg The MAVLink message to compress the data into 117 | * @param obs_air_temp C-struct to read the message contents from 118 | */ 119 | static inline uint16_t mavlink_msg_obs_air_temp_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_air_temp_t* obs_air_temp) 120 | { 121 | return mavlink_msg_obs_air_temp_pack_chan(system_id, component_id, chan, msg, obs_air_temp->airT); 122 | } 123 | 124 | /** 125 | * @brief Send a obs_air_temp message 126 | * @param chan MAVLink channel to send the message 127 | * 128 | * @param airT 129 | 130 | 131 | */ 132 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 133 | 134 | static inline void mavlink_msg_obs_air_temp_send(mavlink_channel_t chan, float airT) 135 | { 136 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 137 | char buf[MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN]; 138 | _mav_put_float(buf, 0, airT); 139 | 140 | #if MAVLINK_CRC_EXTRA 141 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, buf, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN, MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC); 142 | #else 143 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, buf, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); 144 | #endif 145 | #else 146 | mavlink_obs_air_temp_t packet; 147 | packet.airT = airT; 148 | 149 | #if MAVLINK_CRC_EXTRA 150 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, (const char *)&packet, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN, MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC); 151 | #else 152 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, (const char *)&packet, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); 153 | #endif 154 | #endif 155 | } 156 | 157 | #endif 158 | 159 | // MESSAGE OBS_AIR_TEMP UNPACKING 160 | 161 | 162 | /** 163 | * @brief Get field airT from obs_air_temp message 164 | * 165 | * @return 166 | 167 | 168 | */ 169 | static inline float mavlink_msg_obs_air_temp_get_airT(const mavlink_message_t* msg) 170 | { 171 | return _MAV_RETURN_float(msg, 0); 172 | } 173 | 174 | /** 175 | * @brief Decode a obs_air_temp message into a struct 176 | * 177 | * @param msg The message to decode 178 | * @param obs_air_temp C-struct to decode the message contents into 179 | */ 180 | static inline void mavlink_msg_obs_air_temp_decode(const mavlink_message_t* msg, mavlink_obs_air_temp_t* obs_air_temp) 181 | { 182 | #if MAVLINK_NEED_BYTE_SWAP 183 | obs_air_temp->airT = mavlink_msg_obs_air_temp_get_airT(msg); 184 | #else 185 | memcpy(obs_air_temp, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); 186 | #endif 187 | } 188 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/examples/mavgraph.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | ''' 3 | graph a MAVLink log file 4 | Andrew Tridgell August 2011 5 | ''' 6 | 7 | import sys, struct, time, os, datetime 8 | import math, re 9 | import pylab, pytz, matplotlib 10 | from math import * 11 | 12 | # allow import from the parent directory, where mavlink.py is 13 | sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) 14 | 15 | from mavextra import * 16 | 17 | locator = None 18 | formatter = None 19 | 20 | def plotit(x, y, fields, colors=[]): 21 | '''plot a set of graphs using date for x axis''' 22 | global locator, formatter 23 | pylab.ion() 24 | fig = pylab.figure(num=1, figsize=(12,6)) 25 | ax1 = fig.gca() 26 | ax2 = None 27 | xrange = 0.0 28 | for i in range(0, len(fields)): 29 | if len(x[i]) == 0: continue 30 | if x[i][-1] - x[i][0] > xrange: 31 | xrange = x[i][-1] - x[i][0] 32 | xrange *= 24 * 60 * 60 33 | if formatter is None: 34 | if xrange < 1000: 35 | formatter = matplotlib.dates.DateFormatter('%H:%M:%S') 36 | else: 37 | formatter = matplotlib.dates.DateFormatter('%H:%M') 38 | interval = 1 39 | intervals = [ 1, 2, 5, 10, 15, 30, 60, 120, 240, 300, 600, 40 | 900, 1800, 3600, 7200, 5*3600, 10*3600, 24*3600 ] 41 | for interval in intervals: 42 | if xrange / interval < 15: 43 | break 44 | locator = matplotlib.dates.SecondLocator(interval=interval) 45 | ax1.xaxis.set_major_locator(locator) 46 | ax1.xaxis.set_major_formatter(formatter) 47 | empty = True 48 | ax1_labels = [] 49 | ax2_labels = [] 50 | for i in range(0, len(fields)): 51 | if len(x[i]) == 0: 52 | print("Failed to find any values for field %s" % fields[i]) 53 | continue 54 | if i < len(colors): 55 | color = colors[i] 56 | else: 57 | color = 'red' 58 | (tz, tzdst) = time.tzname 59 | if axes[i] == 2: 60 | if ax2 == None: 61 | ax2 = ax1.twinx() 62 | ax = ax2 63 | ax2.xaxis.set_major_locator(locator) 64 | ax2.xaxis.set_major_formatter(formatter) 65 | label = fields[i] 66 | if label.endswith(":2"): 67 | label = label[:-2] 68 | ax2_labels.append(label) 69 | else: 70 | ax1_labels.append(fields[i]) 71 | ax = ax1 72 | ax.plot_date(x[i], y[i], color=color, label=fields[i], 73 | linestyle='-', marker='None', tz=None) 74 | pylab.draw() 75 | empty = False 76 | if ax1_labels != []: 77 | ax1.legend(ax1_labels,loc=opts.legend) 78 | if ax2_labels != []: 79 | ax2.legend(ax2_labels,loc=opts.legend2) 80 | if empty: 81 | print("No data to graph") 82 | return 83 | 84 | 85 | from optparse import OptionParser 86 | parser = OptionParser("mavgraph.py [options] ") 87 | 88 | parser.add_option("--no-timestamps",dest="notimestamps", action='store_true', help="Log doesn't have timestamps") 89 | parser.add_option("--planner",dest="planner", action='store_true', help="use planner file format") 90 | parser.add_option("--condition",dest="condition", default=None, help="select packets by a condition") 91 | parser.add_option("--labels",dest="labels", default=None, help="comma separated field labels") 92 | parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") 93 | parser.add_option("--legend", default='upper left', help="default legend position") 94 | parser.add_option("--legend2", default='upper right', help="default legend2 position") 95 | (opts, args) = parser.parse_args() 96 | 97 | if opts.mav10: 98 | os.environ['MAVLINK10'] = '1' 99 | import mavutil 100 | 101 | if len(args) < 2: 102 | print("Usage: mavlogdump.py [options] ") 103 | sys.exit(1) 104 | 105 | filenames = [] 106 | fields = [] 107 | for f in args: 108 | if os.path.exists(f): 109 | filenames.append(f) 110 | else: 111 | fields.append(f) 112 | msg_types = set() 113 | multiplier = [] 114 | field_types = [] 115 | 116 | colors = [ 'red', 'green', 'blue', 'orange', 'olive', 'black', 'grey' ] 117 | 118 | # work out msg types we are interested in 119 | x = [] 120 | y = [] 121 | axes = [] 122 | first_only = [] 123 | re_caps = re.compile('[A-Z_]+') 124 | for f in fields: 125 | caps = set(re.findall(re_caps, f)) 126 | msg_types = msg_types.union(caps) 127 | field_types.append(caps) 128 | y.append([]) 129 | x.append([]) 130 | axes.append(1) 131 | first_only.append(False) 132 | 133 | def add_data(t, msg, vars): 134 | '''add some data''' 135 | mtype = msg.get_type() 136 | if mtype not in msg_types: 137 | return 138 | for i in range(0, len(fields)): 139 | if mtype not in field_types[i]: 140 | continue 141 | f = fields[i] 142 | if f.endswith(":2"): 143 | axes[i] = 2 144 | f = f[:-2] 145 | if f.endswith(":1"): 146 | first_only[i] = True 147 | f = f[:-2] 148 | v = mavutil.evaluate_expression(f, vars) 149 | if v is None: 150 | continue 151 | y[i].append(v) 152 | x[i].append(t) 153 | 154 | def process_file(filename): 155 | '''process one file''' 156 | print("Processing %s" % filename) 157 | mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps) 158 | vars = {} 159 | 160 | while True: 161 | msg = mlog.recv_match(opts.condition) 162 | if msg is None: break 163 | tdays = (msg._timestamp - time.timezone) / (24 * 60 * 60) 164 | tdays += 719163 # pylab wants it since 0001-01-01 165 | add_data(tdays, msg, mlog.messages) 166 | 167 | if len(filenames) == 0: 168 | print("No files to process") 169 | sys.exit(1) 170 | 171 | if opts.labels is not None: 172 | labels = opts.labels.split(',') 173 | if len(labels) != len(fields)*len(filenames): 174 | print("Number of labels (%u) must match number of fields (%u)" % ( 175 | len(labels), len(fields)*len(filenames))) 176 | sys.exit(1) 177 | else: 178 | labels = None 179 | 180 | for fi in range(0, len(filenames)): 181 | f = filenames[fi] 182 | process_file(f) 183 | for i in range(0, len(x)): 184 | if first_only[i] and fi != 0: 185 | x[i] = [] 186 | y[i] = [] 187 | if labels: 188 | lab = labels[fi*len(fields):(fi+1)*len(fields)] 189 | else: 190 | lab = fields[:] 191 | plotit(x, y, lab, colors=colors[fi*len(fields):]) 192 | for i in range(0, len(x)): 193 | x[i] = [] 194 | y[i] = [] 195 | pylab.show() 196 | raw_input('press enter to exit....') 197 | -------------------------------------------------------------------------------- /mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_types.h: -------------------------------------------------------------------------------- 1 | #ifndef MAVLINK_TYPES_H_ 2 | #define MAVLINK_TYPES_H_ 3 | 4 | #include 5 | 6 | #ifndef MAVLINK_MAX_PAYLOAD_LEN 7 | // it is possible to override this, but be careful! 8 | #define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length 9 | #endif 10 | 11 | #define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte) 12 | #define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + 1) ///< Length of all header bytes, including core and checksum 13 | #define MAVLINK_NUM_CHECKSUM_BYTES 2 14 | #define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES) 15 | 16 | #define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length 17 | 18 | #define MAVLINK_MSG_ID_EXTENDED_MESSAGE 255 19 | #define MAVLINK_EXTENDED_HEADER_LEN 14 20 | 21 | #if (defined _MSC_VER) | ((defined __APPLE__) & (defined __MACH__)) | (defined __linux__) 22 | /* full fledged 32bit++ OS */ 23 | #define MAVLINK_MAX_EXTENDED_PACKET_LEN 65507 24 | #else 25 | /* small microcontrollers */ 26 | #define MAVLINK_MAX_EXTENDED_PACKET_LEN 2048 27 | #endif 28 | 29 | #define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES) 30 | 31 | typedef struct param_union { 32 | union { 33 | float param_float; 34 | int32_t param_int32; 35 | uint32_t param_uint32; 36 | uint8_t param_uint8; 37 | uint8_t bytes[4]; 38 | }; 39 | uint8_t type; 40 | } mavlink_param_union_t; 41 | 42 | typedef struct __mavlink_system { 43 | uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function 44 | uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function 45 | uint8_t type; ///< Unused, can be used by user to store the system's type 46 | uint8_t state; ///< Unused, can be used by user to store the system's state 47 | uint8_t mode; ///< Unused, can be used by user to store the system's mode 48 | uint8_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode 49 | } mavlink_system_t; 50 | 51 | typedef struct __mavlink_message { 52 | uint16_t checksum; /// sent at end of packet 53 | uint8_t magic; ///< protocol magic marker 54 | uint8_t len; ///< Length of payload 55 | uint8_t seq; ///< Sequence of packet 56 | uint8_t sysid; ///< ID of message sender system/aircraft 57 | uint8_t compid; ///< ID of the message sender component 58 | uint8_t msgid; ///< ID of message in payload 59 | uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; 60 | } mavlink_message_t; 61 | 62 | 63 | typedef struct __mavlink_extended_message { 64 | mavlink_message_t base_msg; 65 | int32_t extended_payload_len; ///< Length of extended payload if any 66 | uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN]; 67 | } mavlink_extended_message_t; 68 | 69 | 70 | typedef enum { 71 | MAVLINK_TYPE_CHAR = 0, 72 | MAVLINK_TYPE_UINT8_T = 1, 73 | MAVLINK_TYPE_INT8_T = 2, 74 | MAVLINK_TYPE_UINT16_T = 3, 75 | MAVLINK_TYPE_INT16_T = 4, 76 | MAVLINK_TYPE_UINT32_T = 5, 77 | MAVLINK_TYPE_INT32_T = 6, 78 | MAVLINK_TYPE_UINT64_T = 7, 79 | MAVLINK_TYPE_INT64_T = 8, 80 | MAVLINK_TYPE_FLOAT = 9, 81 | MAVLINK_TYPE_DOUBLE = 10 82 | } mavlink_message_type_t; 83 | 84 | #define MAVLINK_MAX_FIELDS 64 85 | 86 | typedef struct __mavlink_field_info { 87 | const char *name; // name of this field 88 | const char *print_format; // printing format hint, or NULL 89 | mavlink_message_type_t type; // type of this field 90 | unsigned int array_length; // if non-zero, field is an array 91 | unsigned int wire_offset; // offset of each field in the payload 92 | unsigned int structure_offset; // offset in a C structure 93 | } mavlink_field_info_t; 94 | 95 | // note that in this structure the order of fields is the order 96 | // in the XML file, not necessary the wire order 97 | typedef struct __mavlink_message_info { 98 | const char *name; // name of the message 99 | unsigned num_fields; // how many fields in this message 100 | mavlink_field_info_t fields[MAVLINK_MAX_FIELDS]; // field information 101 | } mavlink_message_info_t; 102 | 103 | #define _MAV_PAYLOAD(msg) ((const char *)(&((msg)->payload64[0]))) 104 | #define _MAV_PAYLOAD_NON_CONST(msg) ((char *)(&((msg)->payload64[0]))) 105 | 106 | // checksum is immediately after the payload bytes 107 | #define mavlink_ck_a(msg) *((msg)->len + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg)) 108 | #define mavlink_ck_b(msg) *(((msg)->len+(uint16_t)1) + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg)) 109 | 110 | typedef enum { 111 | MAVLINK_COMM_0, 112 | MAVLINK_COMM_1, 113 | MAVLINK_COMM_2, 114 | MAVLINK_COMM_3 115 | } mavlink_channel_t; 116 | 117 | /* 118 | * applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number 119 | * of buffers they will use. If more are used, then the result will be 120 | * a stack overrun 121 | */ 122 | #ifndef MAVLINK_COMM_NUM_BUFFERS 123 | #if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) 124 | # define MAVLINK_COMM_NUM_BUFFERS 16 125 | #else 126 | # define MAVLINK_COMM_NUM_BUFFERS 4 127 | #endif 128 | #endif 129 | 130 | typedef enum { 131 | MAVLINK_PARSE_STATE_UNINIT=0, 132 | MAVLINK_PARSE_STATE_IDLE, 133 | MAVLINK_PARSE_STATE_GOT_STX, 134 | MAVLINK_PARSE_STATE_GOT_SEQ, 135 | MAVLINK_PARSE_STATE_GOT_LENGTH, 136 | MAVLINK_PARSE_STATE_GOT_SYSID, 137 | MAVLINK_PARSE_STATE_GOT_COMPID, 138 | MAVLINK_PARSE_STATE_GOT_MSGID, 139 | MAVLINK_PARSE_STATE_GOT_PAYLOAD, 140 | MAVLINK_PARSE_STATE_GOT_CRC1 141 | } mavlink_parse_state_t; ///< The state machine for the comm parser 142 | 143 | typedef struct __mavlink_status { 144 | uint8_t msg_received; ///< Number of received messages 145 | uint8_t buffer_overrun; ///< Number of buffer overruns 146 | uint8_t parse_error; ///< Number of parse errors 147 | mavlink_parse_state_t parse_state; ///< Parsing state machine 148 | uint8_t packet_idx; ///< Index in current packet 149 | uint8_t current_rx_seq; ///< Sequence number of last packet received 150 | uint8_t current_tx_seq; ///< Sequence number of last packet sent 151 | uint16_t packet_rx_success_count; ///< Received packets 152 | uint16_t packet_rx_drop_count; ///< Number of packet drops 153 | } mavlink_status_t; 154 | 155 | #define MAVLINK_BIG_ENDIAN 0 156 | #define MAVLINK_LITTLE_ENDIAN 1 157 | 158 | #endif /* MAVLINK_TYPES_H_ */ 159 | -------------------------------------------------------------------------------- /mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h: -------------------------------------------------------------------------------- 1 | // MESSAGE MISSION_CURRENT PACKING 2 | 3 | #define MAVLINK_MSG_ID_MISSION_CURRENT 42 4 | 5 | typedef struct __mavlink_mission_current_t 6 | { 7 | uint16_t seq; ///< Sequence 8 | } mavlink_mission_current_t; 9 | 10 | #define MAVLINK_MSG_ID_MISSION_CURRENT_LEN 2 11 | #define MAVLINK_MSG_ID_42_LEN 2 12 | 13 | #define MAVLINK_MSG_ID_MISSION_CURRENT_CRC 28 14 | #define MAVLINK_MSG_ID_42_CRC 28 15 | 16 | 17 | 18 | #define MAVLINK_MESSAGE_INFO_MISSION_CURRENT { \ 19 | "MISSION_CURRENT", \ 20 | 1, \ 21 | { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_current_t, seq) }, \ 22 | } \ 23 | } 24 | 25 | 26 | /** 27 | * @brief Pack a mission_current message 28 | * @param system_id ID of this system 29 | * @param component_id ID of this component (e.g. 200 for IMU) 30 | * @param msg The MAVLink message to compress the data into 31 | * 32 | * @param seq Sequence 33 | * @return length of the message in bytes (excluding serial stream start sign) 34 | */ 35 | static inline uint16_t mavlink_msg_mission_current_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 36 | uint16_t seq) 37 | { 38 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 39 | char buf[MAVLINK_MSG_ID_MISSION_CURRENT_LEN]; 40 | _mav_put_uint16_t(buf, 0, seq); 41 | 42 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); 43 | #else 44 | mavlink_mission_current_t packet; 45 | packet.seq = seq; 46 | 47 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); 48 | #endif 49 | 50 | msg->msgid = MAVLINK_MSG_ID_MISSION_CURRENT; 51 | #if MAVLINK_CRC_EXTRA 52 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); 53 | #else 54 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); 55 | #endif 56 | } 57 | 58 | /** 59 | * @brief Pack a mission_current message on a channel 60 | * @param system_id ID of this system 61 | * @param component_id ID of this component (e.g. 200 for IMU) 62 | * @param chan The MAVLink channel this message will be sent over 63 | * @param msg The MAVLink message to compress the data into 64 | * @param seq Sequence 65 | * @return length of the message in bytes (excluding serial stream start sign) 66 | */ 67 | static inline uint16_t mavlink_msg_mission_current_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 68 | mavlink_message_t* msg, 69 | uint16_t seq) 70 | { 71 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 72 | char buf[MAVLINK_MSG_ID_MISSION_CURRENT_LEN]; 73 | _mav_put_uint16_t(buf, 0, seq); 74 | 75 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); 76 | #else 77 | mavlink_mission_current_t packet; 78 | packet.seq = seq; 79 | 80 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); 81 | #endif 82 | 83 | msg->msgid = MAVLINK_MSG_ID_MISSION_CURRENT; 84 | #if MAVLINK_CRC_EXTRA 85 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); 86 | #else 87 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); 88 | #endif 89 | } 90 | 91 | /** 92 | * @brief Encode a mission_current struct 93 | * 94 | * @param system_id ID of this system 95 | * @param component_id ID of this component (e.g. 200 for IMU) 96 | * @param msg The MAVLink message to compress the data into 97 | * @param mission_current C-struct to read the message contents from 98 | */ 99 | static inline uint16_t mavlink_msg_mission_current_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_current_t* mission_current) 100 | { 101 | return mavlink_msg_mission_current_pack(system_id, component_id, msg, mission_current->seq); 102 | } 103 | 104 | /** 105 | * @brief Encode a mission_current struct on a channel 106 | * 107 | * @param system_id ID of this system 108 | * @param component_id ID of this component (e.g. 200 for IMU) 109 | * @param chan The MAVLink channel this message will be sent over 110 | * @param msg The MAVLink message to compress the data into 111 | * @param mission_current C-struct to read the message contents from 112 | */ 113 | static inline uint16_t mavlink_msg_mission_current_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_current_t* mission_current) 114 | { 115 | return mavlink_msg_mission_current_pack_chan(system_id, component_id, chan, msg, mission_current->seq); 116 | } 117 | 118 | /** 119 | * @brief Send a mission_current message 120 | * @param chan MAVLink channel to send the message 121 | * 122 | * @param seq Sequence 123 | */ 124 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 125 | 126 | static inline void mavlink_msg_mission_current_send(mavlink_channel_t chan, uint16_t seq) 127 | { 128 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 129 | char buf[MAVLINK_MSG_ID_MISSION_CURRENT_LEN]; 130 | _mav_put_uint16_t(buf, 0, seq); 131 | 132 | #if MAVLINK_CRC_EXTRA 133 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); 134 | #else 135 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); 136 | #endif 137 | #else 138 | mavlink_mission_current_t packet; 139 | packet.seq = seq; 140 | 141 | #if MAVLINK_CRC_EXTRA 142 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); 143 | #else 144 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); 145 | #endif 146 | #endif 147 | } 148 | 149 | #endif 150 | 151 | // MESSAGE MISSION_CURRENT UNPACKING 152 | 153 | 154 | /** 155 | * @brief Get field seq from mission_current message 156 | * 157 | * @return Sequence 158 | */ 159 | static inline uint16_t mavlink_msg_mission_current_get_seq(const mavlink_message_t* msg) 160 | { 161 | return _MAV_RETURN_uint16_t(msg, 0); 162 | } 163 | 164 | /** 165 | * @brief Decode a mission_current message into a struct 166 | * 167 | * @param msg The message to decode 168 | * @param mission_current C-struct to decode the message contents into 169 | */ 170 | static inline void mavlink_msg_mission_current_decode(const mavlink_message_t* msg, mavlink_mission_current_t* mission_current) 171 | { 172 | #if MAVLINK_NEED_BYTE_SWAP 173 | mission_current->seq = mavlink_msg_mission_current_get_seq(msg); 174 | #else 175 | memcpy(mission_current, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_CURRENT_LEN); 176 | #endif 177 | } 178 | -------------------------------------------------------------------------------- /mavlink/include/mavlink/v1.0/mavlink_types.h: -------------------------------------------------------------------------------- 1 | #ifndef MAVLINK_TYPES_H_ 2 | #define MAVLINK_TYPES_H_ 3 | 4 | #include 5 | 6 | #ifndef MAVLINK_MAX_PAYLOAD_LEN 7 | // it is possible to override this, but be careful! 8 | #define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length 9 | #endif 10 | 11 | #define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte) 12 | #define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + 1) ///< Length of all header bytes, including core and checksum 13 | #define MAVLINK_NUM_CHECKSUM_BYTES 2 14 | #define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES) 15 | 16 | #define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length 17 | 18 | #define MAVLINK_MSG_ID_EXTENDED_MESSAGE 255 19 | #define MAVLINK_EXTENDED_HEADER_LEN 14 20 | 21 | #if (defined _MSC_VER) | ((defined __APPLE__) & (defined __MACH__)) | (defined __linux__) 22 | /* full fledged 32bit++ OS */ 23 | #define MAVLINK_MAX_EXTENDED_PACKET_LEN 65507 24 | #else 25 | /* small microcontrollers */ 26 | #define MAVLINK_MAX_EXTENDED_PACKET_LEN 2048 27 | #endif 28 | 29 | #define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES) 30 | 31 | typedef struct param_union { 32 | union { 33 | float param_float; 34 | int32_t param_int32; 35 | uint32_t param_uint32; 36 | int16_t param_int16; 37 | uint16_t param_uint16; 38 | int8_t param_int8; 39 | uint8_t param_uint8; 40 | uint8_t bytes[4]; 41 | }; 42 | uint8_t type; 43 | } mavlink_param_union_t; 44 | 45 | typedef struct __mavlink_system { 46 | uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function 47 | uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function 48 | uint8_t type; ///< Unused, can be used by user to store the system's type 49 | uint8_t state; ///< Unused, can be used by user to store the system's state 50 | uint8_t mode; ///< Unused, can be used by user to store the system's mode 51 | uint32_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode 52 | } mavlink_system_t; 53 | 54 | typedef struct __mavlink_message { 55 | uint16_t checksum; /// sent at end of packet 56 | uint8_t magic; ///< protocol magic marker 57 | uint8_t len; ///< Length of payload 58 | uint8_t seq; ///< Sequence of packet 59 | uint8_t sysid; ///< ID of message sender system/aircraft 60 | uint8_t compid; ///< ID of the message sender component 61 | uint8_t msgid; ///< ID of message in payload 62 | uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; 63 | } mavlink_message_t; 64 | 65 | 66 | typedef struct __mavlink_extended_message { 67 | mavlink_message_t base_msg; 68 | int32_t extended_payload_len; ///< Length of extended payload if any 69 | uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN]; 70 | } mavlink_extended_message_t; 71 | 72 | 73 | typedef enum { 74 | MAVLINK_TYPE_CHAR = 0, 75 | MAVLINK_TYPE_UINT8_T = 1, 76 | MAVLINK_TYPE_INT8_T = 2, 77 | MAVLINK_TYPE_UINT16_T = 3, 78 | MAVLINK_TYPE_INT16_T = 4, 79 | MAVLINK_TYPE_UINT32_T = 5, 80 | MAVLINK_TYPE_INT32_T = 6, 81 | MAVLINK_TYPE_UINT64_T = 7, 82 | MAVLINK_TYPE_INT64_T = 8, 83 | MAVLINK_TYPE_FLOAT = 9, 84 | MAVLINK_TYPE_DOUBLE = 10 85 | } mavlink_message_type_t; 86 | 87 | #define MAVLINK_MAX_FIELDS 64 88 | 89 | typedef struct __mavlink_field_info { 90 | const char *name; // name of this field 91 | const char *print_format; // printing format hint, or NULL 92 | mavlink_message_type_t type; // type of this field 93 | unsigned int array_length; // if non-zero, field is an array 94 | unsigned int wire_offset; // offset of each field in the payload 95 | unsigned int structure_offset; // offset in a C structure 96 | } mavlink_field_info_t; 97 | 98 | // note that in this structure the order of fields is the order 99 | // in the XML file, not necessary the wire order 100 | typedef struct __mavlink_message_info { 101 | const char *name; // name of the message 102 | unsigned num_fields; // how many fields in this message 103 | mavlink_field_info_t fields[MAVLINK_MAX_FIELDS]; // field information 104 | } mavlink_message_info_t; 105 | 106 | #define _MAV_PAYLOAD(msg) ((const char *)(&((msg)->payload64[0]))) 107 | #define _MAV_PAYLOAD_NON_CONST(msg) ((char *)(&((msg)->payload64[0]))) 108 | 109 | // checksum is immediately after the payload bytes 110 | #define mavlink_ck_a(msg) *((msg)->len + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg)) 111 | #define mavlink_ck_b(msg) *(((msg)->len+(uint16_t)1) + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg)) 112 | 113 | typedef enum { 114 | MAVLINK_COMM_0, 115 | MAVLINK_COMM_1, 116 | MAVLINK_COMM_2, 117 | MAVLINK_COMM_3 118 | } mavlink_channel_t; 119 | 120 | /* 121 | * applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number 122 | * of buffers they will use. If more are used, then the result will be 123 | * a stack overrun 124 | */ 125 | #ifndef MAVLINK_COMM_NUM_BUFFERS 126 | #if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) 127 | # define MAVLINK_COMM_NUM_BUFFERS 16 128 | #else 129 | # define MAVLINK_COMM_NUM_BUFFERS 4 130 | #endif 131 | #endif 132 | 133 | typedef enum { 134 | MAVLINK_PARSE_STATE_UNINIT=0, 135 | MAVLINK_PARSE_STATE_IDLE, 136 | MAVLINK_PARSE_STATE_GOT_STX, 137 | MAVLINK_PARSE_STATE_GOT_SEQ, 138 | MAVLINK_PARSE_STATE_GOT_LENGTH, 139 | MAVLINK_PARSE_STATE_GOT_SYSID, 140 | MAVLINK_PARSE_STATE_GOT_COMPID, 141 | MAVLINK_PARSE_STATE_GOT_MSGID, 142 | MAVLINK_PARSE_STATE_GOT_PAYLOAD, 143 | MAVLINK_PARSE_STATE_GOT_CRC1 144 | } mavlink_parse_state_t; ///< The state machine for the comm parser 145 | 146 | typedef struct __mavlink_status { 147 | uint8_t msg_received; ///< Number of received messages 148 | uint8_t buffer_overrun; ///< Number of buffer overruns 149 | uint8_t parse_error; ///< Number of parse errors 150 | mavlink_parse_state_t parse_state; ///< Parsing state machine 151 | uint8_t packet_idx; ///< Index in current packet 152 | uint8_t current_rx_seq; ///< Sequence number of last packet received 153 | uint8_t current_tx_seq; ///< Sequence number of last packet sent 154 | uint16_t packet_rx_success_count; ///< Received packets 155 | uint16_t packet_rx_drop_count; ///< Number of packet drops 156 | } mavlink_status_t; 157 | 158 | #define MAVLINK_BIG_ENDIAN 0 159 | #define MAVLINK_LITTLE_ENDIAN 1 160 | 161 | #endif /* MAVLINK_TYPES_H_ */ 162 | -------------------------------------------------------------------------------- /mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h: -------------------------------------------------------------------------------- 1 | // MESSAGE OBS_VELOCITY PACKING 2 | 3 | #define MAVLINK_MSG_ID_OBS_VELOCITY 172 4 | 5 | typedef struct __mavlink_obs_velocity_t 6 | { 7 | float vel[3]; ///< 8 | 9 | 10 | } mavlink_obs_velocity_t; 11 | 12 | #define MAVLINK_MSG_ID_OBS_VELOCITY_LEN 12 13 | #define MAVLINK_MSG_ID_172_LEN 12 14 | 15 | #define MAVLINK_MSG_ID_OBS_VELOCITY_CRC 108 16 | #define MAVLINK_MSG_ID_172_CRC 108 17 | 18 | #define MAVLINK_MSG_OBS_VELOCITY_FIELD_VEL_LEN 3 19 | 20 | #define MAVLINK_MESSAGE_INFO_OBS_VELOCITY { \ 21 | "OBS_VELOCITY", \ 22 | 1, \ 23 | { { "vel", NULL, MAVLINK_TYPE_FLOAT, 3, 0, offsetof(mavlink_obs_velocity_t, vel) }, \ 24 | } \ 25 | } 26 | 27 | 28 | /** 29 | * @brief Pack a obs_velocity message 30 | * @param system_id ID of this system 31 | * @param component_id ID of this component (e.g. 200 for IMU) 32 | * @param msg The MAVLink message to compress the data into 33 | * 34 | * @param vel 35 | 36 | 37 | * @return length of the message in bytes (excluding serial stream start sign) 38 | */ 39 | static inline uint16_t mavlink_msg_obs_velocity_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 40 | const float *vel) 41 | { 42 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 43 | char buf[MAVLINK_MSG_ID_OBS_VELOCITY_LEN]; 44 | 45 | _mav_put_float_array(buf, 0, vel, 3); 46 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); 47 | #else 48 | mavlink_obs_velocity_t packet; 49 | 50 | mav_array_memcpy(packet.vel, vel, sizeof(float)*3); 51 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); 52 | #endif 53 | 54 | msg->msgid = MAVLINK_MSG_ID_OBS_VELOCITY; 55 | #if MAVLINK_CRC_EXTRA 56 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_VELOCITY_CRC); 57 | #else 58 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); 59 | #endif 60 | } 61 | 62 | /** 63 | * @brief Pack a obs_velocity message on a channel 64 | * @param system_id ID of this system 65 | * @param component_id ID of this component (e.g. 200 for IMU) 66 | * @param chan The MAVLink channel this message will be sent over 67 | * @param msg The MAVLink message to compress the data into 68 | * @param vel 69 | 70 | 71 | * @return length of the message in bytes (excluding serial stream start sign) 72 | */ 73 | static inline uint16_t mavlink_msg_obs_velocity_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 74 | mavlink_message_t* msg, 75 | const float *vel) 76 | { 77 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 78 | char buf[MAVLINK_MSG_ID_OBS_VELOCITY_LEN]; 79 | 80 | _mav_put_float_array(buf, 0, vel, 3); 81 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); 82 | #else 83 | mavlink_obs_velocity_t packet; 84 | 85 | mav_array_memcpy(packet.vel, vel, sizeof(float)*3); 86 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); 87 | #endif 88 | 89 | msg->msgid = MAVLINK_MSG_ID_OBS_VELOCITY; 90 | #if MAVLINK_CRC_EXTRA 91 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_VELOCITY_CRC); 92 | #else 93 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); 94 | #endif 95 | } 96 | 97 | /** 98 | * @brief Encode a obs_velocity struct 99 | * 100 | * @param system_id ID of this system 101 | * @param component_id ID of this component (e.g. 200 for IMU) 102 | * @param msg The MAVLink message to compress the data into 103 | * @param obs_velocity C-struct to read the message contents from 104 | */ 105 | static inline uint16_t mavlink_msg_obs_velocity_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_velocity_t* obs_velocity) 106 | { 107 | return mavlink_msg_obs_velocity_pack(system_id, component_id, msg, obs_velocity->vel); 108 | } 109 | 110 | /** 111 | * @brief Encode a obs_velocity struct on a channel 112 | * 113 | * @param system_id ID of this system 114 | * @param component_id ID of this component (e.g. 200 for IMU) 115 | * @param chan The MAVLink channel this message will be sent over 116 | * @param msg The MAVLink message to compress the data into 117 | * @param obs_velocity C-struct to read the message contents from 118 | */ 119 | static inline uint16_t mavlink_msg_obs_velocity_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_velocity_t* obs_velocity) 120 | { 121 | return mavlink_msg_obs_velocity_pack_chan(system_id, component_id, chan, msg, obs_velocity->vel); 122 | } 123 | 124 | /** 125 | * @brief Send a obs_velocity message 126 | * @param chan MAVLink channel to send the message 127 | * 128 | * @param vel 129 | 130 | 131 | */ 132 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 133 | 134 | static inline void mavlink_msg_obs_velocity_send(mavlink_channel_t chan, const float *vel) 135 | { 136 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 137 | char buf[MAVLINK_MSG_ID_OBS_VELOCITY_LEN]; 138 | 139 | _mav_put_float_array(buf, 0, vel, 3); 140 | #if MAVLINK_CRC_EXTRA 141 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, buf, MAVLINK_MSG_ID_OBS_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_VELOCITY_CRC); 142 | #else 143 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, buf, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); 144 | #endif 145 | #else 146 | mavlink_obs_velocity_t packet; 147 | 148 | mav_array_memcpy(packet.vel, vel, sizeof(float)*3); 149 | #if MAVLINK_CRC_EXTRA 150 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, (const char *)&packet, MAVLINK_MSG_ID_OBS_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_VELOCITY_CRC); 151 | #else 152 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, (const char *)&packet, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); 153 | #endif 154 | #endif 155 | } 156 | 157 | #endif 158 | 159 | // MESSAGE OBS_VELOCITY UNPACKING 160 | 161 | 162 | /** 163 | * @brief Get field vel from obs_velocity message 164 | * 165 | * @return 166 | 167 | 168 | */ 169 | static inline uint16_t mavlink_msg_obs_velocity_get_vel(const mavlink_message_t* msg, float *vel) 170 | { 171 | return _MAV_RETURN_float_array(msg, vel, 3, 0); 172 | } 173 | 174 | /** 175 | * @brief Decode a obs_velocity message into a struct 176 | * 177 | * @param msg The message to decode 178 | * @param obs_velocity C-struct to decode the message contents into 179 | */ 180 | static inline void mavlink_msg_obs_velocity_decode(const mavlink_message_t* msg, mavlink_obs_velocity_t* obs_velocity) 181 | { 182 | #if MAVLINK_NEED_BYTE_SWAP 183 | mavlink_msg_obs_velocity_get_vel(msg, obs_velocity->vel); 184 | #else 185 | memcpy(obs_velocity, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_VELOCITY_LEN); 186 | #endif 187 | } 188 | --------------------------------------------------------------------------------