├── src ├── mavlink_sha256.h ├── common │ ├── version.h │ ├── mavlink.h │ ├── mavlink_msg_auth_key.h │ ├── mavlink_msg_mission_current.h │ ├── mavlink_msg_timesync.h │ ├── mavlink_msg_mission_item_reached.h │ ├── mavlink_msg_terrain_check.h │ ├── mavlink_msg_log_erase.h │ ├── mavlink_msg_camera_trigger.h │ ├── mavlink_msg_statustext.h │ └── mavlink_msg_debug.h ├── icarous │ ├── version.h │ ├── mavlink.h │ ├── icarous.h │ ├── testsuite.h │ └── mavlink_msg_icarous_heartbeat.h ├── uAvionix │ ├── version.h │ ├── mavlink.h │ └── uAvionix.h ├── ardupilotmega │ ├── version.h │ ├── mavlink.h │ ├── mavlink_msg_rpm.h │ ├── mavlink_msg_hwstatus.h │ ├── mavlink_msg_rangefinder.h │ ├── mavlink_msg_battery2.h │ ├── mavlink_msg_data16.h │ ├── mavlink_msg_data32.h │ ├── mavlink_msg_data64.h │ └── mavlink_msg_data96.h ├── LICENSE.txt ├── mavlink_get_info.h ├── checksum.h └── mavlink_conversions.h ├── keywords.txt ├── RaspberryPi_Pixhawk_wiring1.jpg ├── library.properties ├── examples └── Acceleration │ └── Acceleration.ino ├── PixhawkArduinoMAVLink.h ├── LICENSE ├── guide.md ├── Readme.md └── PixhawkArduinoMAVLink.cpp /src/mavlink_sha256.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shashikg/PixhawkArduinoMAVLink/HEAD/src/mavlink_sha256.h -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | PixhawkArduinoMAVLink KEYWORD1 2 | begin KEYWORD2 3 | ReadAcceleration KEYWORD2 4 | Stream KEYWORD2 5 | -------------------------------------------------------------------------------- /RaspberryPi_Pixhawk_wiring1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shashikg/PixhawkArduinoMAVLink/HEAD/RaspberryPi_Pixhawk_wiring1.jpg -------------------------------------------------------------------------------- /src/common/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from common.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Sat Jun 23 2018" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/icarous/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from icarous.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Sat Jun 23 2018" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/uAvionix/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from uAvionix.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Sat Jun 23 2018" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/ardupilotmega/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from ardupilotmega.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Sat Jun 23 2018" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=PixhawkArduinoMAVLink 2 | version=1.0.1 3 | author=Shashi Kant Gupta 4 | url=https://github.com/shashikg/PixhawkArduinoMAVLink 5 | maintainer=Shashi Kant Gupta 6 | sentence=Arduino Library to receive Pixhawk sensor's data 7 | paragraph=Library for using Arduino to recieve Pixhawk's sensor data as well as some other usefull data which you might need. Uses MAVLink C headers files generated from the with the help of mavgenerator. 8 | category=Sensors 9 | architectures=* 10 | -------------------------------------------------------------------------------- /src/common/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from common.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_IDX 1 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "common.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/icarous/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from icarous.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_IDX 3 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "icarous.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/uAvionix/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from uAvionix.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_IDX 2 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "uAvionix.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/ardupilotmega/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from ardupilotmega.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_IDX 0 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "ardupilotmega.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /examples/Acceleration/Acceleration.ino: -------------------------------------------------------------------------------- 1 | // Example Code to read acceleration data from pixhawk 2 | // Check HardwareSerial connection for your Arduino board and connect them according to the connection shown in guide.md 3 | 4 | #include 5 | #include 6 | 7 | HardwareSerial &hs = Serial1; 8 | PixhawkArduinoMAVLink mav(hs); 9 | 10 | void setup(){ 11 | Serial.begin(57600); 12 | while(!mav.begin()){ 13 | Serial.println("Not Connected!"); 14 | delay(1000); 15 | } 16 | mav.Stream(); 17 | delay(2000); 18 | } 19 | 20 | void loop(){ 21 | float xacc, yacc, zacc; 22 | 23 | mav.ReadAcceleration(&xacc, &yacc, &zacc); 24 | Serial.print("X: "); Serial.print(xacc); 25 | Serial.print(" Y: "); Serial.print(yacc); 26 | Serial.print(" Z: "); Serial.println(zacc); 27 | 28 | delay(100); 29 | } 30 | -------------------------------------------------------------------------------- /PixhawkArduinoMAVLink.h: -------------------------------------------------------------------------------- 1 | /* 2 | PixhawkArduinoMAVLink.h - Library for using Arduino to recieve Pixhawk sensors data. 3 | Created by Shashi Kant, June 23, 2018. 4 | Using MAVLink C headers files generated from the ardupilotmega.xml with the help of mavgenerator. 5 | */ 6 | 7 | #ifndef PixhawkArduinoMAVLink_h 8 | #define PixhawkArduinoMAVLink_h 9 | 10 | #include "src/ardupilotmega/mavlink.h" 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | class PixhawkArduinoMAVLink 18 | { 19 | public: 20 | PixhawkArduinoMAVLink(HardwareSerial &hs); 21 | bool begin(); 22 | void ReadAcceleration(float *xacc, float *yacc, float *zacc); 23 | void Stream(); 24 | private: 25 | HardwareSerial* _MAVSerial; 26 | double MILLIG_TO_MS2; 27 | uint8_t system_id; 28 | uint8_t component_id; 29 | uint8_t type; 30 | uint8_t autopilot; 31 | uint8_t received_sysid; // Pixhawk sysid 32 | uint8_t received_compid; // Pixhawk compid 33 | }; 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Shashi Kant Gupta 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /guide.md: -------------------------------------------------------------------------------- 1 | # Guide 2 | ## Contents 3 | 4 | | Serial No. | Tutorial Name | Link | 5 | | ---------- | ------------- | ----- | 6 | | 1 | How to use this library? | [Link](#how-to-use-this-library) | 7 | | 2 | Reading Acceleration Data | [Link](#reading-acceleration-data) | 8 | 9 | ## How to use this library? 10 | Add the following header files to your arduino script: 11 | ``` 12 | #include 13 | #include 14 | ``` 15 | Define a ```HardwareSerial``` which is to be connected to Pixhawk see the exact connection for the Rpi on Pixhawk's website and replace Rpi with Arduino. 16 | 17 | ![Connection for Pixhawk 2](RaspberryPi_Pixhawk_wiring1.jpg) 18 | 19 | Create the PixhawkArduinoMAVLink object by using the following code: 20 | ``` 21 | HardwareSerial &hs = Serial1; 22 | PixhawkArduinoMAVLink mav(hs); 23 | ``` 24 | to start using it call the function: 25 | ``` 26 | mav.begin(); // to start the serial 27 | mav.Stream(); // to initialise Pixhawk and send a req to stream data 28 | ``` 29 | After that use the respective function which you need. See below for the use of different function. 30 | 31 | ## Reading Acceleration Data 32 | Gives the actual acceleration values after compensating **g** from the imu acceleration data! Uses Quaternion Attitude and High Res IMU Data from Pixhawk. Simply call the function your acceleration datas will be saved in your passed variables. 33 | 34 | **Function:** ```void ReadAcceleration(float *xacc, float *yacc, float *zacc)``` 35 | 36 | **Values to Pass:** 3 float address respectively for xacc, yacc, zacc. 37 | -------------------------------------------------------------------------------- /src/LICENSE.txt: -------------------------------------------------------------------------------- 1 | This repository contains the generator for the MAVLink protocol. The generator itself is 2 | (L)GPL v3 licensed, while the generated code is subject to different licenses: 3 | 4 | 5 | ======================================================================================== 6 | 7 | Exception to the (L)GPL v.3: 8 | 9 | As an exception, if you use this Software to compile your source code and 10 | portions of this Software, including messages ("the generator output"), are embedded 11 | into the binary product as a result, you may redistribute such product and such 12 | product is hereby licensed under the following MIT license: 13 | 14 | Permission is hereby granted, free of charge, to any person obtaining a copy 15 | of the generated software (the "Generated Software"), to deal 16 | in the Generated Software without restriction, including without limitation the 17 | rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 18 | copies of the Generated Software, and to permit persons to whom the Generated 19 | Software is furnished to do so, subject to the following conditions: 20 | 21 | The above copyright notice and this permission notice shall be included in 22 | all copies or substantial portions of the Generated Software. 23 | 24 | THE GENERATED SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 25 | OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 26 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 27 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 28 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 29 | OUT OF OR IN CONNECTION WITH THE GENERATED SOFTWARE OR THE USE OR OTHER DEALINGS 30 | IN THE GENERATED SOFTWARE. 31 | 32 | ========================================================================================= 33 | -------------------------------------------------------------------------------- /Readme.md: -------------------------------------------------------------------------------- 1 | # MAVLINK for Arduino Pixhawk Communication 2 | After scrolling over the internet and finding very less usefull stuffs about how to retrieve datas from Pixhawk to Arduino Board, I decided about writing a Arduino Library for this. Moreover, most of them have some of their own problem! Well Juan Pedro's Article really helped a lot! but that too didn't worked for me, maybe due to the older version of mavlink library! So, I started about writing my own solution to this. Using mavlink to generate C libs, I had written a whole bunch of Arduino library to hide lots of processing scenes which are working behind the window! so as to provide easy access to the usefull data which is required in different projects! 3 | 4 | **Created By:** Shashi Kant 5 | 6 | **Date Started:** 23/06/2018 7 | 8 | **Sources:** See the Links Below (Huge thanks to their contributors!). Special thanks to [Juan Pedro](https://discuss.ardupilot.org/u/jplopezll/) for his excellent article on [MAVLink and Arduino](https://discuss.ardupilot.org/t/mavlink-and-arduino-step-by-step/25566) 9 | 10 | **Project Status:** On Halt 11 | 12 | # Some Usefull Links: 13 | 1. [MAVLink and Arduino: step by step](https://discuss.ardupilot.org/t/mavlink-and-arduino-step-by-step/25566) 14 | 1. [MAVLink Developer Guide](https://mavlink.io/en/) 15 | 1. [MAVLink Step By Step](https://discuss.ardupilot.org/t/mavlink-step-by-step/9629) 16 | 1. [MAVLink Tutorial for Absolute Dummies (Part –I)](https://diydrones.com/group/arducopterusergroup/forum/topics/mavlink-tutorial-for-absolute-dummies-part-i?xg_source=activity) 17 | 1. [MAVLink Erle Robotics](http://erlerobot.github.io/erle_gitbook/en/mavlink/mavlink.html) 18 | 19 | # How to install this library? 20 | 1. Git Clone this repo or [download the zip version for it](https://github.com/shashikg/PixhawkArduinoMAVLink/archive/master.zip). 21 | 1. (In the Arduino IDE) Sketch > Include Library > Add .ZIP Library > select the downloaded file > Open 22 | 1. To use it follow this [Guide](guide.md) 23 | 24 | # License 25 | PixhawkArduinoMAVLink is licensed under the terms of the MIT License. It uses C-language version of MAVLink header libraries which are inside ```libraries/PixhawkArduinoMAVLink/src/``` directory generated using the mavlink generator. The generated files are also under MIT License 26 | 1. See the [LICENSE](LICENSE) file for this project. 27 | 1. [LICENSE.txt](libraries/PixhawkArduinoMAVLink/src/LICENSE.txt) file for the MIT License declaration by mavlink. See the mavlink's COPYING file for more information about license of MAVLink header files. 28 | -------------------------------------------------------------------------------- /src/mavlink_get_info.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifdef MAVLINK_USE_MESSAGE_INFO 4 | #define MAVLINK_HAVE_GET_MESSAGE_INFO 5 | 6 | /* 7 | return the message_info struct for a message 8 | */ 9 | MAVLINK_HELPER const mavlink_message_info_t *mavlink_get_message_info_by_id(uint32_t msgid) 10 | { 11 | static const mavlink_message_info_t mavlink_message_info[] = MAVLINK_MESSAGE_INFO; 12 | /* 13 | use a bisection search to find the right entry. A perfect hash may be better 14 | Note that this assumes the table is sorted with primary key msgid 15 | */ 16 | uint32_t low=0, high=sizeof(mavlink_message_info)/sizeof(mavlink_message_info[0]); 17 | while (low < high) { 18 | uint32_t mid = (low+1+high)/2; 19 | if (msgid < mavlink_message_info[mid].msgid) { 20 | high = mid-1; 21 | continue; 22 | } 23 | if (msgid > mavlink_message_info[mid].msgid) { 24 | low = mid; 25 | continue; 26 | } 27 | low = mid; 28 | break; 29 | } 30 | if (mavlink_message_info[low].msgid == msgid) { 31 | return &mavlink_message_info[low]; 32 | } 33 | return NULL; 34 | } 35 | 36 | /* 37 | return the message_info struct for a message 38 | */ 39 | MAVLINK_HELPER const mavlink_message_info_t *mavlink_get_message_info(const mavlink_message_t *msg) 40 | { 41 | return mavlink_get_message_info_by_id(msg->msgid); 42 | } 43 | 44 | /* 45 | return the message_info struct for a message 46 | */ 47 | MAVLINK_HELPER const mavlink_message_info_t *mavlink_get_message_info_by_name(const char *name) 48 | { 49 | static const struct { const char *name; uint32_t msgid; } mavlink_message_names[] = MAVLINK_MESSAGE_NAMES; 50 | /* 51 | use a bisection search to find the right entry. A perfect hash may be better 52 | Note that this assumes the table is sorted with primary key name 53 | */ 54 | uint32_t low=0, high=sizeof(mavlink_message_names)/sizeof(mavlink_message_names[0]); 55 | while (low < high) { 56 | uint32_t mid = (low+1+high)/2; 57 | int cmp = strcmp(mavlink_message_names[mid].name, name); 58 | if (cmp == 0) { 59 | return mavlink_get_message_info_by_id(mavlink_message_names[mid].msgid); 60 | } 61 | if (cmp > 0) { 62 | high = mid-1; 63 | } else { 64 | low = mid; 65 | } 66 | } 67 | return NULL; 68 | } 69 | #endif // MAVLINK_USE_MESSAGE_INFO 70 | 71 | 72 | -------------------------------------------------------------------------------- /src/checksum.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #if defined(MAVLINK_USE_CXX_NAMESPACE) 4 | namespace mavlink { 5 | #elif defined(__cplusplus) 6 | extern "C" { 7 | #endif 8 | 9 | // Visual Studio versions before 2010 don't have stdint.h, so we just error out. 10 | #if (defined _MSC_VER) && (_MSC_VER < 1600) 11 | #error "The C-MAVLink implementation requires Visual Studio 2010 or greater" 12 | #endif 13 | 14 | #include 15 | 16 | /** 17 | * 18 | * CALCULATE THE CHECKSUM 19 | * 20 | */ 21 | 22 | #define X25_INIT_CRC 0xffff 23 | #define X25_VALIDATE_CRC 0xf0b8 24 | 25 | #ifndef HAVE_CRC_ACCUMULATE 26 | /** 27 | * @brief Accumulate the X.25 CRC by adding one char at a time. 28 | * 29 | * The checksum function adds the hash of one char at a time to the 30 | * 16 bit checksum (uint16_t). 31 | * 32 | * @param data new char to hash 33 | * @param crcAccum the already accumulated checksum 34 | **/ 35 | static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) 36 | { 37 | /*Accumulate one byte of data into the CRC*/ 38 | uint8_t tmp; 39 | 40 | tmp = data ^ (uint8_t)(*crcAccum &0xff); 41 | tmp ^= (tmp<<4); 42 | *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); 43 | } 44 | #endif 45 | 46 | 47 | /** 48 | * @brief Initiliaze the buffer for the X.25 CRC 49 | * 50 | * @param crcAccum the 16 bit X.25 CRC 51 | */ 52 | static inline void crc_init(uint16_t* crcAccum) 53 | { 54 | *crcAccum = X25_INIT_CRC; 55 | } 56 | 57 | 58 | /** 59 | * @brief Calculates the X.25 checksum on a byte buffer 60 | * 61 | * @param pBuffer buffer containing the byte array to hash 62 | * @param length length of the byte array 63 | * @return the checksum over the buffer bytes 64 | **/ 65 | static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length) 66 | { 67 | uint16_t crcTmp; 68 | crc_init(&crcTmp); 69 | while (length--) { 70 | crc_accumulate(*pBuffer++, &crcTmp); 71 | } 72 | return crcTmp; 73 | } 74 | 75 | 76 | /** 77 | * @brief Accumulate the X.25 CRC by adding an array of bytes 78 | * 79 | * The checksum function adds the hash of one char at a time to the 80 | * 16 bit checksum (uint16_t). 81 | * 82 | * @param data new bytes to hash 83 | * @param crcAccum the already accumulated checksum 84 | **/ 85 | static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint16_t length) 86 | { 87 | const uint8_t *p = (const uint8_t *)pBuffer; 88 | while (length--) { 89 | crc_accumulate(*p++, crcAccum); 90 | } 91 | } 92 | 93 | #if defined(MAVLINK_USE_CXX_NAMESPACE) || defined(__cplusplus) 94 | } 95 | #endif 96 | -------------------------------------------------------------------------------- /src/icarous/icarous.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol generated from icarous.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_ICAROUS_H 7 | #define MAVLINK_ICAROUS_H 8 | 9 | #ifndef MAVLINK_H 10 | #error Wrong include order: MAVLINK_ICAROUS.H MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set ALL AND EVERY defines from MAVLINK.H manually accordingly, including the #define MAVLINK_H call. 11 | #endif 12 | 13 | #undef MAVLINK_THIS_XML_IDX 14 | #define MAVLINK_THIS_XML_IDX 3 15 | 16 | #ifdef __cplusplus 17 | extern "C" { 18 | #endif 19 | 20 | // MESSAGE LENGTHS AND CRCS 21 | 22 | #ifndef MAVLINK_MESSAGE_LENGTHS 23 | #define MAVLINK_MESSAGE_LENGTHS {} 24 | #endif 25 | 26 | #ifndef MAVLINK_MESSAGE_CRCS 27 | #define MAVLINK_MESSAGE_CRCS {{42000, 227, 1, 0, 0, 0}, {42001, 239, 46, 0, 0, 0}} 28 | #endif 29 | 30 | #include "../protocol.h" 31 | 32 | #define MAVLINK_ENABLED_ICAROUS 33 | 34 | // ENUM DEFINITIONS 35 | 36 | 37 | /** @brief */ 38 | #ifndef HAVE_ENUM_ICAROUS_TRACK_BAND_TYPES 39 | #define HAVE_ENUM_ICAROUS_TRACK_BAND_TYPES 40 | typedef enum ICAROUS_TRACK_BAND_TYPES 41 | { 42 | ICAROUS_TRACK_BAND_TYPE_NONE=0, /* | */ 43 | ICAROUS_TRACK_BAND_TYPE_NEAR=1, /* | */ 44 | ICAROUS_TRACK_BAND_TYPE_RECOVERY=2, /* | */ 45 | ICAROUS_TRACK_BAND_TYPES_ENUM_END=3, /* | */ 46 | } ICAROUS_TRACK_BAND_TYPES; 47 | #endif 48 | 49 | /** @brief */ 50 | #ifndef HAVE_ENUM_ICAROUS_FMS_STATE 51 | #define HAVE_ENUM_ICAROUS_FMS_STATE 52 | typedef enum ICAROUS_FMS_STATE 53 | { 54 | ICAROUS_FMS_STATE_IDLE=0, /* | */ 55 | ICAROUS_FMS_STATE_TAKEOFF=1, /* | */ 56 | ICAROUS_FMS_STATE_CLIMB=2, /* | */ 57 | ICAROUS_FMS_STATE_CRUISE=3, /* | */ 58 | ICAROUS_FMS_STATE_APPROACH=4, /* | */ 59 | ICAROUS_FMS_STATE_LAND=5, /* | */ 60 | ICAROUS_FMS_STATE_ENUM_END=6, /* | */ 61 | } ICAROUS_FMS_STATE; 62 | #endif 63 | 64 | // MAVLINK VERSION 65 | 66 | #ifndef MAVLINK_VERSION 67 | #define MAVLINK_VERSION 2 68 | #endif 69 | 70 | #if (MAVLINK_VERSION == 0) 71 | #undef MAVLINK_VERSION 72 | #define MAVLINK_VERSION 2 73 | #endif 74 | 75 | // MESSAGE DEFINITIONS 76 | #include "./mavlink_msg_icarous_heartbeat.h" 77 | #include "./mavlink_msg_icarous_kinematic_bands.h" 78 | 79 | // base include 80 | 81 | 82 | #undef MAVLINK_THIS_XML_IDX 83 | #define MAVLINK_THIS_XML_IDX 3 84 | 85 | #if MAVLINK_THIS_XML_IDX == MAVLINK_PRIMARY_XML_IDX 86 | # define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_ICAROUS_HEARTBEAT, MAVLINK_MESSAGE_INFO_ICAROUS_KINEMATIC_BANDS} 87 | # define MAVLINK_MESSAGE_NAMES {{ "ICAROUS_HEARTBEAT", 42000 }, { "ICAROUS_KINEMATIC_BANDS", 42001 }} 88 | # if MAVLINK_COMMAND_24BIT 89 | # include "../mavlink_get_info.h" 90 | # endif 91 | #endif 92 | 93 | #ifdef __cplusplus 94 | } 95 | #endif // __cplusplus 96 | #endif // MAVLINK_ICAROUS_H 97 | -------------------------------------------------------------------------------- /PixhawkArduinoMAVLink.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | PixhawkArduinoMAVLink.cpp - Library for using Arduino to recieve Pixhawk's sensor data as well as some other usefull data which you might need. 3 | Created by Shashi Kant, June 23, 2018. 4 | Using MAVLink C headers files generated from the ardupilotmega.xml with the help of mavgenerator. 5 | */ 6 | 7 | #include "PixhawkArduinoMAVLink.h" 8 | 9 | PixhawkArduinoMAVLink::PixhawkArduinoMAVLink(HardwareSerial &hs){ 10 | _MAVSerial = &hs; 11 | MILLIG_TO_MS2 = 9.80665 / 1000.0; 12 | system_id = 1; // Your i.e. Arduino sysid 13 | component_id = 158; // Your i.e. Arduino compid 14 | type = MAV_TYPE_QUADROTOR; 15 | autopilot = MAV_AUTOPILOT_INVALID; 16 | } 17 | 18 | bool PixhawkArduinoMAVLink::begin(){ 19 | _MAVSerial->begin(57600); 20 | if(_MAVSerial->available()<=0){ 21 | return 0; 22 | }else{ 23 | return 1; 24 | } 25 | } 26 | 27 | // At first we will send some HeartBeats to Pixhawk to check whether it's available or not?? 28 | // After that we will check for whether we are recieving HeartBeats back from Pixhawk if Yes, 29 | // We will note down its sysid and compid to send it a req to Stream Data. 30 | void PixhawkArduinoMAVLink::Stream(){ 31 | delay(2000); 32 | int flag=1; 33 | Serial.println("Sending Heartbeats..."); 34 | mavlink_message_t msghb; 35 | mavlink_heartbeat_t heartbeat; 36 | uint8_t bufhb[MAVLINK_MAX_PACKET_LEN]; 37 | mavlink_msg_heartbeat_pack(system_id, component_id, &msghb, type, autopilot, MAV_MODE_PREFLIGHT, 0, MAV_STATE_STANDBY); 38 | uint16_t lenhb = mavlink_msg_to_send_buffer(bufhb, &msghb); 39 | delay(1000); 40 | _MAVSerial->write(bufhb,lenhb); 41 | Serial.println("Heartbeats sent! Now will check for recieved heartbeats to record sysid and compid..."); 42 | 43 | // Looping untill we get the required data. 44 | while(flag==1){ 45 | delay(1); 46 | while(_MAVSerial->available()>0){ 47 | mavlink_message_t msgpx; 48 | mavlink_status_t statuspx; 49 | uint8_t ch = _MAVSerial->read(); 50 | if(mavlink_parse_char(MAVLINK_COMM_0, ch, &msgpx, &statuspx)){ 51 | Serial.println("Message Parsing Done!"); 52 | switch(msgpx.msgid){ 53 | case MAVLINK_MSG_ID_HEARTBEAT: 54 | { 55 | mavlink_heartbeat_t packet; 56 | mavlink_msg_heartbeat_decode(&msgpx, &packet); 57 | received_sysid = msgpx.sysid; // Pixhawk sysid 58 | received_compid = msgpx.compid; // Pixhawk compid 59 | Serial.println("sysid and compid successfully recorded"); 60 | flag = 0; 61 | break; 62 | } 63 | } 64 | } 65 | } 66 | } 67 | 68 | // Sending request for data stream... 69 | Serial.println("Now sending request for data stream..."); 70 | delay(2000); 71 | mavlink_message_t msgds; 72 | uint8_t bufds[MAVLINK_MAX_PACKET_LEN]; 73 | mavlink_msg_request_data_stream_pack(system_id, component_id, &msgds, received_sysid, received_compid, MAV_DATA_STREAM_ALL , 0x05, 1); 74 | uint16_t lends = mavlink_msg_to_send_buffer(bufds, &msgds); 75 | delay(1000); 76 | _MAVSerial->write(bufds,lends); 77 | Serial.println("Request sent! Now you are ready to recieve datas..."); 78 | 79 | } 80 | 81 | void PixhawkArduinoMAVLink::ReadAcceleration(float *xacc, float *yacc, float *zacc){ 82 | int flagI = 1; 83 | int flagA = 1; 84 | float xa, ya, za, q0, q1, q2, q3; 85 | 86 | while((flagI==1)||(flagA==1)){ 87 | delay(10); 88 | while(_MAVSerial->available() > 0){ 89 | mavlink_message_t msg; 90 | mavlink_status_t status1; 91 | uint8_t ch = _MAVSerial->read(); 92 | // Serial.println(ch); 93 | if(mavlink_parse_char(MAVLINK_COMM_0, ch, &msg, &status1)){ 94 | //Serial.println("Message Parsing Done!"); 95 | switch(msg.msgid){ 96 | case MAVLINK_MSG_ID_HIGHRES_IMU: 97 | { 98 | Serial.println("Sending Highres IMU Data"); 99 | mavlink_highres_imu_t data; 100 | mavlink_msg_highres_imu_decode(&msg, &data); 101 | xa = (data.xacc); 102 | ya = (data.yacc); 103 | za = (data.zacc); 104 | flagI = 0; 105 | break; 106 | } 107 | case MAVLINK_MSG_ID_ATTITUDE_QUATERNION: 108 | { 109 | Serial.println("Sending Quaternion Attitude"); 110 | mavlink_attitude_quaternion_t data; 111 | mavlink_msg_attitude_quaternion_decode(&msg, &data); 112 | q0 = data.q1; 113 | q1 = data.q2; 114 | q2 = data.q3; 115 | q3 = data.q4; 116 | flagA = 0; 117 | break; 118 | } 119 | } 120 | } 121 | } 122 | } 123 | 124 | *xacc = xa + (9.80665)*2*(q1*q3-q0*q2); 125 | *yacc = ya + (9.80665)*2*(q0*q1+q3*q2); 126 | *zacc = za + (9.80665)*(1-2*(q1*q1+q2*q2)); 127 | 128 | return; 129 | } 130 | -------------------------------------------------------------------------------- /src/mavlink_conversions.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifndef MAVLINK_NO_CONVERSION_HELPERS 4 | 5 | /* enable math defines on Windows */ 6 | #ifdef _MSC_VER 7 | #ifndef _USE_MATH_DEFINES 8 | #define _USE_MATH_DEFINES 9 | #endif 10 | #endif 11 | #include 12 | 13 | #ifndef M_PI_2 14 | #define M_PI_2 ((float)asin(1)) 15 | #endif 16 | 17 | /** 18 | * @file mavlink_conversions.h 19 | * 20 | * These conversion functions follow the NASA rotation standards definition file 21 | * available online. 22 | * 23 | * Their intent is to lower the barrier for MAVLink adopters to use gimbal-lock free 24 | * (both rotation matrices, sometimes called DCM, and quaternions are gimbal-lock free) 25 | * rotation representations. Euler angles (roll, pitch, yaw) will be phased out of the 26 | * protocol as widely as possible. 27 | * 28 | * @author James Goppert 29 | * @author Thomas Gubler 30 | */ 31 | 32 | 33 | /** 34 | * Converts a quaternion to a rotation matrix 35 | * 36 | * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0) 37 | * @param dcm a 3x3 rotation matrix 38 | */ 39 | MAVLINK_HELPER void mavlink_quaternion_to_dcm(const float quaternion[4], float dcm[3][3]) 40 | { 41 | double a = quaternion[0]; 42 | double b = quaternion[1]; 43 | double c = quaternion[2]; 44 | double d = quaternion[3]; 45 | double aSq = a * a; 46 | double bSq = b * b; 47 | double cSq = c * c; 48 | double dSq = d * d; 49 | dcm[0][0] = aSq + bSq - cSq - dSq; 50 | dcm[0][1] = 2 * (b * c - a * d); 51 | dcm[0][2] = 2 * (a * c + b * d); 52 | dcm[1][0] = 2 * (b * c + a * d); 53 | dcm[1][1] = aSq - bSq + cSq - dSq; 54 | dcm[1][2] = 2 * (c * d - a * b); 55 | dcm[2][0] = 2 * (b * d - a * c); 56 | dcm[2][1] = 2 * (a * b + c * d); 57 | dcm[2][2] = aSq - bSq - cSq + dSq; 58 | } 59 | 60 | 61 | /** 62 | * Converts a rotation matrix to euler angles 63 | * 64 | * @param dcm a 3x3 rotation matrix 65 | * @param roll the roll angle in radians 66 | * @param pitch the pitch angle in radians 67 | * @param yaw the yaw angle in radians 68 | */ 69 | MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, float* pitch, float* yaw) 70 | { 71 | float phi, theta, psi; 72 | theta = asinf(-dcm[2][0]); 73 | 74 | if (fabsf(theta - (float)M_PI_2) < 1.0e-3f) { 75 | phi = 0.0f; 76 | psi = (atan2f(dcm[1][2] - dcm[0][1], 77 | dcm[0][2] + dcm[1][1]) + phi); 78 | 79 | } else if (fabsf(theta + (float)M_PI_2) < 1.0e-3f) { 80 | phi = 0.0f; 81 | psi = atan2f(dcm[1][2] - dcm[0][1], 82 | dcm[0][2] + dcm[1][1] - phi); 83 | 84 | } else { 85 | phi = atan2f(dcm[2][1], dcm[2][2]); 86 | psi = atan2f(dcm[1][0], dcm[0][0]); 87 | } 88 | 89 | *roll = phi; 90 | *pitch = theta; 91 | *yaw = psi; 92 | } 93 | 94 | 95 | /** 96 | * Converts a quaternion to euler angles 97 | * 98 | * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0) 99 | * @param roll the roll angle in radians 100 | * @param pitch the pitch angle in radians 101 | * @param yaw the yaw angle in radians 102 | */ 103 | MAVLINK_HELPER void mavlink_quaternion_to_euler(const float quaternion[4], float* roll, float* pitch, float* yaw) 104 | { 105 | float dcm[3][3]; 106 | mavlink_quaternion_to_dcm(quaternion, dcm); 107 | mavlink_dcm_to_euler((const float(*)[3])dcm, roll, pitch, yaw); 108 | } 109 | 110 | 111 | /** 112 | * Converts euler angles to a quaternion 113 | * 114 | * @param roll the roll angle in radians 115 | * @param pitch the pitch angle in radians 116 | * @param yaw the yaw angle in radians 117 | * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0) 118 | */ 119 | MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float yaw, float quaternion[4]) 120 | { 121 | float cosPhi_2 = cosf(roll / 2); 122 | float sinPhi_2 = sinf(roll / 2); 123 | float cosTheta_2 = cosf(pitch / 2); 124 | float sinTheta_2 = sinf(pitch / 2); 125 | float cosPsi_2 = cosf(yaw / 2); 126 | float sinPsi_2 = sinf(yaw / 2); 127 | quaternion[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 + 128 | sinPhi_2 * sinTheta_2 * sinPsi_2); 129 | quaternion[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 - 130 | cosPhi_2 * sinTheta_2 * sinPsi_2); 131 | quaternion[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 + 132 | sinPhi_2 * cosTheta_2 * sinPsi_2); 133 | quaternion[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 - 134 | sinPhi_2 * sinTheta_2 * cosPsi_2); 135 | } 136 | 137 | 138 | /** 139 | * Converts a rotation matrix to a quaternion 140 | * Reference: 141 | * - Shoemake, Quaternions, 142 | * http://www.cs.ucr.edu/~vbz/resources/quatut.pdf 143 | * 144 | * @param dcm a 3x3 rotation matrix 145 | * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0) 146 | */ 147 | MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quaternion[4]) 148 | { 149 | float tr = dcm[0][0] + dcm[1][1] + dcm[2][2]; 150 | if (tr > 0.0f) { 151 | float s = sqrtf(tr + 1.0f); 152 | quaternion[0] = s * 0.5f; 153 | s = 0.5f / s; 154 | quaternion[1] = (dcm[2][1] - dcm[1][2]) * s; 155 | quaternion[2] = (dcm[0][2] - dcm[2][0]) * s; 156 | quaternion[3] = (dcm[1][0] - dcm[0][1]) * s; 157 | } else { 158 | /* Find maximum diagonal element in dcm 159 | * store index in dcm_i */ 160 | int dcm_i = 0; 161 | int i; 162 | for (i = 1; i < 3; i++) { 163 | if (dcm[i][i] > dcm[dcm_i][dcm_i]) { 164 | dcm_i = i; 165 | } 166 | } 167 | 168 | int dcm_j = (dcm_i + 1) % 3; 169 | int dcm_k = (dcm_i + 2) % 3; 170 | 171 | float s = sqrtf((dcm[dcm_i][dcm_i] - dcm[dcm_j][dcm_j] - 172 | dcm[dcm_k][dcm_k]) + 1.0f); 173 | quaternion[dcm_i + 1] = s * 0.5f; 174 | s = 0.5f / s; 175 | quaternion[dcm_j + 1] = (dcm[dcm_i][dcm_j] + dcm[dcm_j][dcm_i]) * s; 176 | quaternion[dcm_k + 1] = (dcm[dcm_k][dcm_i] + dcm[dcm_i][dcm_k]) * s; 177 | quaternion[0] = (dcm[dcm_k][dcm_j] - dcm[dcm_j][dcm_k]) * s; 178 | } 179 | } 180 | 181 | 182 | /** 183 | * Converts euler angles to a rotation matrix 184 | * 185 | * @param roll the roll angle in radians 186 | * @param pitch the pitch angle in radians 187 | * @param yaw the yaw angle in radians 188 | * @param dcm a 3x3 rotation matrix 189 | */ 190 | MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, float dcm[3][3]) 191 | { 192 | float cosPhi = cosf(roll); 193 | float sinPhi = sinf(roll); 194 | float cosThe = cosf(pitch); 195 | float sinThe = sinf(pitch); 196 | float cosPsi = cosf(yaw); 197 | float sinPsi = sinf(yaw); 198 | 199 | dcm[0][0] = cosThe * cosPsi; 200 | dcm[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; 201 | dcm[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi; 202 | 203 | dcm[1][0] = cosThe * sinPsi; 204 | dcm[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi; 205 | dcm[1][2] = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi; 206 | 207 | dcm[2][0] = -sinThe; 208 | dcm[2][1] = sinPhi * cosThe; 209 | dcm[2][2] = cosPhi * cosThe; 210 | } 211 | 212 | #endif // MAVLINK_NO_CONVERSION_HELPERS 213 | -------------------------------------------------------------------------------- /src/icarous/testsuite.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol testsuite generated from icarous.xml 3 | * @see http://qgroundcontrol.org/mavlink/ 4 | */ 5 | #pragma once 6 | #ifndef ICAROUS_TESTSUITE_H 7 | #define ICAROUS_TESTSUITE_H 8 | 9 | #ifdef __cplusplus 10 | extern "C" { 11 | #endif 12 | 13 | #ifndef MAVLINK_TEST_ALL 14 | #define MAVLINK_TEST_ALL 15 | 16 | static void mavlink_test_icarous(uint8_t, uint8_t, mavlink_message_t *last_msg); 17 | 18 | static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) 19 | { 20 | 21 | mavlink_test_icarous(system_id, component_id, last_msg); 22 | } 23 | #endif 24 | 25 | 26 | 27 | 28 | static void mavlink_test_icarous_heartbeat(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) 29 | { 30 | #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 31 | mavlink_status_t *status = mavlink_get_channel_status(MAVLINK_COMM_0); 32 | if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_ICAROUS_HEARTBEAT >= 256) { 33 | return; 34 | } 35 | #endif 36 | mavlink_message_t msg; 37 | uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; 38 | uint16_t i; 39 | mavlink_icarous_heartbeat_t packet_in = { 40 | 5 41 | }; 42 | mavlink_icarous_heartbeat_t packet1, packet2; 43 | memset(&packet1, 0, sizeof(packet1)); 44 | packet1.status = packet_in.status; 45 | 46 | 47 | #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 48 | if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { 49 | // cope with extensions 50 | memset(MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_MIN_LEN); 51 | } 52 | #endif 53 | memset(&packet2, 0, sizeof(packet2)); 54 | mavlink_msg_icarous_heartbeat_encode(system_id, component_id, &msg, &packet1); 55 | mavlink_msg_icarous_heartbeat_decode(&msg, &packet2); 56 | MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); 57 | 58 | memset(&packet2, 0, sizeof(packet2)); 59 | mavlink_msg_icarous_heartbeat_pack(system_id, component_id, &msg , packet1.status ); 60 | mavlink_msg_icarous_heartbeat_decode(&msg, &packet2); 61 | MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); 62 | 63 | memset(&packet2, 0, sizeof(packet2)); 64 | mavlink_msg_icarous_heartbeat_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.status ); 65 | mavlink_msg_icarous_heartbeat_decode(&msg, &packet2); 66 | MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); 67 | 68 | memset(&packet2, 0, sizeof(packet2)); 69 | mavlink_msg_to_send_buffer(buffer, &msg); 70 | for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_ICAROUS_KINEMATIC_BANDS >= 256) { 87 | return; 88 | } 89 | #endif 90 | mavlink_message_t msg; 91 | uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; 92 | uint16_t i; 93 | mavlink_icarous_kinematic_bands_t packet_in = { 94 | 17.0,45.0,73.0,101.0,129.0,157.0,185.0,213.0,241.0,269.0,125,192,3,70,137,204 95 | }; 96 | mavlink_icarous_kinematic_bands_t packet1, packet2; 97 | memset(&packet1, 0, sizeof(packet1)); 98 | packet1.min1 = packet_in.min1; 99 | packet1.max1 = packet_in.max1; 100 | packet1.min2 = packet_in.min2; 101 | packet1.max2 = packet_in.max2; 102 | packet1.min3 = packet_in.min3; 103 | packet1.max3 = packet_in.max3; 104 | packet1.min4 = packet_in.min4; 105 | packet1.max4 = packet_in.max4; 106 | packet1.min5 = packet_in.min5; 107 | packet1.max5 = packet_in.max5; 108 | packet1.numBands = packet_in.numBands; 109 | packet1.type1 = packet_in.type1; 110 | packet1.type2 = packet_in.type2; 111 | packet1.type3 = packet_in.type3; 112 | packet1.type4 = packet_in.type4; 113 | packet1.type5 = packet_in.type5; 114 | 115 | 116 | #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 117 | if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { 118 | // cope with extensions 119 | memset(MAVLINK_MSG_ID_ICAROUS_KINEMATIC_BANDS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ICAROUS_KINEMATIC_BANDS_MIN_LEN); 120 | } 121 | #endif 122 | memset(&packet2, 0, sizeof(packet2)); 123 | mavlink_msg_icarous_kinematic_bands_encode(system_id, component_id, &msg, &packet1); 124 | mavlink_msg_icarous_kinematic_bands_decode(&msg, &packet2); 125 | MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); 126 | 127 | memset(&packet2, 0, sizeof(packet2)); 128 | mavlink_msg_icarous_kinematic_bands_pack(system_id, component_id, &msg , packet1.numBands , packet1.type1 , packet1.min1 , packet1.max1 , packet1.type2 , packet1.min2 , packet1.max2 , packet1.type3 , packet1.min3 , packet1.max3 , packet1.type4 , packet1.min4 , packet1.max4 , packet1.type5 , packet1.min5 , packet1.max5 ); 129 | mavlink_msg_icarous_kinematic_bands_decode(&msg, &packet2); 130 | MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); 131 | 132 | memset(&packet2, 0, sizeof(packet2)); 133 | mavlink_msg_icarous_kinematic_bands_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.numBands , packet1.type1 , packet1.min1 , packet1.max1 , packet1.type2 , packet1.min2 , packet1.max2 , packet1.type3 , packet1.min3 , packet1.max3 , packet1.type4 , packet1.min4 , packet1.max4 , packet1.type5 , packet1.min5 , packet1.max5 ); 134 | mavlink_msg_icarous_kinematic_bands_decode(&msg, &packet2); 135 | MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); 136 | 137 | memset(&packet2, 0, sizeof(packet2)); 138 | mavlink_msg_to_send_buffer(buffer, &msg); 139 | for (i=0; imsgid = MAVLINK_MSG_ID_AUTH_KEY; 63 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTH_KEY_MIN_LEN, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); 64 | } 65 | 66 | /** 67 | * @brief Pack a auth_key message on a channel 68 | * @param system_id ID of this system 69 | * @param component_id ID of this component (e.g. 200 for IMU) 70 | * @param chan The MAVLink channel this message will be sent over 71 | * @param msg The MAVLink message to compress the data into 72 | * @param key key 73 | * @return length of the message in bytes (excluding serial stream start sign) 74 | */ 75 | static inline uint16_t mavlink_msg_auth_key_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 76 | mavlink_message_t* msg, 77 | const char *key) 78 | { 79 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 80 | char buf[MAVLINK_MSG_ID_AUTH_KEY_LEN]; 81 | 82 | _mav_put_char_array(buf, 0, key, 32); 83 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTH_KEY_LEN); 84 | #else 85 | mavlink_auth_key_t packet; 86 | 87 | mav_array_memcpy(packet.key, key, sizeof(char)*32); 88 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTH_KEY_LEN); 89 | #endif 90 | 91 | msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; 92 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUTH_KEY_MIN_LEN, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); 93 | } 94 | 95 | /** 96 | * @brief Encode a auth_key struct 97 | * 98 | * @param system_id ID of this system 99 | * @param component_id ID of this component (e.g. 200 for IMU) 100 | * @param msg The MAVLink message to compress the data into 101 | * @param auth_key C-struct to read the message contents from 102 | */ 103 | 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) 104 | { 105 | return mavlink_msg_auth_key_pack(system_id, component_id, msg, auth_key->key); 106 | } 107 | 108 | /** 109 | * @brief Encode a auth_key struct on a channel 110 | * 111 | * @param system_id ID of this system 112 | * @param component_id ID of this component (e.g. 200 for IMU) 113 | * @param chan The MAVLink channel this message will be sent over 114 | * @param msg The MAVLink message to compress the data into 115 | * @param auth_key C-struct to read the message contents from 116 | */ 117 | 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) 118 | { 119 | return mavlink_msg_auth_key_pack_chan(system_id, component_id, chan, msg, auth_key->key); 120 | } 121 | 122 | /** 123 | * @brief Send a auth_key message 124 | * @param chan MAVLink channel to send the message 125 | * 126 | * @param key key 127 | */ 128 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 129 | 130 | static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char *key) 131 | { 132 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 133 | char buf[MAVLINK_MSG_ID_AUTH_KEY_LEN]; 134 | 135 | _mav_put_char_array(buf, 0, key, 32); 136 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, MAVLINK_MSG_ID_AUTH_KEY_MIN_LEN, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); 137 | #else 138 | mavlink_auth_key_t packet; 139 | 140 | mav_array_memcpy(packet.key, key, sizeof(char)*32); 141 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)&packet, MAVLINK_MSG_ID_AUTH_KEY_MIN_LEN, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); 142 | #endif 143 | } 144 | 145 | /** 146 | * @brief Send a auth_key message 147 | * @param chan MAVLink channel to send the message 148 | * @param struct The MAVLink struct to serialize 149 | */ 150 | static inline void mavlink_msg_auth_key_send_struct(mavlink_channel_t chan, const mavlink_auth_key_t* auth_key) 151 | { 152 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 153 | mavlink_msg_auth_key_send(chan, auth_key->key); 154 | #else 155 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)auth_key, MAVLINK_MSG_ID_AUTH_KEY_MIN_LEN, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); 156 | #endif 157 | } 158 | 159 | #if MAVLINK_MSG_ID_AUTH_KEY_LEN <= MAVLINK_MAX_PAYLOAD_LEN 160 | /* 161 | This varient of _send() can be used to save stack space by re-using 162 | memory from the receive buffer. The caller provides a 163 | mavlink_message_t which is the size of a full mavlink message. This 164 | is usually the receive buffer for the channel, and allows a reply to an 165 | incoming message with minimum stack space usage. 166 | */ 167 | static inline void mavlink_msg_auth_key_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const char *key) 168 | { 169 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 170 | char *buf = (char *)msgbuf; 171 | 172 | _mav_put_char_array(buf, 0, key, 32); 173 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, MAVLINK_MSG_ID_AUTH_KEY_MIN_LEN, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); 174 | #else 175 | mavlink_auth_key_t *packet = (mavlink_auth_key_t *)msgbuf; 176 | 177 | mav_array_memcpy(packet->key, key, sizeof(char)*32); 178 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)packet, MAVLINK_MSG_ID_AUTH_KEY_MIN_LEN, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); 179 | #endif 180 | } 181 | #endif 182 | 183 | #endif 184 | 185 | // MESSAGE AUTH_KEY UNPACKING 186 | 187 | 188 | /** 189 | * @brief Get field key from auth_key message 190 | * 191 | * @return key 192 | */ 193 | static inline uint16_t mavlink_msg_auth_key_get_key(const mavlink_message_t* msg, char *key) 194 | { 195 | return _MAV_RETURN_char_array(msg, key, 32, 0); 196 | } 197 | 198 | /** 199 | * @brief Decode a auth_key message into a struct 200 | * 201 | * @param msg The message to decode 202 | * @param auth_key C-struct to decode the message contents into 203 | */ 204 | static inline void mavlink_msg_auth_key_decode(const mavlink_message_t* msg, mavlink_auth_key_t* auth_key) 205 | { 206 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 207 | mavlink_msg_auth_key_get_key(msg, auth_key->key); 208 | #else 209 | uint8_t len = msg->len < MAVLINK_MSG_ID_AUTH_KEY_LEN? msg->len : MAVLINK_MSG_ID_AUTH_KEY_LEN; 210 | memset(auth_key, 0, MAVLINK_MSG_ID_AUTH_KEY_LEN); 211 | memcpy(auth_key, _MAV_PAYLOAD(msg), len); 212 | #endif 213 | } 214 | -------------------------------------------------------------------------------- /src/ardupilotmega/mavlink_msg_rpm.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // MESSAGE RPM PACKING 3 | 4 | #define MAVLINK_MSG_ID_RPM 226 5 | 6 | MAVPACKED( 7 | typedef struct __mavlink_rpm_t { 8 | float rpm1; /*< RPM Sensor1*/ 9 | float rpm2; /*< RPM Sensor2*/ 10 | }) mavlink_rpm_t; 11 | 12 | #define MAVLINK_MSG_ID_RPM_LEN 8 13 | #define MAVLINK_MSG_ID_RPM_MIN_LEN 8 14 | #define MAVLINK_MSG_ID_226_LEN 8 15 | #define MAVLINK_MSG_ID_226_MIN_LEN 8 16 | 17 | #define MAVLINK_MSG_ID_RPM_CRC 207 18 | #define MAVLINK_MSG_ID_226_CRC 207 19 | 20 | 21 | 22 | #if MAVLINK_COMMAND_24BIT 23 | #define MAVLINK_MESSAGE_INFO_RPM { \ 24 | 226, \ 25 | "RPM", \ 26 | 2, \ 27 | { { "rpm1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_rpm_t, rpm1) }, \ 28 | { "rpm2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_rpm_t, rpm2) }, \ 29 | } \ 30 | } 31 | #else 32 | #define MAVLINK_MESSAGE_INFO_RPM { \ 33 | "RPM", \ 34 | 2, \ 35 | { { "rpm1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_rpm_t, rpm1) }, \ 36 | { "rpm2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_rpm_t, rpm2) }, \ 37 | } \ 38 | } 39 | #endif 40 | 41 | /** 42 | * @brief Pack a rpm message 43 | * @param system_id ID of this system 44 | * @param component_id ID of this component (e.g. 200 for IMU) 45 | * @param msg The MAVLink message to compress the data into 46 | * 47 | * @param rpm1 RPM Sensor1 48 | * @param rpm2 RPM Sensor2 49 | * @return length of the message in bytes (excluding serial stream start sign) 50 | */ 51 | static inline uint16_t mavlink_msg_rpm_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 52 | float rpm1, float rpm2) 53 | { 54 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 55 | char buf[MAVLINK_MSG_ID_RPM_LEN]; 56 | _mav_put_float(buf, 0, rpm1); 57 | _mav_put_float(buf, 4, rpm2); 58 | 59 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RPM_LEN); 60 | #else 61 | mavlink_rpm_t packet; 62 | packet.rpm1 = rpm1; 63 | packet.rpm2 = rpm2; 64 | 65 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RPM_LEN); 66 | #endif 67 | 68 | msg->msgid = MAVLINK_MSG_ID_RPM; 69 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RPM_MIN_LEN, MAVLINK_MSG_ID_RPM_LEN, MAVLINK_MSG_ID_RPM_CRC); 70 | } 71 | 72 | /** 73 | * @brief Pack a rpm message on a channel 74 | * @param system_id ID of this system 75 | * @param component_id ID of this component (e.g. 200 for IMU) 76 | * @param chan The MAVLink channel this message will be sent over 77 | * @param msg The MAVLink message to compress the data into 78 | * @param rpm1 RPM Sensor1 79 | * @param rpm2 RPM Sensor2 80 | * @return length of the message in bytes (excluding serial stream start sign) 81 | */ 82 | static inline uint16_t mavlink_msg_rpm_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 83 | mavlink_message_t* msg, 84 | float rpm1,float rpm2) 85 | { 86 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 87 | char buf[MAVLINK_MSG_ID_RPM_LEN]; 88 | _mav_put_float(buf, 0, rpm1); 89 | _mav_put_float(buf, 4, rpm2); 90 | 91 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RPM_LEN); 92 | #else 93 | mavlink_rpm_t packet; 94 | packet.rpm1 = rpm1; 95 | packet.rpm2 = rpm2; 96 | 97 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RPM_LEN); 98 | #endif 99 | 100 | msg->msgid = MAVLINK_MSG_ID_RPM; 101 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RPM_MIN_LEN, MAVLINK_MSG_ID_RPM_LEN, MAVLINK_MSG_ID_RPM_CRC); 102 | } 103 | 104 | /** 105 | * @brief Encode a rpm struct 106 | * 107 | * @param system_id ID of this system 108 | * @param component_id ID of this component (e.g. 200 for IMU) 109 | * @param msg The MAVLink message to compress the data into 110 | * @param rpm C-struct to read the message contents from 111 | */ 112 | static inline uint16_t mavlink_msg_rpm_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rpm_t* rpm) 113 | { 114 | return mavlink_msg_rpm_pack(system_id, component_id, msg, rpm->rpm1, rpm->rpm2); 115 | } 116 | 117 | /** 118 | * @brief Encode a rpm struct on a channel 119 | * 120 | * @param system_id ID of this system 121 | * @param component_id ID of this component (e.g. 200 for IMU) 122 | * @param chan The MAVLink channel this message will be sent over 123 | * @param msg The MAVLink message to compress the data into 124 | * @param rpm C-struct to read the message contents from 125 | */ 126 | static inline uint16_t mavlink_msg_rpm_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rpm_t* rpm) 127 | { 128 | return mavlink_msg_rpm_pack_chan(system_id, component_id, chan, msg, rpm->rpm1, rpm->rpm2); 129 | } 130 | 131 | /** 132 | * @brief Send a rpm message 133 | * @param chan MAVLink channel to send the message 134 | * 135 | * @param rpm1 RPM Sensor1 136 | * @param rpm2 RPM Sensor2 137 | */ 138 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 139 | 140 | static inline void mavlink_msg_rpm_send(mavlink_channel_t chan, float rpm1, float rpm2) 141 | { 142 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 143 | char buf[MAVLINK_MSG_ID_RPM_LEN]; 144 | _mav_put_float(buf, 0, rpm1); 145 | _mav_put_float(buf, 4, rpm2); 146 | 147 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RPM, buf, MAVLINK_MSG_ID_RPM_MIN_LEN, MAVLINK_MSG_ID_RPM_LEN, MAVLINK_MSG_ID_RPM_CRC); 148 | #else 149 | mavlink_rpm_t packet; 150 | packet.rpm1 = rpm1; 151 | packet.rpm2 = rpm2; 152 | 153 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RPM, (const char *)&packet, MAVLINK_MSG_ID_RPM_MIN_LEN, MAVLINK_MSG_ID_RPM_LEN, MAVLINK_MSG_ID_RPM_CRC); 154 | #endif 155 | } 156 | 157 | /** 158 | * @brief Send a rpm message 159 | * @param chan MAVLink channel to send the message 160 | * @param struct The MAVLink struct to serialize 161 | */ 162 | static inline void mavlink_msg_rpm_send_struct(mavlink_channel_t chan, const mavlink_rpm_t* rpm) 163 | { 164 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 165 | mavlink_msg_rpm_send(chan, rpm->rpm1, rpm->rpm2); 166 | #else 167 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RPM, (const char *)rpm, MAVLINK_MSG_ID_RPM_MIN_LEN, MAVLINK_MSG_ID_RPM_LEN, MAVLINK_MSG_ID_RPM_CRC); 168 | #endif 169 | } 170 | 171 | #if MAVLINK_MSG_ID_RPM_LEN <= MAVLINK_MAX_PAYLOAD_LEN 172 | /* 173 | This varient of _send() can be used to save stack space by re-using 174 | memory from the receive buffer. The caller provides a 175 | mavlink_message_t which is the size of a full mavlink message. This 176 | is usually the receive buffer for the channel, and allows a reply to an 177 | incoming message with minimum stack space usage. 178 | */ 179 | static inline void mavlink_msg_rpm_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float rpm1, float rpm2) 180 | { 181 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 182 | char *buf = (char *)msgbuf; 183 | _mav_put_float(buf, 0, rpm1); 184 | _mav_put_float(buf, 4, rpm2); 185 | 186 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RPM, buf, MAVLINK_MSG_ID_RPM_MIN_LEN, MAVLINK_MSG_ID_RPM_LEN, MAVLINK_MSG_ID_RPM_CRC); 187 | #else 188 | mavlink_rpm_t *packet = (mavlink_rpm_t *)msgbuf; 189 | packet->rpm1 = rpm1; 190 | packet->rpm2 = rpm2; 191 | 192 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RPM, (const char *)packet, MAVLINK_MSG_ID_RPM_MIN_LEN, MAVLINK_MSG_ID_RPM_LEN, MAVLINK_MSG_ID_RPM_CRC); 193 | #endif 194 | } 195 | #endif 196 | 197 | #endif 198 | 199 | // MESSAGE RPM UNPACKING 200 | 201 | 202 | /** 203 | * @brief Get field rpm1 from rpm message 204 | * 205 | * @return RPM Sensor1 206 | */ 207 | static inline float mavlink_msg_rpm_get_rpm1(const mavlink_message_t* msg) 208 | { 209 | return _MAV_RETURN_float(msg, 0); 210 | } 211 | 212 | /** 213 | * @brief Get field rpm2 from rpm message 214 | * 215 | * @return RPM Sensor2 216 | */ 217 | static inline float mavlink_msg_rpm_get_rpm2(const mavlink_message_t* msg) 218 | { 219 | return _MAV_RETURN_float(msg, 4); 220 | } 221 | 222 | /** 223 | * @brief Decode a rpm message into a struct 224 | * 225 | * @param msg The message to decode 226 | * @param rpm C-struct to decode the message contents into 227 | */ 228 | static inline void mavlink_msg_rpm_decode(const mavlink_message_t* msg, mavlink_rpm_t* rpm) 229 | { 230 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 231 | rpm->rpm1 = mavlink_msg_rpm_get_rpm1(msg); 232 | rpm->rpm2 = mavlink_msg_rpm_get_rpm2(msg); 233 | #else 234 | uint8_t len = msg->len < MAVLINK_MSG_ID_RPM_LEN? msg->len : MAVLINK_MSG_ID_RPM_LEN; 235 | memset(rpm, 0, MAVLINK_MSG_ID_RPM_LEN); 236 | memcpy(rpm, _MAV_PAYLOAD(msg), len); 237 | #endif 238 | } 239 | -------------------------------------------------------------------------------- /src/common/mavlink_msg_mission_current.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // MESSAGE MISSION_CURRENT PACKING 3 | 4 | #define MAVLINK_MSG_ID_MISSION_CURRENT 42 5 | 6 | MAVPACKED( 7 | typedef struct __mavlink_mission_current_t { 8 | uint16_t seq; /*< Sequence*/ 9 | }) mavlink_mission_current_t; 10 | 11 | #define MAVLINK_MSG_ID_MISSION_CURRENT_LEN 2 12 | #define MAVLINK_MSG_ID_MISSION_CURRENT_MIN_LEN 2 13 | #define MAVLINK_MSG_ID_42_LEN 2 14 | #define MAVLINK_MSG_ID_42_MIN_LEN 2 15 | 16 | #define MAVLINK_MSG_ID_MISSION_CURRENT_CRC 28 17 | #define MAVLINK_MSG_ID_42_CRC 28 18 | 19 | 20 | 21 | #if MAVLINK_COMMAND_24BIT 22 | #define MAVLINK_MESSAGE_INFO_MISSION_CURRENT { \ 23 | 42, \ 24 | "MISSION_CURRENT", \ 25 | 1, \ 26 | { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_current_t, seq) }, \ 27 | } \ 28 | } 29 | #else 30 | #define MAVLINK_MESSAGE_INFO_MISSION_CURRENT { \ 31 | "MISSION_CURRENT", \ 32 | 1, \ 33 | { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_current_t, seq) }, \ 34 | } \ 35 | } 36 | #endif 37 | 38 | /** 39 | * @brief Pack a mission_current message 40 | * @param system_id ID of this system 41 | * @param component_id ID of this component (e.g. 200 for IMU) 42 | * @param msg The MAVLink message to compress the data into 43 | * 44 | * @param seq Sequence 45 | * @return length of the message in bytes (excluding serial stream start sign) 46 | */ 47 | static inline uint16_t mavlink_msg_mission_current_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 48 | uint16_t seq) 49 | { 50 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 51 | char buf[MAVLINK_MSG_ID_MISSION_CURRENT_LEN]; 52 | _mav_put_uint16_t(buf, 0, seq); 53 | 54 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); 55 | #else 56 | mavlink_mission_current_t packet; 57 | packet.seq = seq; 58 | 59 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); 60 | #endif 61 | 62 | msg->msgid = MAVLINK_MSG_ID_MISSION_CURRENT; 63 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_CURRENT_MIN_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); 64 | } 65 | 66 | /** 67 | * @brief Pack a mission_current message on a channel 68 | * @param system_id ID of this system 69 | * @param component_id ID of this component (e.g. 200 for IMU) 70 | * @param chan The MAVLink channel this message will be sent over 71 | * @param msg The MAVLink message to compress the data into 72 | * @param seq Sequence 73 | * @return length of the message in bytes (excluding serial stream start sign) 74 | */ 75 | static inline uint16_t mavlink_msg_mission_current_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 76 | mavlink_message_t* msg, 77 | uint16_t seq) 78 | { 79 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 80 | char buf[MAVLINK_MSG_ID_MISSION_CURRENT_LEN]; 81 | _mav_put_uint16_t(buf, 0, seq); 82 | 83 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); 84 | #else 85 | mavlink_mission_current_t packet; 86 | packet.seq = seq; 87 | 88 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); 89 | #endif 90 | 91 | msg->msgid = MAVLINK_MSG_ID_MISSION_CURRENT; 92 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_CURRENT_MIN_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); 93 | } 94 | 95 | /** 96 | * @brief Encode a mission_current struct 97 | * 98 | * @param system_id ID of this system 99 | * @param component_id ID of this component (e.g. 200 for IMU) 100 | * @param msg The MAVLink message to compress the data into 101 | * @param mission_current C-struct to read the message contents from 102 | */ 103 | 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) 104 | { 105 | return mavlink_msg_mission_current_pack(system_id, component_id, msg, mission_current->seq); 106 | } 107 | 108 | /** 109 | * @brief Encode a mission_current struct on a channel 110 | * 111 | * @param system_id ID of this system 112 | * @param component_id ID of this component (e.g. 200 for IMU) 113 | * @param chan The MAVLink channel this message will be sent over 114 | * @param msg The MAVLink message to compress the data into 115 | * @param mission_current C-struct to read the message contents from 116 | */ 117 | 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) 118 | { 119 | return mavlink_msg_mission_current_pack_chan(system_id, component_id, chan, msg, mission_current->seq); 120 | } 121 | 122 | /** 123 | * @brief Send a mission_current message 124 | * @param chan MAVLink channel to send the message 125 | * 126 | * @param seq Sequence 127 | */ 128 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 129 | 130 | static inline void mavlink_msg_mission_current_send(mavlink_channel_t chan, uint16_t seq) 131 | { 132 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 133 | char buf[MAVLINK_MSG_ID_MISSION_CURRENT_LEN]; 134 | _mav_put_uint16_t(buf, 0, seq); 135 | 136 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, buf, MAVLINK_MSG_ID_MISSION_CURRENT_MIN_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); 137 | #else 138 | mavlink_mission_current_t packet; 139 | packet.seq = seq; 140 | 141 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_CURRENT_MIN_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); 142 | #endif 143 | } 144 | 145 | /** 146 | * @brief Send a mission_current message 147 | * @param chan MAVLink channel to send the message 148 | * @param struct The MAVLink struct to serialize 149 | */ 150 | static inline void mavlink_msg_mission_current_send_struct(mavlink_channel_t chan, const mavlink_mission_current_t* mission_current) 151 | { 152 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 153 | mavlink_msg_mission_current_send(chan, mission_current->seq); 154 | #else 155 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)mission_current, MAVLINK_MSG_ID_MISSION_CURRENT_MIN_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); 156 | #endif 157 | } 158 | 159 | #if MAVLINK_MSG_ID_MISSION_CURRENT_LEN <= MAVLINK_MAX_PAYLOAD_LEN 160 | /* 161 | This varient of _send() can be used to save stack space by re-using 162 | memory from the receive buffer. The caller provides a 163 | mavlink_message_t which is the size of a full mavlink message. This 164 | is usually the receive buffer for the channel, and allows a reply to an 165 | incoming message with minimum stack space usage. 166 | */ 167 | static inline void mavlink_msg_mission_current_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t seq) 168 | { 169 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 170 | char *buf = (char *)msgbuf; 171 | _mav_put_uint16_t(buf, 0, seq); 172 | 173 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, buf, MAVLINK_MSG_ID_MISSION_CURRENT_MIN_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); 174 | #else 175 | mavlink_mission_current_t *packet = (mavlink_mission_current_t *)msgbuf; 176 | packet->seq = seq; 177 | 178 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_CURRENT_MIN_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); 179 | #endif 180 | } 181 | #endif 182 | 183 | #endif 184 | 185 | // MESSAGE MISSION_CURRENT UNPACKING 186 | 187 | 188 | /** 189 | * @brief Get field seq from mission_current message 190 | * 191 | * @return Sequence 192 | */ 193 | static inline uint16_t mavlink_msg_mission_current_get_seq(const mavlink_message_t* msg) 194 | { 195 | return _MAV_RETURN_uint16_t(msg, 0); 196 | } 197 | 198 | /** 199 | * @brief Decode a mission_current message into a struct 200 | * 201 | * @param msg The message to decode 202 | * @param mission_current C-struct to decode the message contents into 203 | */ 204 | static inline void mavlink_msg_mission_current_decode(const mavlink_message_t* msg, mavlink_mission_current_t* mission_current) 205 | { 206 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 207 | mission_current->seq = mavlink_msg_mission_current_get_seq(msg); 208 | #else 209 | uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_CURRENT_LEN? msg->len : MAVLINK_MSG_ID_MISSION_CURRENT_LEN; 210 | memset(mission_current, 0, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); 211 | memcpy(mission_current, _MAV_PAYLOAD(msg), len); 212 | #endif 213 | } 214 | -------------------------------------------------------------------------------- /src/icarous/mavlink_msg_icarous_heartbeat.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // MESSAGE ICAROUS_HEARTBEAT PACKING 3 | 4 | #define MAVLINK_MSG_ID_ICAROUS_HEARTBEAT 42000 5 | 6 | MAVPACKED( 7 | typedef struct __mavlink_icarous_heartbeat_t { 8 | uint8_t status; /*< See the FMS_STATE enum.*/ 9 | }) mavlink_icarous_heartbeat_t; 10 | 11 | #define MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_LEN 1 12 | #define MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_MIN_LEN 1 13 | #define MAVLINK_MSG_ID_42000_LEN 1 14 | #define MAVLINK_MSG_ID_42000_MIN_LEN 1 15 | 16 | #define MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_CRC 227 17 | #define MAVLINK_MSG_ID_42000_CRC 227 18 | 19 | 20 | 21 | #if MAVLINK_COMMAND_24BIT 22 | #define MAVLINK_MESSAGE_INFO_ICAROUS_HEARTBEAT { \ 23 | 42000, \ 24 | "ICAROUS_HEARTBEAT", \ 25 | 1, \ 26 | { { "status", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_icarous_heartbeat_t, status) }, \ 27 | } \ 28 | } 29 | #else 30 | #define MAVLINK_MESSAGE_INFO_ICAROUS_HEARTBEAT { \ 31 | "ICAROUS_HEARTBEAT", \ 32 | 1, \ 33 | { { "status", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_icarous_heartbeat_t, status) }, \ 34 | } \ 35 | } 36 | #endif 37 | 38 | /** 39 | * @brief Pack a icarous_heartbeat message 40 | * @param system_id ID of this system 41 | * @param component_id ID of this component (e.g. 200 for IMU) 42 | * @param msg The MAVLink message to compress the data into 43 | * 44 | * @param status See the FMS_STATE enum. 45 | * @return length of the message in bytes (excluding serial stream start sign) 46 | */ 47 | static inline uint16_t mavlink_msg_icarous_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 48 | uint8_t status) 49 | { 50 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 51 | char buf[MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_LEN]; 52 | _mav_put_uint8_t(buf, 0, status); 53 | 54 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_LEN); 55 | #else 56 | mavlink_icarous_heartbeat_t packet; 57 | packet.status = status; 58 | 59 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_LEN); 60 | #endif 61 | 62 | msg->msgid = MAVLINK_MSG_ID_ICAROUS_HEARTBEAT; 63 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_LEN, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_CRC); 64 | } 65 | 66 | /** 67 | * @brief Pack a icarous_heartbeat message on a channel 68 | * @param system_id ID of this system 69 | * @param component_id ID of this component (e.g. 200 for IMU) 70 | * @param chan The MAVLink channel this message will be sent over 71 | * @param msg The MAVLink message to compress the data into 72 | * @param status See the FMS_STATE enum. 73 | * @return length of the message in bytes (excluding serial stream start sign) 74 | */ 75 | static inline uint16_t mavlink_msg_icarous_heartbeat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 76 | mavlink_message_t* msg, 77 | uint8_t status) 78 | { 79 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 80 | char buf[MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_LEN]; 81 | _mav_put_uint8_t(buf, 0, status); 82 | 83 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_LEN); 84 | #else 85 | mavlink_icarous_heartbeat_t packet; 86 | packet.status = status; 87 | 88 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_LEN); 89 | #endif 90 | 91 | msg->msgid = MAVLINK_MSG_ID_ICAROUS_HEARTBEAT; 92 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_LEN, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_CRC); 93 | } 94 | 95 | /** 96 | * @brief Encode a icarous_heartbeat struct 97 | * 98 | * @param system_id ID of this system 99 | * @param component_id ID of this component (e.g. 200 for IMU) 100 | * @param msg The MAVLink message to compress the data into 101 | * @param icarous_heartbeat C-struct to read the message contents from 102 | */ 103 | static inline uint16_t mavlink_msg_icarous_heartbeat_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_icarous_heartbeat_t* icarous_heartbeat) 104 | { 105 | return mavlink_msg_icarous_heartbeat_pack(system_id, component_id, msg, icarous_heartbeat->status); 106 | } 107 | 108 | /** 109 | * @brief Encode a icarous_heartbeat struct on a channel 110 | * 111 | * @param system_id ID of this system 112 | * @param component_id ID of this component (e.g. 200 for IMU) 113 | * @param chan The MAVLink channel this message will be sent over 114 | * @param msg The MAVLink message to compress the data into 115 | * @param icarous_heartbeat C-struct to read the message contents from 116 | */ 117 | static inline uint16_t mavlink_msg_icarous_heartbeat_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_icarous_heartbeat_t* icarous_heartbeat) 118 | { 119 | return mavlink_msg_icarous_heartbeat_pack_chan(system_id, component_id, chan, msg, icarous_heartbeat->status); 120 | } 121 | 122 | /** 123 | * @brief Send a icarous_heartbeat message 124 | * @param chan MAVLink channel to send the message 125 | * 126 | * @param status See the FMS_STATE enum. 127 | */ 128 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 129 | 130 | static inline void mavlink_msg_icarous_heartbeat_send(mavlink_channel_t chan, uint8_t status) 131 | { 132 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 133 | char buf[MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_LEN]; 134 | _mav_put_uint8_t(buf, 0, status); 135 | 136 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT, buf, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_LEN, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_CRC); 137 | #else 138 | mavlink_icarous_heartbeat_t packet; 139 | packet.status = status; 140 | 141 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT, (const char *)&packet, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_LEN, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_CRC); 142 | #endif 143 | } 144 | 145 | /** 146 | * @brief Send a icarous_heartbeat message 147 | * @param chan MAVLink channel to send the message 148 | * @param struct The MAVLink struct to serialize 149 | */ 150 | static inline void mavlink_msg_icarous_heartbeat_send_struct(mavlink_channel_t chan, const mavlink_icarous_heartbeat_t* icarous_heartbeat) 151 | { 152 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 153 | mavlink_msg_icarous_heartbeat_send(chan, icarous_heartbeat->status); 154 | #else 155 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT, (const char *)icarous_heartbeat, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_LEN, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_CRC); 156 | #endif 157 | } 158 | 159 | #if MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_LEN <= MAVLINK_MAX_PAYLOAD_LEN 160 | /* 161 | This varient of _send() can be used to save stack space by re-using 162 | memory from the receive buffer. The caller provides a 163 | mavlink_message_t which is the size of a full mavlink message. This 164 | is usually the receive buffer for the channel, and allows a reply to an 165 | incoming message with minimum stack space usage. 166 | */ 167 | static inline void mavlink_msg_icarous_heartbeat_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t status) 168 | { 169 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 170 | char *buf = (char *)msgbuf; 171 | _mav_put_uint8_t(buf, 0, status); 172 | 173 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT, buf, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_LEN, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_CRC); 174 | #else 175 | mavlink_icarous_heartbeat_t *packet = (mavlink_icarous_heartbeat_t *)msgbuf; 176 | packet->status = status; 177 | 178 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_LEN, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_CRC); 179 | #endif 180 | } 181 | #endif 182 | 183 | #endif 184 | 185 | // MESSAGE ICAROUS_HEARTBEAT UNPACKING 186 | 187 | 188 | /** 189 | * @brief Get field status from icarous_heartbeat message 190 | * 191 | * @return See the FMS_STATE enum. 192 | */ 193 | static inline uint8_t mavlink_msg_icarous_heartbeat_get_status(const mavlink_message_t* msg) 194 | { 195 | return _MAV_RETURN_uint8_t(msg, 0); 196 | } 197 | 198 | /** 199 | * @brief Decode a icarous_heartbeat message into a struct 200 | * 201 | * @param msg The message to decode 202 | * @param icarous_heartbeat C-struct to decode the message contents into 203 | */ 204 | static inline void mavlink_msg_icarous_heartbeat_decode(const mavlink_message_t* msg, mavlink_icarous_heartbeat_t* icarous_heartbeat) 205 | { 206 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 207 | icarous_heartbeat->status = mavlink_msg_icarous_heartbeat_get_status(msg); 208 | #else 209 | uint8_t len = msg->len < MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_LEN? msg->len : MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_LEN; 210 | memset(icarous_heartbeat, 0, MAVLINK_MSG_ID_ICAROUS_HEARTBEAT_LEN); 211 | memcpy(icarous_heartbeat, _MAV_PAYLOAD(msg), len); 212 | #endif 213 | } 214 | -------------------------------------------------------------------------------- /src/common/mavlink_msg_timesync.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // MESSAGE TIMESYNC PACKING 3 | 4 | #define MAVLINK_MSG_ID_TIMESYNC 111 5 | 6 | MAVPACKED( 7 | typedef struct __mavlink_timesync_t { 8 | int64_t tc1; /*< Time sync timestamp 1*/ 9 | int64_t ts1; /*< Time sync timestamp 2*/ 10 | }) mavlink_timesync_t; 11 | 12 | #define MAVLINK_MSG_ID_TIMESYNC_LEN 16 13 | #define MAVLINK_MSG_ID_TIMESYNC_MIN_LEN 16 14 | #define MAVLINK_MSG_ID_111_LEN 16 15 | #define MAVLINK_MSG_ID_111_MIN_LEN 16 16 | 17 | #define MAVLINK_MSG_ID_TIMESYNC_CRC 34 18 | #define MAVLINK_MSG_ID_111_CRC 34 19 | 20 | 21 | 22 | #if MAVLINK_COMMAND_24BIT 23 | #define MAVLINK_MESSAGE_INFO_TIMESYNC { \ 24 | 111, \ 25 | "TIMESYNC", \ 26 | 2, \ 27 | { { "tc1", NULL, MAVLINK_TYPE_INT64_T, 0, 0, offsetof(mavlink_timesync_t, tc1) }, \ 28 | { "ts1", NULL, MAVLINK_TYPE_INT64_T, 0, 8, offsetof(mavlink_timesync_t, ts1) }, \ 29 | } \ 30 | } 31 | #else 32 | #define MAVLINK_MESSAGE_INFO_TIMESYNC { \ 33 | "TIMESYNC", \ 34 | 2, \ 35 | { { "tc1", NULL, MAVLINK_TYPE_INT64_T, 0, 0, offsetof(mavlink_timesync_t, tc1) }, \ 36 | { "ts1", NULL, MAVLINK_TYPE_INT64_T, 0, 8, offsetof(mavlink_timesync_t, ts1) }, \ 37 | } \ 38 | } 39 | #endif 40 | 41 | /** 42 | * @brief Pack a timesync message 43 | * @param system_id ID of this system 44 | * @param component_id ID of this component (e.g. 200 for IMU) 45 | * @param msg The MAVLink message to compress the data into 46 | * 47 | * @param tc1 Time sync timestamp 1 48 | * @param ts1 Time sync timestamp 2 49 | * @return length of the message in bytes (excluding serial stream start sign) 50 | */ 51 | static inline uint16_t mavlink_msg_timesync_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 52 | int64_t tc1, int64_t ts1) 53 | { 54 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 55 | char buf[MAVLINK_MSG_ID_TIMESYNC_LEN]; 56 | _mav_put_int64_t(buf, 0, tc1); 57 | _mav_put_int64_t(buf, 8, ts1); 58 | 59 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TIMESYNC_LEN); 60 | #else 61 | mavlink_timesync_t packet; 62 | packet.tc1 = tc1; 63 | packet.ts1 = ts1; 64 | 65 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TIMESYNC_LEN); 66 | #endif 67 | 68 | msg->msgid = MAVLINK_MSG_ID_TIMESYNC; 69 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TIMESYNC_MIN_LEN, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); 70 | } 71 | 72 | /** 73 | * @brief Pack a timesync message on a channel 74 | * @param system_id ID of this system 75 | * @param component_id ID of this component (e.g. 200 for IMU) 76 | * @param chan The MAVLink channel this message will be sent over 77 | * @param msg The MAVLink message to compress the data into 78 | * @param tc1 Time sync timestamp 1 79 | * @param ts1 Time sync timestamp 2 80 | * @return length of the message in bytes (excluding serial stream start sign) 81 | */ 82 | static inline uint16_t mavlink_msg_timesync_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 83 | mavlink_message_t* msg, 84 | int64_t tc1,int64_t ts1) 85 | { 86 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 87 | char buf[MAVLINK_MSG_ID_TIMESYNC_LEN]; 88 | _mav_put_int64_t(buf, 0, tc1); 89 | _mav_put_int64_t(buf, 8, ts1); 90 | 91 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TIMESYNC_LEN); 92 | #else 93 | mavlink_timesync_t packet; 94 | packet.tc1 = tc1; 95 | packet.ts1 = ts1; 96 | 97 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TIMESYNC_LEN); 98 | #endif 99 | 100 | msg->msgid = MAVLINK_MSG_ID_TIMESYNC; 101 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TIMESYNC_MIN_LEN, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); 102 | } 103 | 104 | /** 105 | * @brief Encode a timesync struct 106 | * 107 | * @param system_id ID of this system 108 | * @param component_id ID of this component (e.g. 200 for IMU) 109 | * @param msg The MAVLink message to compress the data into 110 | * @param timesync C-struct to read the message contents from 111 | */ 112 | static inline uint16_t mavlink_msg_timesync_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_timesync_t* timesync) 113 | { 114 | return mavlink_msg_timesync_pack(system_id, component_id, msg, timesync->tc1, timesync->ts1); 115 | } 116 | 117 | /** 118 | * @brief Encode a timesync struct on a channel 119 | * 120 | * @param system_id ID of this system 121 | * @param component_id ID of this component (e.g. 200 for IMU) 122 | * @param chan The MAVLink channel this message will be sent over 123 | * @param msg The MAVLink message to compress the data into 124 | * @param timesync C-struct to read the message contents from 125 | */ 126 | static inline uint16_t mavlink_msg_timesync_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_timesync_t* timesync) 127 | { 128 | return mavlink_msg_timesync_pack_chan(system_id, component_id, chan, msg, timesync->tc1, timesync->ts1); 129 | } 130 | 131 | /** 132 | * @brief Send a timesync message 133 | * @param chan MAVLink channel to send the message 134 | * 135 | * @param tc1 Time sync timestamp 1 136 | * @param ts1 Time sync timestamp 2 137 | */ 138 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 139 | 140 | static inline void mavlink_msg_timesync_send(mavlink_channel_t chan, int64_t tc1, int64_t ts1) 141 | { 142 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 143 | char buf[MAVLINK_MSG_ID_TIMESYNC_LEN]; 144 | _mav_put_int64_t(buf, 0, tc1); 145 | _mav_put_int64_t(buf, 8, ts1); 146 | 147 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, buf, MAVLINK_MSG_ID_TIMESYNC_MIN_LEN, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); 148 | #else 149 | mavlink_timesync_t packet; 150 | packet.tc1 = tc1; 151 | packet.ts1 = ts1; 152 | 153 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, (const char *)&packet, MAVLINK_MSG_ID_TIMESYNC_MIN_LEN, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); 154 | #endif 155 | } 156 | 157 | /** 158 | * @brief Send a timesync message 159 | * @param chan MAVLink channel to send the message 160 | * @param struct The MAVLink struct to serialize 161 | */ 162 | static inline void mavlink_msg_timesync_send_struct(mavlink_channel_t chan, const mavlink_timesync_t* timesync) 163 | { 164 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 165 | mavlink_msg_timesync_send(chan, timesync->tc1, timesync->ts1); 166 | #else 167 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, (const char *)timesync, MAVLINK_MSG_ID_TIMESYNC_MIN_LEN, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); 168 | #endif 169 | } 170 | 171 | #if MAVLINK_MSG_ID_TIMESYNC_LEN <= MAVLINK_MAX_PAYLOAD_LEN 172 | /* 173 | This varient of _send() can be used to save stack space by re-using 174 | memory from the receive buffer. The caller provides a 175 | mavlink_message_t which is the size of a full mavlink message. This 176 | is usually the receive buffer for the channel, and allows a reply to an 177 | incoming message with minimum stack space usage. 178 | */ 179 | static inline void mavlink_msg_timesync_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int64_t tc1, int64_t ts1) 180 | { 181 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 182 | char *buf = (char *)msgbuf; 183 | _mav_put_int64_t(buf, 0, tc1); 184 | _mav_put_int64_t(buf, 8, ts1); 185 | 186 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, buf, MAVLINK_MSG_ID_TIMESYNC_MIN_LEN, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); 187 | #else 188 | mavlink_timesync_t *packet = (mavlink_timesync_t *)msgbuf; 189 | packet->tc1 = tc1; 190 | packet->ts1 = ts1; 191 | 192 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, (const char *)packet, MAVLINK_MSG_ID_TIMESYNC_MIN_LEN, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); 193 | #endif 194 | } 195 | #endif 196 | 197 | #endif 198 | 199 | // MESSAGE TIMESYNC UNPACKING 200 | 201 | 202 | /** 203 | * @brief Get field tc1 from timesync message 204 | * 205 | * @return Time sync timestamp 1 206 | */ 207 | static inline int64_t mavlink_msg_timesync_get_tc1(const mavlink_message_t* msg) 208 | { 209 | return _MAV_RETURN_int64_t(msg, 0); 210 | } 211 | 212 | /** 213 | * @brief Get field ts1 from timesync message 214 | * 215 | * @return Time sync timestamp 2 216 | */ 217 | static inline int64_t mavlink_msg_timesync_get_ts1(const mavlink_message_t* msg) 218 | { 219 | return _MAV_RETURN_int64_t(msg, 8); 220 | } 221 | 222 | /** 223 | * @brief Decode a timesync message into a struct 224 | * 225 | * @param msg The message to decode 226 | * @param timesync C-struct to decode the message contents into 227 | */ 228 | static inline void mavlink_msg_timesync_decode(const mavlink_message_t* msg, mavlink_timesync_t* timesync) 229 | { 230 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 231 | timesync->tc1 = mavlink_msg_timesync_get_tc1(msg); 232 | timesync->ts1 = mavlink_msg_timesync_get_ts1(msg); 233 | #else 234 | uint8_t len = msg->len < MAVLINK_MSG_ID_TIMESYNC_LEN? msg->len : MAVLINK_MSG_ID_TIMESYNC_LEN; 235 | memset(timesync, 0, MAVLINK_MSG_ID_TIMESYNC_LEN); 236 | memcpy(timesync, _MAV_PAYLOAD(msg), len); 237 | #endif 238 | } 239 | -------------------------------------------------------------------------------- /src/common/mavlink_msg_mission_item_reached.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // MESSAGE MISSION_ITEM_REACHED PACKING 3 | 4 | #define MAVLINK_MSG_ID_MISSION_ITEM_REACHED 46 5 | 6 | MAVPACKED( 7 | typedef struct __mavlink_mission_item_reached_t { 8 | uint16_t seq; /*< Sequence*/ 9 | }) mavlink_mission_item_reached_t; 10 | 11 | #define MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN 2 12 | #define MAVLINK_MSG_ID_MISSION_ITEM_REACHED_MIN_LEN 2 13 | #define MAVLINK_MSG_ID_46_LEN 2 14 | #define MAVLINK_MSG_ID_46_MIN_LEN 2 15 | 16 | #define MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC 11 17 | #define MAVLINK_MSG_ID_46_CRC 11 18 | 19 | 20 | 21 | #if MAVLINK_COMMAND_24BIT 22 | #define MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED { \ 23 | 46, \ 24 | "MISSION_ITEM_REACHED", \ 25 | 1, \ 26 | { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_item_reached_t, seq) }, \ 27 | } \ 28 | } 29 | #else 30 | #define MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED { \ 31 | "MISSION_ITEM_REACHED", \ 32 | 1, \ 33 | { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_item_reached_t, seq) }, \ 34 | } \ 35 | } 36 | #endif 37 | 38 | /** 39 | * @brief Pack a mission_item_reached message 40 | * @param system_id ID of this system 41 | * @param component_id ID of this component (e.g. 200 for IMU) 42 | * @param msg The MAVLink message to compress the data into 43 | * 44 | * @param seq Sequence 45 | * @return length of the message in bytes (excluding serial stream start sign) 46 | */ 47 | static inline uint16_t mavlink_msg_mission_item_reached_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 48 | uint16_t seq) 49 | { 50 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 51 | char buf[MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN]; 52 | _mav_put_uint16_t(buf, 0, seq); 53 | 54 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); 55 | #else 56 | mavlink_mission_item_reached_t packet; 57 | packet.seq = seq; 58 | 59 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); 60 | #endif 61 | 62 | msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM_REACHED; 63 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); 64 | } 65 | 66 | /** 67 | * @brief Pack a mission_item_reached message on a channel 68 | * @param system_id ID of this system 69 | * @param component_id ID of this component (e.g. 200 for IMU) 70 | * @param chan The MAVLink channel this message will be sent over 71 | * @param msg The MAVLink message to compress the data into 72 | * @param seq Sequence 73 | * @return length of the message in bytes (excluding serial stream start sign) 74 | */ 75 | static inline uint16_t mavlink_msg_mission_item_reached_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 76 | mavlink_message_t* msg, 77 | uint16_t seq) 78 | { 79 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 80 | char buf[MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN]; 81 | _mav_put_uint16_t(buf, 0, seq); 82 | 83 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); 84 | #else 85 | mavlink_mission_item_reached_t packet; 86 | packet.seq = seq; 87 | 88 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); 89 | #endif 90 | 91 | msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM_REACHED; 92 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); 93 | } 94 | 95 | /** 96 | * @brief Encode a mission_item_reached struct 97 | * 98 | * @param system_id ID of this system 99 | * @param component_id ID of this component (e.g. 200 for IMU) 100 | * @param msg The MAVLink message to compress the data into 101 | * @param mission_item_reached C-struct to read the message contents from 102 | */ 103 | static inline uint16_t mavlink_msg_mission_item_reached_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_item_reached_t* mission_item_reached) 104 | { 105 | return mavlink_msg_mission_item_reached_pack(system_id, component_id, msg, mission_item_reached->seq); 106 | } 107 | 108 | /** 109 | * @brief Encode a mission_item_reached struct on a channel 110 | * 111 | * @param system_id ID of this system 112 | * @param component_id ID of this component (e.g. 200 for IMU) 113 | * @param chan The MAVLink channel this message will be sent over 114 | * @param msg The MAVLink message to compress the data into 115 | * @param mission_item_reached C-struct to read the message contents from 116 | */ 117 | static inline uint16_t mavlink_msg_mission_item_reached_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_item_reached_t* mission_item_reached) 118 | { 119 | return mavlink_msg_mission_item_reached_pack_chan(system_id, component_id, chan, msg, mission_item_reached->seq); 120 | } 121 | 122 | /** 123 | * @brief Send a mission_item_reached message 124 | * @param chan MAVLink channel to send the message 125 | * 126 | * @param seq Sequence 127 | */ 128 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 129 | 130 | static inline void mavlink_msg_mission_item_reached_send(mavlink_channel_t chan, uint16_t seq) 131 | { 132 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 133 | char buf[MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN]; 134 | _mav_put_uint16_t(buf, 0, seq); 135 | 136 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); 137 | #else 138 | mavlink_mission_item_reached_t packet; 139 | packet.seq = seq; 140 | 141 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, (const char *)&packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); 142 | #endif 143 | } 144 | 145 | /** 146 | * @brief Send a mission_item_reached message 147 | * @param chan MAVLink channel to send the message 148 | * @param struct The MAVLink struct to serialize 149 | */ 150 | static inline void mavlink_msg_mission_item_reached_send_struct(mavlink_channel_t chan, const mavlink_mission_item_reached_t* mission_item_reached) 151 | { 152 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 153 | mavlink_msg_mission_item_reached_send(chan, mission_item_reached->seq); 154 | #else 155 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, (const char *)mission_item_reached, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); 156 | #endif 157 | } 158 | 159 | #if MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN <= MAVLINK_MAX_PAYLOAD_LEN 160 | /* 161 | This varient of _send() can be used to save stack space by re-using 162 | memory from the receive buffer. The caller provides a 163 | mavlink_message_t which is the size of a full mavlink message. This 164 | is usually the receive buffer for the channel, and allows a reply to an 165 | incoming message with minimum stack space usage. 166 | */ 167 | static inline void mavlink_msg_mission_item_reached_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t seq) 168 | { 169 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 170 | char *buf = (char *)msgbuf; 171 | _mav_put_uint16_t(buf, 0, seq); 172 | 173 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); 174 | #else 175 | mavlink_mission_item_reached_t *packet = (mavlink_mission_item_reached_t *)msgbuf; 176 | packet->seq = seq; 177 | 178 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); 179 | #endif 180 | } 181 | #endif 182 | 183 | #endif 184 | 185 | // MESSAGE MISSION_ITEM_REACHED UNPACKING 186 | 187 | 188 | /** 189 | * @brief Get field seq from mission_item_reached message 190 | * 191 | * @return Sequence 192 | */ 193 | static inline uint16_t mavlink_msg_mission_item_reached_get_seq(const mavlink_message_t* msg) 194 | { 195 | return _MAV_RETURN_uint16_t(msg, 0); 196 | } 197 | 198 | /** 199 | * @brief Decode a mission_item_reached message into a struct 200 | * 201 | * @param msg The message to decode 202 | * @param mission_item_reached C-struct to decode the message contents into 203 | */ 204 | static inline void mavlink_msg_mission_item_reached_decode(const mavlink_message_t* msg, mavlink_mission_item_reached_t* mission_item_reached) 205 | { 206 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 207 | mission_item_reached->seq = mavlink_msg_mission_item_reached_get_seq(msg); 208 | #else 209 | uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN? msg->len : MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN; 210 | memset(mission_item_reached, 0, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); 211 | memcpy(mission_item_reached, _MAV_PAYLOAD(msg), len); 212 | #endif 213 | } 214 | -------------------------------------------------------------------------------- /src/ardupilotmega/mavlink_msg_hwstatus.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // MESSAGE HWSTATUS PACKING 3 | 4 | #define MAVLINK_MSG_ID_HWSTATUS 165 5 | 6 | MAVPACKED( 7 | typedef struct __mavlink_hwstatus_t { 8 | uint16_t Vcc; /*< board voltage (mV)*/ 9 | uint8_t I2Cerr; /*< I2C error count*/ 10 | }) mavlink_hwstatus_t; 11 | 12 | #define MAVLINK_MSG_ID_HWSTATUS_LEN 3 13 | #define MAVLINK_MSG_ID_HWSTATUS_MIN_LEN 3 14 | #define MAVLINK_MSG_ID_165_LEN 3 15 | #define MAVLINK_MSG_ID_165_MIN_LEN 3 16 | 17 | #define MAVLINK_MSG_ID_HWSTATUS_CRC 21 18 | #define MAVLINK_MSG_ID_165_CRC 21 19 | 20 | 21 | 22 | #if MAVLINK_COMMAND_24BIT 23 | #define MAVLINK_MESSAGE_INFO_HWSTATUS { \ 24 | 165, \ 25 | "HWSTATUS", \ 26 | 2, \ 27 | { { "Vcc", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_hwstatus_t, Vcc) }, \ 28 | { "I2Cerr", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_hwstatus_t, I2Cerr) }, \ 29 | } \ 30 | } 31 | #else 32 | #define MAVLINK_MESSAGE_INFO_HWSTATUS { \ 33 | "HWSTATUS", \ 34 | 2, \ 35 | { { "Vcc", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_hwstatus_t, Vcc) }, \ 36 | { "I2Cerr", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_hwstatus_t, I2Cerr) }, \ 37 | } \ 38 | } 39 | #endif 40 | 41 | /** 42 | * @brief Pack a hwstatus message 43 | * @param system_id ID of this system 44 | * @param component_id ID of this component (e.g. 200 for IMU) 45 | * @param msg The MAVLink message to compress the data into 46 | * 47 | * @param Vcc board voltage (mV) 48 | * @param I2Cerr I2C error count 49 | * @return length of the message in bytes (excluding serial stream start sign) 50 | */ 51 | static inline uint16_t mavlink_msg_hwstatus_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 52 | uint16_t Vcc, uint8_t I2Cerr) 53 | { 54 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 55 | char buf[MAVLINK_MSG_ID_HWSTATUS_LEN]; 56 | _mav_put_uint16_t(buf, 0, Vcc); 57 | _mav_put_uint8_t(buf, 2, I2Cerr); 58 | 59 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HWSTATUS_LEN); 60 | #else 61 | mavlink_hwstatus_t packet; 62 | packet.Vcc = Vcc; 63 | packet.I2Cerr = I2Cerr; 64 | 65 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HWSTATUS_LEN); 66 | #endif 67 | 68 | msg->msgid = MAVLINK_MSG_ID_HWSTATUS; 69 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HWSTATUS_MIN_LEN, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); 70 | } 71 | 72 | /** 73 | * @brief Pack a hwstatus message on a channel 74 | * @param system_id ID of this system 75 | * @param component_id ID of this component (e.g. 200 for IMU) 76 | * @param chan The MAVLink channel this message will be sent over 77 | * @param msg The MAVLink message to compress the data into 78 | * @param Vcc board voltage (mV) 79 | * @param I2Cerr I2C error count 80 | * @return length of the message in bytes (excluding serial stream start sign) 81 | */ 82 | static inline uint16_t mavlink_msg_hwstatus_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 83 | mavlink_message_t* msg, 84 | uint16_t Vcc,uint8_t I2Cerr) 85 | { 86 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 87 | char buf[MAVLINK_MSG_ID_HWSTATUS_LEN]; 88 | _mav_put_uint16_t(buf, 0, Vcc); 89 | _mav_put_uint8_t(buf, 2, I2Cerr); 90 | 91 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HWSTATUS_LEN); 92 | #else 93 | mavlink_hwstatus_t packet; 94 | packet.Vcc = Vcc; 95 | packet.I2Cerr = I2Cerr; 96 | 97 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HWSTATUS_LEN); 98 | #endif 99 | 100 | msg->msgid = MAVLINK_MSG_ID_HWSTATUS; 101 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HWSTATUS_MIN_LEN, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); 102 | } 103 | 104 | /** 105 | * @brief Encode a hwstatus struct 106 | * 107 | * @param system_id ID of this system 108 | * @param component_id ID of this component (e.g. 200 for IMU) 109 | * @param msg The MAVLink message to compress the data into 110 | * @param hwstatus C-struct to read the message contents from 111 | */ 112 | static inline uint16_t mavlink_msg_hwstatus_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hwstatus_t* hwstatus) 113 | { 114 | return mavlink_msg_hwstatus_pack(system_id, component_id, msg, hwstatus->Vcc, hwstatus->I2Cerr); 115 | } 116 | 117 | /** 118 | * @brief Encode a hwstatus struct on a channel 119 | * 120 | * @param system_id ID of this system 121 | * @param component_id ID of this component (e.g. 200 for IMU) 122 | * @param chan The MAVLink channel this message will be sent over 123 | * @param msg The MAVLink message to compress the data into 124 | * @param hwstatus C-struct to read the message contents from 125 | */ 126 | static inline uint16_t mavlink_msg_hwstatus_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hwstatus_t* hwstatus) 127 | { 128 | return mavlink_msg_hwstatus_pack_chan(system_id, component_id, chan, msg, hwstatus->Vcc, hwstatus->I2Cerr); 129 | } 130 | 131 | /** 132 | * @brief Send a hwstatus message 133 | * @param chan MAVLink channel to send the message 134 | * 135 | * @param Vcc board voltage (mV) 136 | * @param I2Cerr I2C error count 137 | */ 138 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 139 | 140 | static inline void mavlink_msg_hwstatus_send(mavlink_channel_t chan, uint16_t Vcc, uint8_t I2Cerr) 141 | { 142 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 143 | char buf[MAVLINK_MSG_ID_HWSTATUS_LEN]; 144 | _mav_put_uint16_t(buf, 0, Vcc); 145 | _mav_put_uint8_t(buf, 2, I2Cerr); 146 | 147 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, buf, MAVLINK_MSG_ID_HWSTATUS_MIN_LEN, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); 148 | #else 149 | mavlink_hwstatus_t packet; 150 | packet.Vcc = Vcc; 151 | packet.I2Cerr = I2Cerr; 152 | 153 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)&packet, MAVLINK_MSG_ID_HWSTATUS_MIN_LEN, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); 154 | #endif 155 | } 156 | 157 | /** 158 | * @brief Send a hwstatus message 159 | * @param chan MAVLink channel to send the message 160 | * @param struct The MAVLink struct to serialize 161 | */ 162 | static inline void mavlink_msg_hwstatus_send_struct(mavlink_channel_t chan, const mavlink_hwstatus_t* hwstatus) 163 | { 164 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 165 | mavlink_msg_hwstatus_send(chan, hwstatus->Vcc, hwstatus->I2Cerr); 166 | #else 167 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)hwstatus, MAVLINK_MSG_ID_HWSTATUS_MIN_LEN, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); 168 | #endif 169 | } 170 | 171 | #if MAVLINK_MSG_ID_HWSTATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN 172 | /* 173 | This varient of _send() can be used to save stack space by re-using 174 | memory from the receive buffer. The caller provides a 175 | mavlink_message_t which is the size of a full mavlink message. This 176 | is usually the receive buffer for the channel, and allows a reply to an 177 | incoming message with minimum stack space usage. 178 | */ 179 | static inline void mavlink_msg_hwstatus_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t Vcc, uint8_t I2Cerr) 180 | { 181 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 182 | char *buf = (char *)msgbuf; 183 | _mav_put_uint16_t(buf, 0, Vcc); 184 | _mav_put_uint8_t(buf, 2, I2Cerr); 185 | 186 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, buf, MAVLINK_MSG_ID_HWSTATUS_MIN_LEN, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); 187 | #else 188 | mavlink_hwstatus_t *packet = (mavlink_hwstatus_t *)msgbuf; 189 | packet->Vcc = Vcc; 190 | packet->I2Cerr = I2Cerr; 191 | 192 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)packet, MAVLINK_MSG_ID_HWSTATUS_MIN_LEN, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); 193 | #endif 194 | } 195 | #endif 196 | 197 | #endif 198 | 199 | // MESSAGE HWSTATUS UNPACKING 200 | 201 | 202 | /** 203 | * @brief Get field Vcc from hwstatus message 204 | * 205 | * @return board voltage (mV) 206 | */ 207 | static inline uint16_t mavlink_msg_hwstatus_get_Vcc(const mavlink_message_t* msg) 208 | { 209 | return _MAV_RETURN_uint16_t(msg, 0); 210 | } 211 | 212 | /** 213 | * @brief Get field I2Cerr from hwstatus message 214 | * 215 | * @return I2C error count 216 | */ 217 | static inline uint8_t mavlink_msg_hwstatus_get_I2Cerr(const mavlink_message_t* msg) 218 | { 219 | return _MAV_RETURN_uint8_t(msg, 2); 220 | } 221 | 222 | /** 223 | * @brief Decode a hwstatus message into a struct 224 | * 225 | * @param msg The message to decode 226 | * @param hwstatus C-struct to decode the message contents into 227 | */ 228 | static inline void mavlink_msg_hwstatus_decode(const mavlink_message_t* msg, mavlink_hwstatus_t* hwstatus) 229 | { 230 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 231 | hwstatus->Vcc = mavlink_msg_hwstatus_get_Vcc(msg); 232 | hwstatus->I2Cerr = mavlink_msg_hwstatus_get_I2Cerr(msg); 233 | #else 234 | uint8_t len = msg->len < MAVLINK_MSG_ID_HWSTATUS_LEN? msg->len : MAVLINK_MSG_ID_HWSTATUS_LEN; 235 | memset(hwstatus, 0, MAVLINK_MSG_ID_HWSTATUS_LEN); 236 | memcpy(hwstatus, _MAV_PAYLOAD(msg), len); 237 | #endif 238 | } 239 | -------------------------------------------------------------------------------- /src/common/mavlink_msg_terrain_check.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // MESSAGE TERRAIN_CHECK PACKING 3 | 4 | #define MAVLINK_MSG_ID_TERRAIN_CHECK 135 5 | 6 | MAVPACKED( 7 | typedef struct __mavlink_terrain_check_t { 8 | int32_t lat; /*< Latitude (degrees *10^7)*/ 9 | int32_t lon; /*< Longitude (degrees *10^7)*/ 10 | }) mavlink_terrain_check_t; 11 | 12 | #define MAVLINK_MSG_ID_TERRAIN_CHECK_LEN 8 13 | #define MAVLINK_MSG_ID_TERRAIN_CHECK_MIN_LEN 8 14 | #define MAVLINK_MSG_ID_135_LEN 8 15 | #define MAVLINK_MSG_ID_135_MIN_LEN 8 16 | 17 | #define MAVLINK_MSG_ID_TERRAIN_CHECK_CRC 203 18 | #define MAVLINK_MSG_ID_135_CRC 203 19 | 20 | 21 | 22 | #if MAVLINK_COMMAND_24BIT 23 | #define MAVLINK_MESSAGE_INFO_TERRAIN_CHECK { \ 24 | 135, \ 25 | "TERRAIN_CHECK", \ 26 | 2, \ 27 | { { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_terrain_check_t, lat) }, \ 28 | { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_terrain_check_t, lon) }, \ 29 | } \ 30 | } 31 | #else 32 | #define MAVLINK_MESSAGE_INFO_TERRAIN_CHECK { \ 33 | "TERRAIN_CHECK", \ 34 | 2, \ 35 | { { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_terrain_check_t, lat) }, \ 36 | { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_terrain_check_t, lon) }, \ 37 | } \ 38 | } 39 | #endif 40 | 41 | /** 42 | * @brief Pack a terrain_check message 43 | * @param system_id ID of this system 44 | * @param component_id ID of this component (e.g. 200 for IMU) 45 | * @param msg The MAVLink message to compress the data into 46 | * 47 | * @param lat Latitude (degrees *10^7) 48 | * @param lon Longitude (degrees *10^7) 49 | * @return length of the message in bytes (excluding serial stream start sign) 50 | */ 51 | static inline uint16_t mavlink_msg_terrain_check_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 52 | int32_t lat, int32_t lon) 53 | { 54 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 55 | char buf[MAVLINK_MSG_ID_TERRAIN_CHECK_LEN]; 56 | _mav_put_int32_t(buf, 0, lat); 57 | _mav_put_int32_t(buf, 4, lon); 58 | 59 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); 60 | #else 61 | mavlink_terrain_check_t packet; 62 | packet.lat = lat; 63 | packet.lon = lon; 64 | 65 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); 66 | #endif 67 | 68 | msg->msgid = MAVLINK_MSG_ID_TERRAIN_CHECK; 69 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TERRAIN_CHECK_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_CRC); 70 | } 71 | 72 | /** 73 | * @brief Pack a terrain_check message on a channel 74 | * @param system_id ID of this system 75 | * @param component_id ID of this component (e.g. 200 for IMU) 76 | * @param chan The MAVLink channel this message will be sent over 77 | * @param msg The MAVLink message to compress the data into 78 | * @param lat Latitude (degrees *10^7) 79 | * @param lon Longitude (degrees *10^7) 80 | * @return length of the message in bytes (excluding serial stream start sign) 81 | */ 82 | static inline uint16_t mavlink_msg_terrain_check_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 83 | mavlink_message_t* msg, 84 | int32_t lat,int32_t lon) 85 | { 86 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 87 | char buf[MAVLINK_MSG_ID_TERRAIN_CHECK_LEN]; 88 | _mav_put_int32_t(buf, 0, lat); 89 | _mav_put_int32_t(buf, 4, lon); 90 | 91 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); 92 | #else 93 | mavlink_terrain_check_t packet; 94 | packet.lat = lat; 95 | packet.lon = lon; 96 | 97 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); 98 | #endif 99 | 100 | msg->msgid = MAVLINK_MSG_ID_TERRAIN_CHECK; 101 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TERRAIN_CHECK_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_CRC); 102 | } 103 | 104 | /** 105 | * @brief Encode a terrain_check struct 106 | * 107 | * @param system_id ID of this system 108 | * @param component_id ID of this component (e.g. 200 for IMU) 109 | * @param msg The MAVLink message to compress the data into 110 | * @param terrain_check C-struct to read the message contents from 111 | */ 112 | static inline uint16_t mavlink_msg_terrain_check_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_terrain_check_t* terrain_check) 113 | { 114 | return mavlink_msg_terrain_check_pack(system_id, component_id, msg, terrain_check->lat, terrain_check->lon); 115 | } 116 | 117 | /** 118 | * @brief Encode a terrain_check struct on a channel 119 | * 120 | * @param system_id ID of this system 121 | * @param component_id ID of this component (e.g. 200 for IMU) 122 | * @param chan The MAVLink channel this message will be sent over 123 | * @param msg The MAVLink message to compress the data into 124 | * @param terrain_check C-struct to read the message contents from 125 | */ 126 | static inline uint16_t mavlink_msg_terrain_check_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_terrain_check_t* terrain_check) 127 | { 128 | return mavlink_msg_terrain_check_pack_chan(system_id, component_id, chan, msg, terrain_check->lat, terrain_check->lon); 129 | } 130 | 131 | /** 132 | * @brief Send a terrain_check message 133 | * @param chan MAVLink channel to send the message 134 | * 135 | * @param lat Latitude (degrees *10^7) 136 | * @param lon Longitude (degrees *10^7) 137 | */ 138 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 139 | 140 | static inline void mavlink_msg_terrain_check_send(mavlink_channel_t chan, int32_t lat, int32_t lon) 141 | { 142 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 143 | char buf[MAVLINK_MSG_ID_TERRAIN_CHECK_LEN]; 144 | _mav_put_int32_t(buf, 0, lat); 145 | _mav_put_int32_t(buf, 4, lon); 146 | 147 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_CHECK, buf, MAVLINK_MSG_ID_TERRAIN_CHECK_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_CRC); 148 | #else 149 | mavlink_terrain_check_t packet; 150 | packet.lat = lat; 151 | packet.lon = lon; 152 | 153 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_CHECK, (const char *)&packet, MAVLINK_MSG_ID_TERRAIN_CHECK_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_CRC); 154 | #endif 155 | } 156 | 157 | /** 158 | * @brief Send a terrain_check message 159 | * @param chan MAVLink channel to send the message 160 | * @param struct The MAVLink struct to serialize 161 | */ 162 | static inline void mavlink_msg_terrain_check_send_struct(mavlink_channel_t chan, const mavlink_terrain_check_t* terrain_check) 163 | { 164 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 165 | mavlink_msg_terrain_check_send(chan, terrain_check->lat, terrain_check->lon); 166 | #else 167 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_CHECK, (const char *)terrain_check, MAVLINK_MSG_ID_TERRAIN_CHECK_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_CRC); 168 | #endif 169 | } 170 | 171 | #if MAVLINK_MSG_ID_TERRAIN_CHECK_LEN <= MAVLINK_MAX_PAYLOAD_LEN 172 | /* 173 | This varient of _send() can be used to save stack space by re-using 174 | memory from the receive buffer. The caller provides a 175 | mavlink_message_t which is the size of a full mavlink message. This 176 | is usually the receive buffer for the channel, and allows a reply to an 177 | incoming message with minimum stack space usage. 178 | */ 179 | static inline void mavlink_msg_terrain_check_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t lat, int32_t lon) 180 | { 181 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 182 | char *buf = (char *)msgbuf; 183 | _mav_put_int32_t(buf, 0, lat); 184 | _mav_put_int32_t(buf, 4, lon); 185 | 186 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_CHECK, buf, MAVLINK_MSG_ID_TERRAIN_CHECK_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_CRC); 187 | #else 188 | mavlink_terrain_check_t *packet = (mavlink_terrain_check_t *)msgbuf; 189 | packet->lat = lat; 190 | packet->lon = lon; 191 | 192 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_CHECK, (const char *)packet, MAVLINK_MSG_ID_TERRAIN_CHECK_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_CRC); 193 | #endif 194 | } 195 | #endif 196 | 197 | #endif 198 | 199 | // MESSAGE TERRAIN_CHECK UNPACKING 200 | 201 | 202 | /** 203 | * @brief Get field lat from terrain_check message 204 | * 205 | * @return Latitude (degrees *10^7) 206 | */ 207 | static inline int32_t mavlink_msg_terrain_check_get_lat(const mavlink_message_t* msg) 208 | { 209 | return _MAV_RETURN_int32_t(msg, 0); 210 | } 211 | 212 | /** 213 | * @brief Get field lon from terrain_check message 214 | * 215 | * @return Longitude (degrees *10^7) 216 | */ 217 | static inline int32_t mavlink_msg_terrain_check_get_lon(const mavlink_message_t* msg) 218 | { 219 | return _MAV_RETURN_int32_t(msg, 4); 220 | } 221 | 222 | /** 223 | * @brief Decode a terrain_check message into a struct 224 | * 225 | * @param msg The message to decode 226 | * @param terrain_check C-struct to decode the message contents into 227 | */ 228 | static inline void mavlink_msg_terrain_check_decode(const mavlink_message_t* msg, mavlink_terrain_check_t* terrain_check) 229 | { 230 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 231 | terrain_check->lat = mavlink_msg_terrain_check_get_lat(msg); 232 | terrain_check->lon = mavlink_msg_terrain_check_get_lon(msg); 233 | #else 234 | uint8_t len = msg->len < MAVLINK_MSG_ID_TERRAIN_CHECK_LEN? msg->len : MAVLINK_MSG_ID_TERRAIN_CHECK_LEN; 235 | memset(terrain_check, 0, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); 236 | memcpy(terrain_check, _MAV_PAYLOAD(msg), len); 237 | #endif 238 | } 239 | -------------------------------------------------------------------------------- /src/ardupilotmega/mavlink_msg_rangefinder.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // MESSAGE RANGEFINDER PACKING 3 | 4 | #define MAVLINK_MSG_ID_RANGEFINDER 173 5 | 6 | MAVPACKED( 7 | typedef struct __mavlink_rangefinder_t { 8 | float distance; /*< distance in meters*/ 9 | float voltage; /*< raw voltage if available, zero otherwise*/ 10 | }) mavlink_rangefinder_t; 11 | 12 | #define MAVLINK_MSG_ID_RANGEFINDER_LEN 8 13 | #define MAVLINK_MSG_ID_RANGEFINDER_MIN_LEN 8 14 | #define MAVLINK_MSG_ID_173_LEN 8 15 | #define MAVLINK_MSG_ID_173_MIN_LEN 8 16 | 17 | #define MAVLINK_MSG_ID_RANGEFINDER_CRC 83 18 | #define MAVLINK_MSG_ID_173_CRC 83 19 | 20 | 21 | 22 | #if MAVLINK_COMMAND_24BIT 23 | #define MAVLINK_MESSAGE_INFO_RANGEFINDER { \ 24 | 173, \ 25 | "RANGEFINDER", \ 26 | 2, \ 27 | { { "distance", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_rangefinder_t, distance) }, \ 28 | { "voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_rangefinder_t, voltage) }, \ 29 | } \ 30 | } 31 | #else 32 | #define MAVLINK_MESSAGE_INFO_RANGEFINDER { \ 33 | "RANGEFINDER", \ 34 | 2, \ 35 | { { "distance", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_rangefinder_t, distance) }, \ 36 | { "voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_rangefinder_t, voltage) }, \ 37 | } \ 38 | } 39 | #endif 40 | 41 | /** 42 | * @brief Pack a rangefinder message 43 | * @param system_id ID of this system 44 | * @param component_id ID of this component (e.g. 200 for IMU) 45 | * @param msg The MAVLink message to compress the data into 46 | * 47 | * @param distance distance in meters 48 | * @param voltage raw voltage if available, zero otherwise 49 | * @return length of the message in bytes (excluding serial stream start sign) 50 | */ 51 | static inline uint16_t mavlink_msg_rangefinder_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 52 | float distance, float voltage) 53 | { 54 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 55 | char buf[MAVLINK_MSG_ID_RANGEFINDER_LEN]; 56 | _mav_put_float(buf, 0, distance); 57 | _mav_put_float(buf, 4, voltage); 58 | 59 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RANGEFINDER_LEN); 60 | #else 61 | mavlink_rangefinder_t packet; 62 | packet.distance = distance; 63 | packet.voltage = voltage; 64 | 65 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RANGEFINDER_LEN); 66 | #endif 67 | 68 | msg->msgid = MAVLINK_MSG_ID_RANGEFINDER; 69 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RANGEFINDER_MIN_LEN, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); 70 | } 71 | 72 | /** 73 | * @brief Pack a rangefinder message on a channel 74 | * @param system_id ID of this system 75 | * @param component_id ID of this component (e.g. 200 for IMU) 76 | * @param chan The MAVLink channel this message will be sent over 77 | * @param msg The MAVLink message to compress the data into 78 | * @param distance distance in meters 79 | * @param voltage raw voltage if available, zero otherwise 80 | * @return length of the message in bytes (excluding serial stream start sign) 81 | */ 82 | static inline uint16_t mavlink_msg_rangefinder_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 83 | mavlink_message_t* msg, 84 | float distance,float voltage) 85 | { 86 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 87 | char buf[MAVLINK_MSG_ID_RANGEFINDER_LEN]; 88 | _mav_put_float(buf, 0, distance); 89 | _mav_put_float(buf, 4, voltage); 90 | 91 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RANGEFINDER_LEN); 92 | #else 93 | mavlink_rangefinder_t packet; 94 | packet.distance = distance; 95 | packet.voltage = voltage; 96 | 97 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RANGEFINDER_LEN); 98 | #endif 99 | 100 | msg->msgid = MAVLINK_MSG_ID_RANGEFINDER; 101 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RANGEFINDER_MIN_LEN, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); 102 | } 103 | 104 | /** 105 | * @brief Encode a rangefinder struct 106 | * 107 | * @param system_id ID of this system 108 | * @param component_id ID of this component (e.g. 200 for IMU) 109 | * @param msg The MAVLink message to compress the data into 110 | * @param rangefinder C-struct to read the message contents from 111 | */ 112 | static inline uint16_t mavlink_msg_rangefinder_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rangefinder_t* rangefinder) 113 | { 114 | return mavlink_msg_rangefinder_pack(system_id, component_id, msg, rangefinder->distance, rangefinder->voltage); 115 | } 116 | 117 | /** 118 | * @brief Encode a rangefinder struct on a channel 119 | * 120 | * @param system_id ID of this system 121 | * @param component_id ID of this component (e.g. 200 for IMU) 122 | * @param chan The MAVLink channel this message will be sent over 123 | * @param msg The MAVLink message to compress the data into 124 | * @param rangefinder C-struct to read the message contents from 125 | */ 126 | static inline uint16_t mavlink_msg_rangefinder_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rangefinder_t* rangefinder) 127 | { 128 | return mavlink_msg_rangefinder_pack_chan(system_id, component_id, chan, msg, rangefinder->distance, rangefinder->voltage); 129 | } 130 | 131 | /** 132 | * @brief Send a rangefinder message 133 | * @param chan MAVLink channel to send the message 134 | * 135 | * @param distance distance in meters 136 | * @param voltage raw voltage if available, zero otherwise 137 | */ 138 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 139 | 140 | static inline void mavlink_msg_rangefinder_send(mavlink_channel_t chan, float distance, float voltage) 141 | { 142 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 143 | char buf[MAVLINK_MSG_ID_RANGEFINDER_LEN]; 144 | _mav_put_float(buf, 0, distance); 145 | _mav_put_float(buf, 4, voltage); 146 | 147 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, buf, MAVLINK_MSG_ID_RANGEFINDER_MIN_LEN, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); 148 | #else 149 | mavlink_rangefinder_t packet; 150 | packet.distance = distance; 151 | packet.voltage = voltage; 152 | 153 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, (const char *)&packet, MAVLINK_MSG_ID_RANGEFINDER_MIN_LEN, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); 154 | #endif 155 | } 156 | 157 | /** 158 | * @brief Send a rangefinder message 159 | * @param chan MAVLink channel to send the message 160 | * @param struct The MAVLink struct to serialize 161 | */ 162 | static inline void mavlink_msg_rangefinder_send_struct(mavlink_channel_t chan, const mavlink_rangefinder_t* rangefinder) 163 | { 164 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 165 | mavlink_msg_rangefinder_send(chan, rangefinder->distance, rangefinder->voltage); 166 | #else 167 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, (const char *)rangefinder, MAVLINK_MSG_ID_RANGEFINDER_MIN_LEN, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); 168 | #endif 169 | } 170 | 171 | #if MAVLINK_MSG_ID_RANGEFINDER_LEN <= MAVLINK_MAX_PAYLOAD_LEN 172 | /* 173 | This varient of _send() can be used to save stack space by re-using 174 | memory from the receive buffer. The caller provides a 175 | mavlink_message_t which is the size of a full mavlink message. This 176 | is usually the receive buffer for the channel, and allows a reply to an 177 | incoming message with minimum stack space usage. 178 | */ 179 | static inline void mavlink_msg_rangefinder_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float distance, float voltage) 180 | { 181 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 182 | char *buf = (char *)msgbuf; 183 | _mav_put_float(buf, 0, distance); 184 | _mav_put_float(buf, 4, voltage); 185 | 186 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, buf, MAVLINK_MSG_ID_RANGEFINDER_MIN_LEN, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); 187 | #else 188 | mavlink_rangefinder_t *packet = (mavlink_rangefinder_t *)msgbuf; 189 | packet->distance = distance; 190 | packet->voltage = voltage; 191 | 192 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, (const char *)packet, MAVLINK_MSG_ID_RANGEFINDER_MIN_LEN, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); 193 | #endif 194 | } 195 | #endif 196 | 197 | #endif 198 | 199 | // MESSAGE RANGEFINDER UNPACKING 200 | 201 | 202 | /** 203 | * @brief Get field distance from rangefinder message 204 | * 205 | * @return distance in meters 206 | */ 207 | static inline float mavlink_msg_rangefinder_get_distance(const mavlink_message_t* msg) 208 | { 209 | return _MAV_RETURN_float(msg, 0); 210 | } 211 | 212 | /** 213 | * @brief Get field voltage from rangefinder message 214 | * 215 | * @return raw voltage if available, zero otherwise 216 | */ 217 | static inline float mavlink_msg_rangefinder_get_voltage(const mavlink_message_t* msg) 218 | { 219 | return _MAV_RETURN_float(msg, 4); 220 | } 221 | 222 | /** 223 | * @brief Decode a rangefinder message into a struct 224 | * 225 | * @param msg The message to decode 226 | * @param rangefinder C-struct to decode the message contents into 227 | */ 228 | static inline void mavlink_msg_rangefinder_decode(const mavlink_message_t* msg, mavlink_rangefinder_t* rangefinder) 229 | { 230 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 231 | rangefinder->distance = mavlink_msg_rangefinder_get_distance(msg); 232 | rangefinder->voltage = mavlink_msg_rangefinder_get_voltage(msg); 233 | #else 234 | uint8_t len = msg->len < MAVLINK_MSG_ID_RANGEFINDER_LEN? msg->len : MAVLINK_MSG_ID_RANGEFINDER_LEN; 235 | memset(rangefinder, 0, MAVLINK_MSG_ID_RANGEFINDER_LEN); 236 | memcpy(rangefinder, _MAV_PAYLOAD(msg), len); 237 | #endif 238 | } 239 | -------------------------------------------------------------------------------- /src/common/mavlink_msg_log_erase.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // MESSAGE LOG_ERASE PACKING 3 | 4 | #define MAVLINK_MSG_ID_LOG_ERASE 121 5 | 6 | MAVPACKED( 7 | typedef struct __mavlink_log_erase_t { 8 | uint8_t target_system; /*< System ID*/ 9 | uint8_t target_component; /*< Component ID*/ 10 | }) mavlink_log_erase_t; 11 | 12 | #define MAVLINK_MSG_ID_LOG_ERASE_LEN 2 13 | #define MAVLINK_MSG_ID_LOG_ERASE_MIN_LEN 2 14 | #define MAVLINK_MSG_ID_121_LEN 2 15 | #define MAVLINK_MSG_ID_121_MIN_LEN 2 16 | 17 | #define MAVLINK_MSG_ID_LOG_ERASE_CRC 237 18 | #define MAVLINK_MSG_ID_121_CRC 237 19 | 20 | 21 | 22 | #if MAVLINK_COMMAND_24BIT 23 | #define MAVLINK_MESSAGE_INFO_LOG_ERASE { \ 24 | 121, \ 25 | "LOG_ERASE", \ 26 | 2, \ 27 | { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_log_erase_t, target_system) }, \ 28 | { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_log_erase_t, target_component) }, \ 29 | } \ 30 | } 31 | #else 32 | #define MAVLINK_MESSAGE_INFO_LOG_ERASE { \ 33 | "LOG_ERASE", \ 34 | 2, \ 35 | { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_log_erase_t, target_system) }, \ 36 | { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_log_erase_t, target_component) }, \ 37 | } \ 38 | } 39 | #endif 40 | 41 | /** 42 | * @brief Pack a log_erase message 43 | * @param system_id ID of this system 44 | * @param component_id ID of this component (e.g. 200 for IMU) 45 | * @param msg The MAVLink message to compress the data into 46 | * 47 | * @param target_system System ID 48 | * @param target_component Component ID 49 | * @return length of the message in bytes (excluding serial stream start sign) 50 | */ 51 | static inline uint16_t mavlink_msg_log_erase_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 52 | uint8_t target_system, uint8_t target_component) 53 | { 54 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 55 | char buf[MAVLINK_MSG_ID_LOG_ERASE_LEN]; 56 | _mav_put_uint8_t(buf, 0, target_system); 57 | _mav_put_uint8_t(buf, 1, target_component); 58 | 59 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_ERASE_LEN); 60 | #else 61 | mavlink_log_erase_t packet; 62 | packet.target_system = target_system; 63 | packet.target_component = target_component; 64 | 65 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_ERASE_LEN); 66 | #endif 67 | 68 | msg->msgid = MAVLINK_MSG_ID_LOG_ERASE; 69 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_ERASE_MIN_LEN, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); 70 | } 71 | 72 | /** 73 | * @brief Pack a log_erase message on a channel 74 | * @param system_id ID of this system 75 | * @param component_id ID of this component (e.g. 200 for IMU) 76 | * @param chan The MAVLink channel this message will be sent over 77 | * @param msg The MAVLink message to compress the data into 78 | * @param target_system System ID 79 | * @param target_component Component ID 80 | * @return length of the message in bytes (excluding serial stream start sign) 81 | */ 82 | static inline uint16_t mavlink_msg_log_erase_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 83 | mavlink_message_t* msg, 84 | uint8_t target_system,uint8_t target_component) 85 | { 86 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 87 | char buf[MAVLINK_MSG_ID_LOG_ERASE_LEN]; 88 | _mav_put_uint8_t(buf, 0, target_system); 89 | _mav_put_uint8_t(buf, 1, target_component); 90 | 91 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_ERASE_LEN); 92 | #else 93 | mavlink_log_erase_t packet; 94 | packet.target_system = target_system; 95 | packet.target_component = target_component; 96 | 97 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_ERASE_LEN); 98 | #endif 99 | 100 | msg->msgid = MAVLINK_MSG_ID_LOG_ERASE; 101 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_ERASE_MIN_LEN, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); 102 | } 103 | 104 | /** 105 | * @brief Encode a log_erase struct 106 | * 107 | * @param system_id ID of this system 108 | * @param component_id ID of this component (e.g. 200 for IMU) 109 | * @param msg The MAVLink message to compress the data into 110 | * @param log_erase C-struct to read the message contents from 111 | */ 112 | static inline uint16_t mavlink_msg_log_erase_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_erase_t* log_erase) 113 | { 114 | return mavlink_msg_log_erase_pack(system_id, component_id, msg, log_erase->target_system, log_erase->target_component); 115 | } 116 | 117 | /** 118 | * @brief Encode a log_erase struct on a channel 119 | * 120 | * @param system_id ID of this system 121 | * @param component_id ID of this component (e.g. 200 for IMU) 122 | * @param chan The MAVLink channel this message will be sent over 123 | * @param msg The MAVLink message to compress the data into 124 | * @param log_erase C-struct to read the message contents from 125 | */ 126 | static inline uint16_t mavlink_msg_log_erase_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_erase_t* log_erase) 127 | { 128 | return mavlink_msg_log_erase_pack_chan(system_id, component_id, chan, msg, log_erase->target_system, log_erase->target_component); 129 | } 130 | 131 | /** 132 | * @brief Send a log_erase message 133 | * @param chan MAVLink channel to send the message 134 | * 135 | * @param target_system System ID 136 | * @param target_component Component ID 137 | */ 138 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 139 | 140 | static inline void mavlink_msg_log_erase_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) 141 | { 142 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 143 | char buf[MAVLINK_MSG_ID_LOG_ERASE_LEN]; 144 | _mav_put_uint8_t(buf, 0, target_system); 145 | _mav_put_uint8_t(buf, 1, target_component); 146 | 147 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, buf, MAVLINK_MSG_ID_LOG_ERASE_MIN_LEN, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); 148 | #else 149 | mavlink_log_erase_t packet; 150 | packet.target_system = target_system; 151 | packet.target_component = target_component; 152 | 153 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, (const char *)&packet, MAVLINK_MSG_ID_LOG_ERASE_MIN_LEN, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); 154 | #endif 155 | } 156 | 157 | /** 158 | * @brief Send a log_erase message 159 | * @param chan MAVLink channel to send the message 160 | * @param struct The MAVLink struct to serialize 161 | */ 162 | static inline void mavlink_msg_log_erase_send_struct(mavlink_channel_t chan, const mavlink_log_erase_t* log_erase) 163 | { 164 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 165 | mavlink_msg_log_erase_send(chan, log_erase->target_system, log_erase->target_component); 166 | #else 167 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, (const char *)log_erase, MAVLINK_MSG_ID_LOG_ERASE_MIN_LEN, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); 168 | #endif 169 | } 170 | 171 | #if MAVLINK_MSG_ID_LOG_ERASE_LEN <= MAVLINK_MAX_PAYLOAD_LEN 172 | /* 173 | This varient of _send() can be used to save stack space by re-using 174 | memory from the receive buffer. The caller provides a 175 | mavlink_message_t which is the size of a full mavlink message. This 176 | is usually the receive buffer for the channel, and allows a reply to an 177 | incoming message with minimum stack space usage. 178 | */ 179 | static inline void mavlink_msg_log_erase_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) 180 | { 181 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 182 | char *buf = (char *)msgbuf; 183 | _mav_put_uint8_t(buf, 0, target_system); 184 | _mav_put_uint8_t(buf, 1, target_component); 185 | 186 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, buf, MAVLINK_MSG_ID_LOG_ERASE_MIN_LEN, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); 187 | #else 188 | mavlink_log_erase_t *packet = (mavlink_log_erase_t *)msgbuf; 189 | packet->target_system = target_system; 190 | packet->target_component = target_component; 191 | 192 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, (const char *)packet, MAVLINK_MSG_ID_LOG_ERASE_MIN_LEN, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); 193 | #endif 194 | } 195 | #endif 196 | 197 | #endif 198 | 199 | // MESSAGE LOG_ERASE UNPACKING 200 | 201 | 202 | /** 203 | * @brief Get field target_system from log_erase message 204 | * 205 | * @return System ID 206 | */ 207 | static inline uint8_t mavlink_msg_log_erase_get_target_system(const mavlink_message_t* msg) 208 | { 209 | return _MAV_RETURN_uint8_t(msg, 0); 210 | } 211 | 212 | /** 213 | * @brief Get field target_component from log_erase message 214 | * 215 | * @return Component ID 216 | */ 217 | static inline uint8_t mavlink_msg_log_erase_get_target_component(const mavlink_message_t* msg) 218 | { 219 | return _MAV_RETURN_uint8_t(msg, 1); 220 | } 221 | 222 | /** 223 | * @brief Decode a log_erase message into a struct 224 | * 225 | * @param msg The message to decode 226 | * @param log_erase C-struct to decode the message contents into 227 | */ 228 | static inline void mavlink_msg_log_erase_decode(const mavlink_message_t* msg, mavlink_log_erase_t* log_erase) 229 | { 230 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 231 | log_erase->target_system = mavlink_msg_log_erase_get_target_system(msg); 232 | log_erase->target_component = mavlink_msg_log_erase_get_target_component(msg); 233 | #else 234 | uint8_t len = msg->len < MAVLINK_MSG_ID_LOG_ERASE_LEN? msg->len : MAVLINK_MSG_ID_LOG_ERASE_LEN; 235 | memset(log_erase, 0, MAVLINK_MSG_ID_LOG_ERASE_LEN); 236 | memcpy(log_erase, _MAV_PAYLOAD(msg), len); 237 | #endif 238 | } 239 | -------------------------------------------------------------------------------- /src/ardupilotmega/mavlink_msg_battery2.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // MESSAGE BATTERY2 PACKING 3 | 4 | #define MAVLINK_MSG_ID_BATTERY2 181 5 | 6 | MAVPACKED( 7 | typedef struct __mavlink_battery2_t { 8 | uint16_t voltage; /*< voltage in millivolts*/ 9 | int16_t current_battery; /*< Battery current, in centiamperes (1 = 10 milliampere), -1: autopilot does not measure the current*/ 10 | }) mavlink_battery2_t; 11 | 12 | #define MAVLINK_MSG_ID_BATTERY2_LEN 4 13 | #define MAVLINK_MSG_ID_BATTERY2_MIN_LEN 4 14 | #define MAVLINK_MSG_ID_181_LEN 4 15 | #define MAVLINK_MSG_ID_181_MIN_LEN 4 16 | 17 | #define MAVLINK_MSG_ID_BATTERY2_CRC 174 18 | #define MAVLINK_MSG_ID_181_CRC 174 19 | 20 | 21 | 22 | #if MAVLINK_COMMAND_24BIT 23 | #define MAVLINK_MESSAGE_INFO_BATTERY2 { \ 24 | 181, \ 25 | "BATTERY2", \ 26 | 2, \ 27 | { { "voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_battery2_t, voltage) }, \ 28 | { "current_battery", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_battery2_t, current_battery) }, \ 29 | } \ 30 | } 31 | #else 32 | #define MAVLINK_MESSAGE_INFO_BATTERY2 { \ 33 | "BATTERY2", \ 34 | 2, \ 35 | { { "voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_battery2_t, voltage) }, \ 36 | { "current_battery", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_battery2_t, current_battery) }, \ 37 | } \ 38 | } 39 | #endif 40 | 41 | /** 42 | * @brief Pack a battery2 message 43 | * @param system_id ID of this system 44 | * @param component_id ID of this component (e.g. 200 for IMU) 45 | * @param msg The MAVLink message to compress the data into 46 | * 47 | * @param voltage voltage in millivolts 48 | * @param current_battery Battery current, in centiamperes (1 = 10 milliampere), -1: autopilot does not measure the current 49 | * @return length of the message in bytes (excluding serial stream start sign) 50 | */ 51 | static inline uint16_t mavlink_msg_battery2_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 52 | uint16_t voltage, int16_t current_battery) 53 | { 54 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 55 | char buf[MAVLINK_MSG_ID_BATTERY2_LEN]; 56 | _mav_put_uint16_t(buf, 0, voltage); 57 | _mav_put_int16_t(buf, 2, current_battery); 58 | 59 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BATTERY2_LEN); 60 | #else 61 | mavlink_battery2_t packet; 62 | packet.voltage = voltage; 63 | packet.current_battery = current_battery; 64 | 65 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BATTERY2_LEN); 66 | #endif 67 | 68 | msg->msgid = MAVLINK_MSG_ID_BATTERY2; 69 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BATTERY2_MIN_LEN, MAVLINK_MSG_ID_BATTERY2_LEN, MAVLINK_MSG_ID_BATTERY2_CRC); 70 | } 71 | 72 | /** 73 | * @brief Pack a battery2 message on a channel 74 | * @param system_id ID of this system 75 | * @param component_id ID of this component (e.g. 200 for IMU) 76 | * @param chan The MAVLink channel this message will be sent over 77 | * @param msg The MAVLink message to compress the data into 78 | * @param voltage voltage in millivolts 79 | * @param current_battery Battery current, in centiamperes (1 = 10 milliampere), -1: autopilot does not measure the current 80 | * @return length of the message in bytes (excluding serial stream start sign) 81 | */ 82 | static inline uint16_t mavlink_msg_battery2_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 83 | mavlink_message_t* msg, 84 | uint16_t voltage,int16_t current_battery) 85 | { 86 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 87 | char buf[MAVLINK_MSG_ID_BATTERY2_LEN]; 88 | _mav_put_uint16_t(buf, 0, voltage); 89 | _mav_put_int16_t(buf, 2, current_battery); 90 | 91 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BATTERY2_LEN); 92 | #else 93 | mavlink_battery2_t packet; 94 | packet.voltage = voltage; 95 | packet.current_battery = current_battery; 96 | 97 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BATTERY2_LEN); 98 | #endif 99 | 100 | msg->msgid = MAVLINK_MSG_ID_BATTERY2; 101 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BATTERY2_MIN_LEN, MAVLINK_MSG_ID_BATTERY2_LEN, MAVLINK_MSG_ID_BATTERY2_CRC); 102 | } 103 | 104 | /** 105 | * @brief Encode a battery2 struct 106 | * 107 | * @param system_id ID of this system 108 | * @param component_id ID of this component (e.g. 200 for IMU) 109 | * @param msg The MAVLink message to compress the data into 110 | * @param battery2 C-struct to read the message contents from 111 | */ 112 | static inline uint16_t mavlink_msg_battery2_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_battery2_t* battery2) 113 | { 114 | return mavlink_msg_battery2_pack(system_id, component_id, msg, battery2->voltage, battery2->current_battery); 115 | } 116 | 117 | /** 118 | * @brief Encode a battery2 struct on a channel 119 | * 120 | * @param system_id ID of this system 121 | * @param component_id ID of this component (e.g. 200 for IMU) 122 | * @param chan The MAVLink channel this message will be sent over 123 | * @param msg The MAVLink message to compress the data into 124 | * @param battery2 C-struct to read the message contents from 125 | */ 126 | static inline uint16_t mavlink_msg_battery2_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_battery2_t* battery2) 127 | { 128 | return mavlink_msg_battery2_pack_chan(system_id, component_id, chan, msg, battery2->voltage, battery2->current_battery); 129 | } 130 | 131 | /** 132 | * @brief Send a battery2 message 133 | * @param chan MAVLink channel to send the message 134 | * 135 | * @param voltage voltage in millivolts 136 | * @param current_battery Battery current, in centiamperes (1 = 10 milliampere), -1: autopilot does not measure the current 137 | */ 138 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 139 | 140 | static inline void mavlink_msg_battery2_send(mavlink_channel_t chan, uint16_t voltage, int16_t current_battery) 141 | { 142 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 143 | char buf[MAVLINK_MSG_ID_BATTERY2_LEN]; 144 | _mav_put_uint16_t(buf, 0, voltage); 145 | _mav_put_int16_t(buf, 2, current_battery); 146 | 147 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY2, buf, MAVLINK_MSG_ID_BATTERY2_MIN_LEN, MAVLINK_MSG_ID_BATTERY2_LEN, MAVLINK_MSG_ID_BATTERY2_CRC); 148 | #else 149 | mavlink_battery2_t packet; 150 | packet.voltage = voltage; 151 | packet.current_battery = current_battery; 152 | 153 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY2, (const char *)&packet, MAVLINK_MSG_ID_BATTERY2_MIN_LEN, MAVLINK_MSG_ID_BATTERY2_LEN, MAVLINK_MSG_ID_BATTERY2_CRC); 154 | #endif 155 | } 156 | 157 | /** 158 | * @brief Send a battery2 message 159 | * @param chan MAVLink channel to send the message 160 | * @param struct The MAVLink struct to serialize 161 | */ 162 | static inline void mavlink_msg_battery2_send_struct(mavlink_channel_t chan, const mavlink_battery2_t* battery2) 163 | { 164 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 165 | mavlink_msg_battery2_send(chan, battery2->voltage, battery2->current_battery); 166 | #else 167 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY2, (const char *)battery2, MAVLINK_MSG_ID_BATTERY2_MIN_LEN, MAVLINK_MSG_ID_BATTERY2_LEN, MAVLINK_MSG_ID_BATTERY2_CRC); 168 | #endif 169 | } 170 | 171 | #if MAVLINK_MSG_ID_BATTERY2_LEN <= MAVLINK_MAX_PAYLOAD_LEN 172 | /* 173 | This varient of _send() can be used to save stack space by re-using 174 | memory from the receive buffer. The caller provides a 175 | mavlink_message_t which is the size of a full mavlink message. This 176 | is usually the receive buffer for the channel, and allows a reply to an 177 | incoming message with minimum stack space usage. 178 | */ 179 | static inline void mavlink_msg_battery2_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t voltage, int16_t current_battery) 180 | { 181 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 182 | char *buf = (char *)msgbuf; 183 | _mav_put_uint16_t(buf, 0, voltage); 184 | _mav_put_int16_t(buf, 2, current_battery); 185 | 186 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY2, buf, MAVLINK_MSG_ID_BATTERY2_MIN_LEN, MAVLINK_MSG_ID_BATTERY2_LEN, MAVLINK_MSG_ID_BATTERY2_CRC); 187 | #else 188 | mavlink_battery2_t *packet = (mavlink_battery2_t *)msgbuf; 189 | packet->voltage = voltage; 190 | packet->current_battery = current_battery; 191 | 192 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY2, (const char *)packet, MAVLINK_MSG_ID_BATTERY2_MIN_LEN, MAVLINK_MSG_ID_BATTERY2_LEN, MAVLINK_MSG_ID_BATTERY2_CRC); 193 | #endif 194 | } 195 | #endif 196 | 197 | #endif 198 | 199 | // MESSAGE BATTERY2 UNPACKING 200 | 201 | 202 | /** 203 | * @brief Get field voltage from battery2 message 204 | * 205 | * @return voltage in millivolts 206 | */ 207 | static inline uint16_t mavlink_msg_battery2_get_voltage(const mavlink_message_t* msg) 208 | { 209 | return _MAV_RETURN_uint16_t(msg, 0); 210 | } 211 | 212 | /** 213 | * @brief Get field current_battery from battery2 message 214 | * 215 | * @return Battery current, in centiamperes (1 = 10 milliampere), -1: autopilot does not measure the current 216 | */ 217 | static inline int16_t mavlink_msg_battery2_get_current_battery(const mavlink_message_t* msg) 218 | { 219 | return _MAV_RETURN_int16_t(msg, 2); 220 | } 221 | 222 | /** 223 | * @brief Decode a battery2 message into a struct 224 | * 225 | * @param msg The message to decode 226 | * @param battery2 C-struct to decode the message contents into 227 | */ 228 | static inline void mavlink_msg_battery2_decode(const mavlink_message_t* msg, mavlink_battery2_t* battery2) 229 | { 230 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 231 | battery2->voltage = mavlink_msg_battery2_get_voltage(msg); 232 | battery2->current_battery = mavlink_msg_battery2_get_current_battery(msg); 233 | #else 234 | uint8_t len = msg->len < MAVLINK_MSG_ID_BATTERY2_LEN? msg->len : MAVLINK_MSG_ID_BATTERY2_LEN; 235 | memset(battery2, 0, MAVLINK_MSG_ID_BATTERY2_LEN); 236 | memcpy(battery2, _MAV_PAYLOAD(msg), len); 237 | #endif 238 | } 239 | -------------------------------------------------------------------------------- /src/common/mavlink_msg_camera_trigger.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // MESSAGE CAMERA_TRIGGER PACKING 3 | 4 | #define MAVLINK_MSG_ID_CAMERA_TRIGGER 112 5 | 6 | MAVPACKED( 7 | typedef struct __mavlink_camera_trigger_t { 8 | uint64_t time_usec; /*< Timestamp for the image frame in microseconds*/ 9 | uint32_t seq; /*< Image frame sequence*/ 10 | }) mavlink_camera_trigger_t; 11 | 12 | #define MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN 12 13 | #define MAVLINK_MSG_ID_CAMERA_TRIGGER_MIN_LEN 12 14 | #define MAVLINK_MSG_ID_112_LEN 12 15 | #define MAVLINK_MSG_ID_112_MIN_LEN 12 16 | 17 | #define MAVLINK_MSG_ID_CAMERA_TRIGGER_CRC 174 18 | #define MAVLINK_MSG_ID_112_CRC 174 19 | 20 | 21 | 22 | #if MAVLINK_COMMAND_24BIT 23 | #define MAVLINK_MESSAGE_INFO_CAMERA_TRIGGER { \ 24 | 112, \ 25 | "CAMERA_TRIGGER", \ 26 | 2, \ 27 | { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_camera_trigger_t, time_usec) }, \ 28 | { "seq", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_camera_trigger_t, seq) }, \ 29 | } \ 30 | } 31 | #else 32 | #define MAVLINK_MESSAGE_INFO_CAMERA_TRIGGER { \ 33 | "CAMERA_TRIGGER", \ 34 | 2, \ 35 | { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_camera_trigger_t, time_usec) }, \ 36 | { "seq", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_camera_trigger_t, seq) }, \ 37 | } \ 38 | } 39 | #endif 40 | 41 | /** 42 | * @brief Pack a camera_trigger message 43 | * @param system_id ID of this system 44 | * @param component_id ID of this component (e.g. 200 for IMU) 45 | * @param msg The MAVLink message to compress the data into 46 | * 47 | * @param time_usec Timestamp for the image frame in microseconds 48 | * @param seq Image frame sequence 49 | * @return length of the message in bytes (excluding serial stream start sign) 50 | */ 51 | static inline uint16_t mavlink_msg_camera_trigger_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 52 | uint64_t time_usec, uint32_t seq) 53 | { 54 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 55 | char buf[MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN]; 56 | _mav_put_uint64_t(buf, 0, time_usec); 57 | _mav_put_uint32_t(buf, 8, seq); 58 | 59 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN); 60 | #else 61 | mavlink_camera_trigger_t packet; 62 | packet.time_usec = time_usec; 63 | packet.seq = seq; 64 | 65 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN); 66 | #endif 67 | 68 | msg->msgid = MAVLINK_MSG_ID_CAMERA_TRIGGER; 69 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_TRIGGER_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_CRC); 70 | } 71 | 72 | /** 73 | * @brief Pack a camera_trigger message on a channel 74 | * @param system_id ID of this system 75 | * @param component_id ID of this component (e.g. 200 for IMU) 76 | * @param chan The MAVLink channel this message will be sent over 77 | * @param msg The MAVLink message to compress the data into 78 | * @param time_usec Timestamp for the image frame in microseconds 79 | * @param seq Image frame sequence 80 | * @return length of the message in bytes (excluding serial stream start sign) 81 | */ 82 | static inline uint16_t mavlink_msg_camera_trigger_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 83 | mavlink_message_t* msg, 84 | uint64_t time_usec,uint32_t seq) 85 | { 86 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 87 | char buf[MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN]; 88 | _mav_put_uint64_t(buf, 0, time_usec); 89 | _mav_put_uint32_t(buf, 8, seq); 90 | 91 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN); 92 | #else 93 | mavlink_camera_trigger_t packet; 94 | packet.time_usec = time_usec; 95 | packet.seq = seq; 96 | 97 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN); 98 | #endif 99 | 100 | msg->msgid = MAVLINK_MSG_ID_CAMERA_TRIGGER; 101 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_TRIGGER_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_CRC); 102 | } 103 | 104 | /** 105 | * @brief Encode a camera_trigger struct 106 | * 107 | * @param system_id ID of this system 108 | * @param component_id ID of this component (e.g. 200 for IMU) 109 | * @param msg The MAVLink message to compress the data into 110 | * @param camera_trigger C-struct to read the message contents from 111 | */ 112 | static inline uint16_t mavlink_msg_camera_trigger_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_trigger_t* camera_trigger) 113 | { 114 | return mavlink_msg_camera_trigger_pack(system_id, component_id, msg, camera_trigger->time_usec, camera_trigger->seq); 115 | } 116 | 117 | /** 118 | * @brief Encode a camera_trigger struct on a channel 119 | * 120 | * @param system_id ID of this system 121 | * @param component_id ID of this component (e.g. 200 for IMU) 122 | * @param chan The MAVLink channel this message will be sent over 123 | * @param msg The MAVLink message to compress the data into 124 | * @param camera_trigger C-struct to read the message contents from 125 | */ 126 | static inline uint16_t mavlink_msg_camera_trigger_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_trigger_t* camera_trigger) 127 | { 128 | return mavlink_msg_camera_trigger_pack_chan(system_id, component_id, chan, msg, camera_trigger->time_usec, camera_trigger->seq); 129 | } 130 | 131 | /** 132 | * @brief Send a camera_trigger message 133 | * @param chan MAVLink channel to send the message 134 | * 135 | * @param time_usec Timestamp for the image frame in microseconds 136 | * @param seq Image frame sequence 137 | */ 138 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 139 | 140 | static inline void mavlink_msg_camera_trigger_send(mavlink_channel_t chan, uint64_t time_usec, uint32_t seq) 141 | { 142 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 143 | char buf[MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN]; 144 | _mav_put_uint64_t(buf, 0, time_usec); 145 | _mav_put_uint32_t(buf, 8, seq); 146 | 147 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRIGGER, buf, MAVLINK_MSG_ID_CAMERA_TRIGGER_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_CRC); 148 | #else 149 | mavlink_camera_trigger_t packet; 150 | packet.time_usec = time_usec; 151 | packet.seq = seq; 152 | 153 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRIGGER, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_TRIGGER_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_CRC); 154 | #endif 155 | } 156 | 157 | /** 158 | * @brief Send a camera_trigger message 159 | * @param chan MAVLink channel to send the message 160 | * @param struct The MAVLink struct to serialize 161 | */ 162 | static inline void mavlink_msg_camera_trigger_send_struct(mavlink_channel_t chan, const mavlink_camera_trigger_t* camera_trigger) 163 | { 164 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 165 | mavlink_msg_camera_trigger_send(chan, camera_trigger->time_usec, camera_trigger->seq); 166 | #else 167 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRIGGER, (const char *)camera_trigger, MAVLINK_MSG_ID_CAMERA_TRIGGER_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_CRC); 168 | #endif 169 | } 170 | 171 | #if MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN <= MAVLINK_MAX_PAYLOAD_LEN 172 | /* 173 | This varient of _send() can be used to save stack space by re-using 174 | memory from the receive buffer. The caller provides a 175 | mavlink_message_t which is the size of a full mavlink message. This 176 | is usually the receive buffer for the channel, and allows a reply to an 177 | incoming message with minimum stack space usage. 178 | */ 179 | static inline void mavlink_msg_camera_trigger_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint32_t seq) 180 | { 181 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 182 | char *buf = (char *)msgbuf; 183 | _mav_put_uint64_t(buf, 0, time_usec); 184 | _mav_put_uint32_t(buf, 8, seq); 185 | 186 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRIGGER, buf, MAVLINK_MSG_ID_CAMERA_TRIGGER_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_CRC); 187 | #else 188 | mavlink_camera_trigger_t *packet = (mavlink_camera_trigger_t *)msgbuf; 189 | packet->time_usec = time_usec; 190 | packet->seq = seq; 191 | 192 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRIGGER, (const char *)packet, MAVLINK_MSG_ID_CAMERA_TRIGGER_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_CRC); 193 | #endif 194 | } 195 | #endif 196 | 197 | #endif 198 | 199 | // MESSAGE CAMERA_TRIGGER UNPACKING 200 | 201 | 202 | /** 203 | * @brief Get field time_usec from camera_trigger message 204 | * 205 | * @return Timestamp for the image frame in microseconds 206 | */ 207 | static inline uint64_t mavlink_msg_camera_trigger_get_time_usec(const mavlink_message_t* msg) 208 | { 209 | return _MAV_RETURN_uint64_t(msg, 0); 210 | } 211 | 212 | /** 213 | * @brief Get field seq from camera_trigger message 214 | * 215 | * @return Image frame sequence 216 | */ 217 | static inline uint32_t mavlink_msg_camera_trigger_get_seq(const mavlink_message_t* msg) 218 | { 219 | return _MAV_RETURN_uint32_t(msg, 8); 220 | } 221 | 222 | /** 223 | * @brief Decode a camera_trigger message into a struct 224 | * 225 | * @param msg The message to decode 226 | * @param camera_trigger C-struct to decode the message contents into 227 | */ 228 | static inline void mavlink_msg_camera_trigger_decode(const mavlink_message_t* msg, mavlink_camera_trigger_t* camera_trigger) 229 | { 230 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 231 | camera_trigger->time_usec = mavlink_msg_camera_trigger_get_time_usec(msg); 232 | camera_trigger->seq = mavlink_msg_camera_trigger_get_seq(msg); 233 | #else 234 | uint8_t len = msg->len < MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN? msg->len : MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN; 235 | memset(camera_trigger, 0, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN); 236 | memcpy(camera_trigger, _MAV_PAYLOAD(msg), len); 237 | #endif 238 | } 239 | -------------------------------------------------------------------------------- /src/ardupilotmega/mavlink_msg_data16.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // MESSAGE DATA16 PACKING 3 | 4 | #define MAVLINK_MSG_ID_DATA16 169 5 | 6 | MAVPACKED( 7 | typedef struct __mavlink_data16_t { 8 | uint8_t type; /*< data type*/ 9 | uint8_t len; /*< data length*/ 10 | uint8_t data[16]; /*< raw data*/ 11 | }) mavlink_data16_t; 12 | 13 | #define MAVLINK_MSG_ID_DATA16_LEN 18 14 | #define MAVLINK_MSG_ID_DATA16_MIN_LEN 18 15 | #define MAVLINK_MSG_ID_169_LEN 18 16 | #define MAVLINK_MSG_ID_169_MIN_LEN 18 17 | 18 | #define MAVLINK_MSG_ID_DATA16_CRC 234 19 | #define MAVLINK_MSG_ID_169_CRC 234 20 | 21 | #define MAVLINK_MSG_DATA16_FIELD_DATA_LEN 16 22 | 23 | #if MAVLINK_COMMAND_24BIT 24 | #define MAVLINK_MESSAGE_INFO_DATA16 { \ 25 | 169, \ 26 | "DATA16", \ 27 | 3, \ 28 | { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data16_t, type) }, \ 29 | { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data16_t, len) }, \ 30 | { "data", NULL, MAVLINK_TYPE_UINT8_T, 16, 2, offsetof(mavlink_data16_t, data) }, \ 31 | } \ 32 | } 33 | #else 34 | #define MAVLINK_MESSAGE_INFO_DATA16 { \ 35 | "DATA16", \ 36 | 3, \ 37 | { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data16_t, type) }, \ 38 | { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data16_t, len) }, \ 39 | { "data", NULL, MAVLINK_TYPE_UINT8_T, 16, 2, offsetof(mavlink_data16_t, data) }, \ 40 | } \ 41 | } 42 | #endif 43 | 44 | /** 45 | * @brief Pack a data16 message 46 | * @param system_id ID of this system 47 | * @param component_id ID of this component (e.g. 200 for IMU) 48 | * @param msg The MAVLink message to compress the data into 49 | * 50 | * @param type data type 51 | * @param len data length 52 | * @param data raw data 53 | * @return length of the message in bytes (excluding serial stream start sign) 54 | */ 55 | static inline uint16_t mavlink_msg_data16_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 56 | uint8_t type, uint8_t len, const uint8_t *data) 57 | { 58 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 59 | char buf[MAVLINK_MSG_ID_DATA16_LEN]; 60 | _mav_put_uint8_t(buf, 0, type); 61 | _mav_put_uint8_t(buf, 1, len); 62 | _mav_put_uint8_t_array(buf, 2, data, 16); 63 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA16_LEN); 64 | #else 65 | mavlink_data16_t packet; 66 | packet.type = type; 67 | packet.len = len; 68 | mav_array_memcpy(packet.data, data, sizeof(uint8_t)*16); 69 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA16_LEN); 70 | #endif 71 | 72 | msg->msgid = MAVLINK_MSG_ID_DATA16; 73 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA16_MIN_LEN, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); 74 | } 75 | 76 | /** 77 | * @brief Pack a data16 message on a channel 78 | * @param system_id ID of this system 79 | * @param component_id ID of this component (e.g. 200 for IMU) 80 | * @param chan The MAVLink channel this message will be sent over 81 | * @param msg The MAVLink message to compress the data into 82 | * @param type data type 83 | * @param len data length 84 | * @param data raw data 85 | * @return length of the message in bytes (excluding serial stream start sign) 86 | */ 87 | static inline uint16_t mavlink_msg_data16_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 88 | mavlink_message_t* msg, 89 | uint8_t type,uint8_t len,const uint8_t *data) 90 | { 91 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 92 | char buf[MAVLINK_MSG_ID_DATA16_LEN]; 93 | _mav_put_uint8_t(buf, 0, type); 94 | _mav_put_uint8_t(buf, 1, len); 95 | _mav_put_uint8_t_array(buf, 2, data, 16); 96 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA16_LEN); 97 | #else 98 | mavlink_data16_t packet; 99 | packet.type = type; 100 | packet.len = len; 101 | mav_array_memcpy(packet.data, data, sizeof(uint8_t)*16); 102 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA16_LEN); 103 | #endif 104 | 105 | msg->msgid = MAVLINK_MSG_ID_DATA16; 106 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA16_MIN_LEN, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); 107 | } 108 | 109 | /** 110 | * @brief Encode a data16 struct 111 | * 112 | * @param system_id ID of this system 113 | * @param component_id ID of this component (e.g. 200 for IMU) 114 | * @param msg The MAVLink message to compress the data into 115 | * @param data16 C-struct to read the message contents from 116 | */ 117 | static inline uint16_t mavlink_msg_data16_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data16_t* data16) 118 | { 119 | return mavlink_msg_data16_pack(system_id, component_id, msg, data16->type, data16->len, data16->data); 120 | } 121 | 122 | /** 123 | * @brief Encode a data16 struct on a channel 124 | * 125 | * @param system_id ID of this system 126 | * @param component_id ID of this component (e.g. 200 for IMU) 127 | * @param chan The MAVLink channel this message will be sent over 128 | * @param msg The MAVLink message to compress the data into 129 | * @param data16 C-struct to read the message contents from 130 | */ 131 | static inline uint16_t mavlink_msg_data16_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data16_t* data16) 132 | { 133 | return mavlink_msg_data16_pack_chan(system_id, component_id, chan, msg, data16->type, data16->len, data16->data); 134 | } 135 | 136 | /** 137 | * @brief Send a data16 message 138 | * @param chan MAVLink channel to send the message 139 | * 140 | * @param type data type 141 | * @param len data length 142 | * @param data raw data 143 | */ 144 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 145 | 146 | static inline void mavlink_msg_data16_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) 147 | { 148 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 149 | char buf[MAVLINK_MSG_ID_DATA16_LEN]; 150 | _mav_put_uint8_t(buf, 0, type); 151 | _mav_put_uint8_t(buf, 1, len); 152 | _mav_put_uint8_t_array(buf, 2, data, 16); 153 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, buf, MAVLINK_MSG_ID_DATA16_MIN_LEN, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); 154 | #else 155 | mavlink_data16_t packet; 156 | packet.type = type; 157 | packet.len = len; 158 | mav_array_memcpy(packet.data, data, sizeof(uint8_t)*16); 159 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, (const char *)&packet, MAVLINK_MSG_ID_DATA16_MIN_LEN, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); 160 | #endif 161 | } 162 | 163 | /** 164 | * @brief Send a data16 message 165 | * @param chan MAVLink channel to send the message 166 | * @param struct The MAVLink struct to serialize 167 | */ 168 | static inline void mavlink_msg_data16_send_struct(mavlink_channel_t chan, const mavlink_data16_t* data16) 169 | { 170 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 171 | mavlink_msg_data16_send(chan, data16->type, data16->len, data16->data); 172 | #else 173 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, (const char *)data16, MAVLINK_MSG_ID_DATA16_MIN_LEN, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); 174 | #endif 175 | } 176 | 177 | #if MAVLINK_MSG_ID_DATA16_LEN <= MAVLINK_MAX_PAYLOAD_LEN 178 | /* 179 | This varient of _send() can be used to save stack space by re-using 180 | memory from the receive buffer. The caller provides a 181 | mavlink_message_t which is the size of a full mavlink message. This 182 | is usually the receive buffer for the channel, and allows a reply to an 183 | incoming message with minimum stack space usage. 184 | */ 185 | static inline void mavlink_msg_data16_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) 186 | { 187 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 188 | char *buf = (char *)msgbuf; 189 | _mav_put_uint8_t(buf, 0, type); 190 | _mav_put_uint8_t(buf, 1, len); 191 | _mav_put_uint8_t_array(buf, 2, data, 16); 192 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, buf, MAVLINK_MSG_ID_DATA16_MIN_LEN, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); 193 | #else 194 | mavlink_data16_t *packet = (mavlink_data16_t *)msgbuf; 195 | packet->type = type; 196 | packet->len = len; 197 | mav_array_memcpy(packet->data, data, sizeof(uint8_t)*16); 198 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, (const char *)packet, MAVLINK_MSG_ID_DATA16_MIN_LEN, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); 199 | #endif 200 | } 201 | #endif 202 | 203 | #endif 204 | 205 | // MESSAGE DATA16 UNPACKING 206 | 207 | 208 | /** 209 | * @brief Get field type from data16 message 210 | * 211 | * @return data type 212 | */ 213 | static inline uint8_t mavlink_msg_data16_get_type(const mavlink_message_t* msg) 214 | { 215 | return _MAV_RETURN_uint8_t(msg, 0); 216 | } 217 | 218 | /** 219 | * @brief Get field len from data16 message 220 | * 221 | * @return data length 222 | */ 223 | static inline uint8_t mavlink_msg_data16_get_len(const mavlink_message_t* msg) 224 | { 225 | return _MAV_RETURN_uint8_t(msg, 1); 226 | } 227 | 228 | /** 229 | * @brief Get field data from data16 message 230 | * 231 | * @return raw data 232 | */ 233 | static inline uint16_t mavlink_msg_data16_get_data(const mavlink_message_t* msg, uint8_t *data) 234 | { 235 | return _MAV_RETURN_uint8_t_array(msg, data, 16, 2); 236 | } 237 | 238 | /** 239 | * @brief Decode a data16 message into a struct 240 | * 241 | * @param msg The message to decode 242 | * @param data16 C-struct to decode the message contents into 243 | */ 244 | static inline void mavlink_msg_data16_decode(const mavlink_message_t* msg, mavlink_data16_t* data16) 245 | { 246 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 247 | data16->type = mavlink_msg_data16_get_type(msg); 248 | data16->len = mavlink_msg_data16_get_len(msg); 249 | mavlink_msg_data16_get_data(msg, data16->data); 250 | #else 251 | uint8_t len = msg->len < MAVLINK_MSG_ID_DATA16_LEN? msg->len : MAVLINK_MSG_ID_DATA16_LEN; 252 | memset(data16, 0, MAVLINK_MSG_ID_DATA16_LEN); 253 | memcpy(data16, _MAV_PAYLOAD(msg), len); 254 | #endif 255 | } 256 | -------------------------------------------------------------------------------- /src/ardupilotmega/mavlink_msg_data32.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // MESSAGE DATA32 PACKING 3 | 4 | #define MAVLINK_MSG_ID_DATA32 170 5 | 6 | MAVPACKED( 7 | typedef struct __mavlink_data32_t { 8 | uint8_t type; /*< data type*/ 9 | uint8_t len; /*< data length*/ 10 | uint8_t data[32]; /*< raw data*/ 11 | }) mavlink_data32_t; 12 | 13 | #define MAVLINK_MSG_ID_DATA32_LEN 34 14 | #define MAVLINK_MSG_ID_DATA32_MIN_LEN 34 15 | #define MAVLINK_MSG_ID_170_LEN 34 16 | #define MAVLINK_MSG_ID_170_MIN_LEN 34 17 | 18 | #define MAVLINK_MSG_ID_DATA32_CRC 73 19 | #define MAVLINK_MSG_ID_170_CRC 73 20 | 21 | #define MAVLINK_MSG_DATA32_FIELD_DATA_LEN 32 22 | 23 | #if MAVLINK_COMMAND_24BIT 24 | #define MAVLINK_MESSAGE_INFO_DATA32 { \ 25 | 170, \ 26 | "DATA32", \ 27 | 3, \ 28 | { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data32_t, type) }, \ 29 | { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data32_t, len) }, \ 30 | { "data", NULL, MAVLINK_TYPE_UINT8_T, 32, 2, offsetof(mavlink_data32_t, data) }, \ 31 | } \ 32 | } 33 | #else 34 | #define MAVLINK_MESSAGE_INFO_DATA32 { \ 35 | "DATA32", \ 36 | 3, \ 37 | { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data32_t, type) }, \ 38 | { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data32_t, len) }, \ 39 | { "data", NULL, MAVLINK_TYPE_UINT8_T, 32, 2, offsetof(mavlink_data32_t, data) }, \ 40 | } \ 41 | } 42 | #endif 43 | 44 | /** 45 | * @brief Pack a data32 message 46 | * @param system_id ID of this system 47 | * @param component_id ID of this component (e.g. 200 for IMU) 48 | * @param msg The MAVLink message to compress the data into 49 | * 50 | * @param type data type 51 | * @param len data length 52 | * @param data raw data 53 | * @return length of the message in bytes (excluding serial stream start sign) 54 | */ 55 | static inline uint16_t mavlink_msg_data32_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 56 | uint8_t type, uint8_t len, const uint8_t *data) 57 | { 58 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 59 | char buf[MAVLINK_MSG_ID_DATA32_LEN]; 60 | _mav_put_uint8_t(buf, 0, type); 61 | _mav_put_uint8_t(buf, 1, len); 62 | _mav_put_uint8_t_array(buf, 2, data, 32); 63 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA32_LEN); 64 | #else 65 | mavlink_data32_t packet; 66 | packet.type = type; 67 | packet.len = len; 68 | mav_array_memcpy(packet.data, data, sizeof(uint8_t)*32); 69 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA32_LEN); 70 | #endif 71 | 72 | msg->msgid = MAVLINK_MSG_ID_DATA32; 73 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA32_MIN_LEN, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); 74 | } 75 | 76 | /** 77 | * @brief Pack a data32 message on a channel 78 | * @param system_id ID of this system 79 | * @param component_id ID of this component (e.g. 200 for IMU) 80 | * @param chan The MAVLink channel this message will be sent over 81 | * @param msg The MAVLink message to compress the data into 82 | * @param type data type 83 | * @param len data length 84 | * @param data raw data 85 | * @return length of the message in bytes (excluding serial stream start sign) 86 | */ 87 | static inline uint16_t mavlink_msg_data32_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 88 | mavlink_message_t* msg, 89 | uint8_t type,uint8_t len,const uint8_t *data) 90 | { 91 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 92 | char buf[MAVLINK_MSG_ID_DATA32_LEN]; 93 | _mav_put_uint8_t(buf, 0, type); 94 | _mav_put_uint8_t(buf, 1, len); 95 | _mav_put_uint8_t_array(buf, 2, data, 32); 96 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA32_LEN); 97 | #else 98 | mavlink_data32_t packet; 99 | packet.type = type; 100 | packet.len = len; 101 | mav_array_memcpy(packet.data, data, sizeof(uint8_t)*32); 102 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA32_LEN); 103 | #endif 104 | 105 | msg->msgid = MAVLINK_MSG_ID_DATA32; 106 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA32_MIN_LEN, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); 107 | } 108 | 109 | /** 110 | * @brief Encode a data32 struct 111 | * 112 | * @param system_id ID of this system 113 | * @param component_id ID of this component (e.g. 200 for IMU) 114 | * @param msg The MAVLink message to compress the data into 115 | * @param data32 C-struct to read the message contents from 116 | */ 117 | static inline uint16_t mavlink_msg_data32_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data32_t* data32) 118 | { 119 | return mavlink_msg_data32_pack(system_id, component_id, msg, data32->type, data32->len, data32->data); 120 | } 121 | 122 | /** 123 | * @brief Encode a data32 struct on a channel 124 | * 125 | * @param system_id ID of this system 126 | * @param component_id ID of this component (e.g. 200 for IMU) 127 | * @param chan The MAVLink channel this message will be sent over 128 | * @param msg The MAVLink message to compress the data into 129 | * @param data32 C-struct to read the message contents from 130 | */ 131 | static inline uint16_t mavlink_msg_data32_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data32_t* data32) 132 | { 133 | return mavlink_msg_data32_pack_chan(system_id, component_id, chan, msg, data32->type, data32->len, data32->data); 134 | } 135 | 136 | /** 137 | * @brief Send a data32 message 138 | * @param chan MAVLink channel to send the message 139 | * 140 | * @param type data type 141 | * @param len data length 142 | * @param data raw data 143 | */ 144 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 145 | 146 | static inline void mavlink_msg_data32_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) 147 | { 148 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 149 | char buf[MAVLINK_MSG_ID_DATA32_LEN]; 150 | _mav_put_uint8_t(buf, 0, type); 151 | _mav_put_uint8_t(buf, 1, len); 152 | _mav_put_uint8_t_array(buf, 2, data, 32); 153 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, buf, MAVLINK_MSG_ID_DATA32_MIN_LEN, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); 154 | #else 155 | mavlink_data32_t packet; 156 | packet.type = type; 157 | packet.len = len; 158 | mav_array_memcpy(packet.data, data, sizeof(uint8_t)*32); 159 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, (const char *)&packet, MAVLINK_MSG_ID_DATA32_MIN_LEN, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); 160 | #endif 161 | } 162 | 163 | /** 164 | * @brief Send a data32 message 165 | * @param chan MAVLink channel to send the message 166 | * @param struct The MAVLink struct to serialize 167 | */ 168 | static inline void mavlink_msg_data32_send_struct(mavlink_channel_t chan, const mavlink_data32_t* data32) 169 | { 170 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 171 | mavlink_msg_data32_send(chan, data32->type, data32->len, data32->data); 172 | #else 173 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, (const char *)data32, MAVLINK_MSG_ID_DATA32_MIN_LEN, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); 174 | #endif 175 | } 176 | 177 | #if MAVLINK_MSG_ID_DATA32_LEN <= MAVLINK_MAX_PAYLOAD_LEN 178 | /* 179 | This varient of _send() can be used to save stack space by re-using 180 | memory from the receive buffer. The caller provides a 181 | mavlink_message_t which is the size of a full mavlink message. This 182 | is usually the receive buffer for the channel, and allows a reply to an 183 | incoming message with minimum stack space usage. 184 | */ 185 | static inline void mavlink_msg_data32_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) 186 | { 187 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 188 | char *buf = (char *)msgbuf; 189 | _mav_put_uint8_t(buf, 0, type); 190 | _mav_put_uint8_t(buf, 1, len); 191 | _mav_put_uint8_t_array(buf, 2, data, 32); 192 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, buf, MAVLINK_MSG_ID_DATA32_MIN_LEN, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); 193 | #else 194 | mavlink_data32_t *packet = (mavlink_data32_t *)msgbuf; 195 | packet->type = type; 196 | packet->len = len; 197 | mav_array_memcpy(packet->data, data, sizeof(uint8_t)*32); 198 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, (const char *)packet, MAVLINK_MSG_ID_DATA32_MIN_LEN, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); 199 | #endif 200 | } 201 | #endif 202 | 203 | #endif 204 | 205 | // MESSAGE DATA32 UNPACKING 206 | 207 | 208 | /** 209 | * @brief Get field type from data32 message 210 | * 211 | * @return data type 212 | */ 213 | static inline uint8_t mavlink_msg_data32_get_type(const mavlink_message_t* msg) 214 | { 215 | return _MAV_RETURN_uint8_t(msg, 0); 216 | } 217 | 218 | /** 219 | * @brief Get field len from data32 message 220 | * 221 | * @return data length 222 | */ 223 | static inline uint8_t mavlink_msg_data32_get_len(const mavlink_message_t* msg) 224 | { 225 | return _MAV_RETURN_uint8_t(msg, 1); 226 | } 227 | 228 | /** 229 | * @brief Get field data from data32 message 230 | * 231 | * @return raw data 232 | */ 233 | static inline uint16_t mavlink_msg_data32_get_data(const mavlink_message_t* msg, uint8_t *data) 234 | { 235 | return _MAV_RETURN_uint8_t_array(msg, data, 32, 2); 236 | } 237 | 238 | /** 239 | * @brief Decode a data32 message into a struct 240 | * 241 | * @param msg The message to decode 242 | * @param data32 C-struct to decode the message contents into 243 | */ 244 | static inline void mavlink_msg_data32_decode(const mavlink_message_t* msg, mavlink_data32_t* data32) 245 | { 246 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 247 | data32->type = mavlink_msg_data32_get_type(msg); 248 | data32->len = mavlink_msg_data32_get_len(msg); 249 | mavlink_msg_data32_get_data(msg, data32->data); 250 | #else 251 | uint8_t len = msg->len < MAVLINK_MSG_ID_DATA32_LEN? msg->len : MAVLINK_MSG_ID_DATA32_LEN; 252 | memset(data32, 0, MAVLINK_MSG_ID_DATA32_LEN); 253 | memcpy(data32, _MAV_PAYLOAD(msg), len); 254 | #endif 255 | } 256 | -------------------------------------------------------------------------------- /src/ardupilotmega/mavlink_msg_data64.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // MESSAGE DATA64 PACKING 3 | 4 | #define MAVLINK_MSG_ID_DATA64 171 5 | 6 | MAVPACKED( 7 | typedef struct __mavlink_data64_t { 8 | uint8_t type; /*< data type*/ 9 | uint8_t len; /*< data length*/ 10 | uint8_t data[64]; /*< raw data*/ 11 | }) mavlink_data64_t; 12 | 13 | #define MAVLINK_MSG_ID_DATA64_LEN 66 14 | #define MAVLINK_MSG_ID_DATA64_MIN_LEN 66 15 | #define MAVLINK_MSG_ID_171_LEN 66 16 | #define MAVLINK_MSG_ID_171_MIN_LEN 66 17 | 18 | #define MAVLINK_MSG_ID_DATA64_CRC 181 19 | #define MAVLINK_MSG_ID_171_CRC 181 20 | 21 | #define MAVLINK_MSG_DATA64_FIELD_DATA_LEN 64 22 | 23 | #if MAVLINK_COMMAND_24BIT 24 | #define MAVLINK_MESSAGE_INFO_DATA64 { \ 25 | 171, \ 26 | "DATA64", \ 27 | 3, \ 28 | { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data64_t, type) }, \ 29 | { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data64_t, len) }, \ 30 | { "data", NULL, MAVLINK_TYPE_UINT8_T, 64, 2, offsetof(mavlink_data64_t, data) }, \ 31 | } \ 32 | } 33 | #else 34 | #define MAVLINK_MESSAGE_INFO_DATA64 { \ 35 | "DATA64", \ 36 | 3, \ 37 | { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data64_t, type) }, \ 38 | { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data64_t, len) }, \ 39 | { "data", NULL, MAVLINK_TYPE_UINT8_T, 64, 2, offsetof(mavlink_data64_t, data) }, \ 40 | } \ 41 | } 42 | #endif 43 | 44 | /** 45 | * @brief Pack a data64 message 46 | * @param system_id ID of this system 47 | * @param component_id ID of this component (e.g. 200 for IMU) 48 | * @param msg The MAVLink message to compress the data into 49 | * 50 | * @param type data type 51 | * @param len data length 52 | * @param data raw data 53 | * @return length of the message in bytes (excluding serial stream start sign) 54 | */ 55 | static inline uint16_t mavlink_msg_data64_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 56 | uint8_t type, uint8_t len, const uint8_t *data) 57 | { 58 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 59 | char buf[MAVLINK_MSG_ID_DATA64_LEN]; 60 | _mav_put_uint8_t(buf, 0, type); 61 | _mav_put_uint8_t(buf, 1, len); 62 | _mav_put_uint8_t_array(buf, 2, data, 64); 63 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA64_LEN); 64 | #else 65 | mavlink_data64_t packet; 66 | packet.type = type; 67 | packet.len = len; 68 | mav_array_memcpy(packet.data, data, sizeof(uint8_t)*64); 69 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA64_LEN); 70 | #endif 71 | 72 | msg->msgid = MAVLINK_MSG_ID_DATA64; 73 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA64_MIN_LEN, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); 74 | } 75 | 76 | /** 77 | * @brief Pack a data64 message on a channel 78 | * @param system_id ID of this system 79 | * @param component_id ID of this component (e.g. 200 for IMU) 80 | * @param chan The MAVLink channel this message will be sent over 81 | * @param msg The MAVLink message to compress the data into 82 | * @param type data type 83 | * @param len data length 84 | * @param data raw data 85 | * @return length of the message in bytes (excluding serial stream start sign) 86 | */ 87 | static inline uint16_t mavlink_msg_data64_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 88 | mavlink_message_t* msg, 89 | uint8_t type,uint8_t len,const uint8_t *data) 90 | { 91 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 92 | char buf[MAVLINK_MSG_ID_DATA64_LEN]; 93 | _mav_put_uint8_t(buf, 0, type); 94 | _mav_put_uint8_t(buf, 1, len); 95 | _mav_put_uint8_t_array(buf, 2, data, 64); 96 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA64_LEN); 97 | #else 98 | mavlink_data64_t packet; 99 | packet.type = type; 100 | packet.len = len; 101 | mav_array_memcpy(packet.data, data, sizeof(uint8_t)*64); 102 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA64_LEN); 103 | #endif 104 | 105 | msg->msgid = MAVLINK_MSG_ID_DATA64; 106 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA64_MIN_LEN, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); 107 | } 108 | 109 | /** 110 | * @brief Encode a data64 struct 111 | * 112 | * @param system_id ID of this system 113 | * @param component_id ID of this component (e.g. 200 for IMU) 114 | * @param msg The MAVLink message to compress the data into 115 | * @param data64 C-struct to read the message contents from 116 | */ 117 | static inline uint16_t mavlink_msg_data64_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data64_t* data64) 118 | { 119 | return mavlink_msg_data64_pack(system_id, component_id, msg, data64->type, data64->len, data64->data); 120 | } 121 | 122 | /** 123 | * @brief Encode a data64 struct on a channel 124 | * 125 | * @param system_id ID of this system 126 | * @param component_id ID of this component (e.g. 200 for IMU) 127 | * @param chan The MAVLink channel this message will be sent over 128 | * @param msg The MAVLink message to compress the data into 129 | * @param data64 C-struct to read the message contents from 130 | */ 131 | static inline uint16_t mavlink_msg_data64_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data64_t* data64) 132 | { 133 | return mavlink_msg_data64_pack_chan(system_id, component_id, chan, msg, data64->type, data64->len, data64->data); 134 | } 135 | 136 | /** 137 | * @brief Send a data64 message 138 | * @param chan MAVLink channel to send the message 139 | * 140 | * @param type data type 141 | * @param len data length 142 | * @param data raw data 143 | */ 144 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 145 | 146 | static inline void mavlink_msg_data64_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) 147 | { 148 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 149 | char buf[MAVLINK_MSG_ID_DATA64_LEN]; 150 | _mav_put_uint8_t(buf, 0, type); 151 | _mav_put_uint8_t(buf, 1, len); 152 | _mav_put_uint8_t_array(buf, 2, data, 64); 153 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, buf, MAVLINK_MSG_ID_DATA64_MIN_LEN, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); 154 | #else 155 | mavlink_data64_t packet; 156 | packet.type = type; 157 | packet.len = len; 158 | mav_array_memcpy(packet.data, data, sizeof(uint8_t)*64); 159 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, (const char *)&packet, MAVLINK_MSG_ID_DATA64_MIN_LEN, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); 160 | #endif 161 | } 162 | 163 | /** 164 | * @brief Send a data64 message 165 | * @param chan MAVLink channel to send the message 166 | * @param struct The MAVLink struct to serialize 167 | */ 168 | static inline void mavlink_msg_data64_send_struct(mavlink_channel_t chan, const mavlink_data64_t* data64) 169 | { 170 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 171 | mavlink_msg_data64_send(chan, data64->type, data64->len, data64->data); 172 | #else 173 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, (const char *)data64, MAVLINK_MSG_ID_DATA64_MIN_LEN, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); 174 | #endif 175 | } 176 | 177 | #if MAVLINK_MSG_ID_DATA64_LEN <= MAVLINK_MAX_PAYLOAD_LEN 178 | /* 179 | This varient of _send() can be used to save stack space by re-using 180 | memory from the receive buffer. The caller provides a 181 | mavlink_message_t which is the size of a full mavlink message. This 182 | is usually the receive buffer for the channel, and allows a reply to an 183 | incoming message with minimum stack space usage. 184 | */ 185 | static inline void mavlink_msg_data64_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) 186 | { 187 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 188 | char *buf = (char *)msgbuf; 189 | _mav_put_uint8_t(buf, 0, type); 190 | _mav_put_uint8_t(buf, 1, len); 191 | _mav_put_uint8_t_array(buf, 2, data, 64); 192 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, buf, MAVLINK_MSG_ID_DATA64_MIN_LEN, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); 193 | #else 194 | mavlink_data64_t *packet = (mavlink_data64_t *)msgbuf; 195 | packet->type = type; 196 | packet->len = len; 197 | mav_array_memcpy(packet->data, data, sizeof(uint8_t)*64); 198 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, (const char *)packet, MAVLINK_MSG_ID_DATA64_MIN_LEN, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); 199 | #endif 200 | } 201 | #endif 202 | 203 | #endif 204 | 205 | // MESSAGE DATA64 UNPACKING 206 | 207 | 208 | /** 209 | * @brief Get field type from data64 message 210 | * 211 | * @return data type 212 | */ 213 | static inline uint8_t mavlink_msg_data64_get_type(const mavlink_message_t* msg) 214 | { 215 | return _MAV_RETURN_uint8_t(msg, 0); 216 | } 217 | 218 | /** 219 | * @brief Get field len from data64 message 220 | * 221 | * @return data length 222 | */ 223 | static inline uint8_t mavlink_msg_data64_get_len(const mavlink_message_t* msg) 224 | { 225 | return _MAV_RETURN_uint8_t(msg, 1); 226 | } 227 | 228 | /** 229 | * @brief Get field data from data64 message 230 | * 231 | * @return raw data 232 | */ 233 | static inline uint16_t mavlink_msg_data64_get_data(const mavlink_message_t* msg, uint8_t *data) 234 | { 235 | return _MAV_RETURN_uint8_t_array(msg, data, 64, 2); 236 | } 237 | 238 | /** 239 | * @brief Decode a data64 message into a struct 240 | * 241 | * @param msg The message to decode 242 | * @param data64 C-struct to decode the message contents into 243 | */ 244 | static inline void mavlink_msg_data64_decode(const mavlink_message_t* msg, mavlink_data64_t* data64) 245 | { 246 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 247 | data64->type = mavlink_msg_data64_get_type(msg); 248 | data64->len = mavlink_msg_data64_get_len(msg); 249 | mavlink_msg_data64_get_data(msg, data64->data); 250 | #else 251 | uint8_t len = msg->len < MAVLINK_MSG_ID_DATA64_LEN? msg->len : MAVLINK_MSG_ID_DATA64_LEN; 252 | memset(data64, 0, MAVLINK_MSG_ID_DATA64_LEN); 253 | memcpy(data64, _MAV_PAYLOAD(msg), len); 254 | #endif 255 | } 256 | -------------------------------------------------------------------------------- /src/ardupilotmega/mavlink_msg_data96.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // MESSAGE DATA96 PACKING 3 | 4 | #define MAVLINK_MSG_ID_DATA96 172 5 | 6 | MAVPACKED( 7 | typedef struct __mavlink_data96_t { 8 | uint8_t type; /*< data type*/ 9 | uint8_t len; /*< data length*/ 10 | uint8_t data[96]; /*< raw data*/ 11 | }) mavlink_data96_t; 12 | 13 | #define MAVLINK_MSG_ID_DATA96_LEN 98 14 | #define MAVLINK_MSG_ID_DATA96_MIN_LEN 98 15 | #define MAVLINK_MSG_ID_172_LEN 98 16 | #define MAVLINK_MSG_ID_172_MIN_LEN 98 17 | 18 | #define MAVLINK_MSG_ID_DATA96_CRC 22 19 | #define MAVLINK_MSG_ID_172_CRC 22 20 | 21 | #define MAVLINK_MSG_DATA96_FIELD_DATA_LEN 96 22 | 23 | #if MAVLINK_COMMAND_24BIT 24 | #define MAVLINK_MESSAGE_INFO_DATA96 { \ 25 | 172, \ 26 | "DATA96", \ 27 | 3, \ 28 | { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data96_t, type) }, \ 29 | { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data96_t, len) }, \ 30 | { "data", NULL, MAVLINK_TYPE_UINT8_T, 96, 2, offsetof(mavlink_data96_t, data) }, \ 31 | } \ 32 | } 33 | #else 34 | #define MAVLINK_MESSAGE_INFO_DATA96 { \ 35 | "DATA96", \ 36 | 3, \ 37 | { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data96_t, type) }, \ 38 | { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data96_t, len) }, \ 39 | { "data", NULL, MAVLINK_TYPE_UINT8_T, 96, 2, offsetof(mavlink_data96_t, data) }, \ 40 | } \ 41 | } 42 | #endif 43 | 44 | /** 45 | * @brief Pack a data96 message 46 | * @param system_id ID of this system 47 | * @param component_id ID of this component (e.g. 200 for IMU) 48 | * @param msg The MAVLink message to compress the data into 49 | * 50 | * @param type data type 51 | * @param len data length 52 | * @param data raw data 53 | * @return length of the message in bytes (excluding serial stream start sign) 54 | */ 55 | static inline uint16_t mavlink_msg_data96_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 56 | uint8_t type, uint8_t len, const uint8_t *data) 57 | { 58 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 59 | char buf[MAVLINK_MSG_ID_DATA96_LEN]; 60 | _mav_put_uint8_t(buf, 0, type); 61 | _mav_put_uint8_t(buf, 1, len); 62 | _mav_put_uint8_t_array(buf, 2, data, 96); 63 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA96_LEN); 64 | #else 65 | mavlink_data96_t packet; 66 | packet.type = type; 67 | packet.len = len; 68 | mav_array_memcpy(packet.data, data, sizeof(uint8_t)*96); 69 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA96_LEN); 70 | #endif 71 | 72 | msg->msgid = MAVLINK_MSG_ID_DATA96; 73 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA96_MIN_LEN, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); 74 | } 75 | 76 | /** 77 | * @brief Pack a data96 message on a channel 78 | * @param system_id ID of this system 79 | * @param component_id ID of this component (e.g. 200 for IMU) 80 | * @param chan The MAVLink channel this message will be sent over 81 | * @param msg The MAVLink message to compress the data into 82 | * @param type data type 83 | * @param len data length 84 | * @param data raw data 85 | * @return length of the message in bytes (excluding serial stream start sign) 86 | */ 87 | static inline uint16_t mavlink_msg_data96_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 88 | mavlink_message_t* msg, 89 | uint8_t type,uint8_t len,const uint8_t *data) 90 | { 91 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 92 | char buf[MAVLINK_MSG_ID_DATA96_LEN]; 93 | _mav_put_uint8_t(buf, 0, type); 94 | _mav_put_uint8_t(buf, 1, len); 95 | _mav_put_uint8_t_array(buf, 2, data, 96); 96 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA96_LEN); 97 | #else 98 | mavlink_data96_t packet; 99 | packet.type = type; 100 | packet.len = len; 101 | mav_array_memcpy(packet.data, data, sizeof(uint8_t)*96); 102 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA96_LEN); 103 | #endif 104 | 105 | msg->msgid = MAVLINK_MSG_ID_DATA96; 106 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA96_MIN_LEN, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); 107 | } 108 | 109 | /** 110 | * @brief Encode a data96 struct 111 | * 112 | * @param system_id ID of this system 113 | * @param component_id ID of this component (e.g. 200 for IMU) 114 | * @param msg The MAVLink message to compress the data into 115 | * @param data96 C-struct to read the message contents from 116 | */ 117 | static inline uint16_t mavlink_msg_data96_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data96_t* data96) 118 | { 119 | return mavlink_msg_data96_pack(system_id, component_id, msg, data96->type, data96->len, data96->data); 120 | } 121 | 122 | /** 123 | * @brief Encode a data96 struct on a channel 124 | * 125 | * @param system_id ID of this system 126 | * @param component_id ID of this component (e.g. 200 for IMU) 127 | * @param chan The MAVLink channel this message will be sent over 128 | * @param msg The MAVLink message to compress the data into 129 | * @param data96 C-struct to read the message contents from 130 | */ 131 | static inline uint16_t mavlink_msg_data96_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data96_t* data96) 132 | { 133 | return mavlink_msg_data96_pack_chan(system_id, component_id, chan, msg, data96->type, data96->len, data96->data); 134 | } 135 | 136 | /** 137 | * @brief Send a data96 message 138 | * @param chan MAVLink channel to send the message 139 | * 140 | * @param type data type 141 | * @param len data length 142 | * @param data raw data 143 | */ 144 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 145 | 146 | static inline void mavlink_msg_data96_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) 147 | { 148 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 149 | char buf[MAVLINK_MSG_ID_DATA96_LEN]; 150 | _mav_put_uint8_t(buf, 0, type); 151 | _mav_put_uint8_t(buf, 1, len); 152 | _mav_put_uint8_t_array(buf, 2, data, 96); 153 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, buf, MAVLINK_MSG_ID_DATA96_MIN_LEN, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); 154 | #else 155 | mavlink_data96_t packet; 156 | packet.type = type; 157 | packet.len = len; 158 | mav_array_memcpy(packet.data, data, sizeof(uint8_t)*96); 159 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, (const char *)&packet, MAVLINK_MSG_ID_DATA96_MIN_LEN, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); 160 | #endif 161 | } 162 | 163 | /** 164 | * @brief Send a data96 message 165 | * @param chan MAVLink channel to send the message 166 | * @param struct The MAVLink struct to serialize 167 | */ 168 | static inline void mavlink_msg_data96_send_struct(mavlink_channel_t chan, const mavlink_data96_t* data96) 169 | { 170 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 171 | mavlink_msg_data96_send(chan, data96->type, data96->len, data96->data); 172 | #else 173 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, (const char *)data96, MAVLINK_MSG_ID_DATA96_MIN_LEN, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); 174 | #endif 175 | } 176 | 177 | #if MAVLINK_MSG_ID_DATA96_LEN <= MAVLINK_MAX_PAYLOAD_LEN 178 | /* 179 | This varient of _send() can be used to save stack space by re-using 180 | memory from the receive buffer. The caller provides a 181 | mavlink_message_t which is the size of a full mavlink message. This 182 | is usually the receive buffer for the channel, and allows a reply to an 183 | incoming message with minimum stack space usage. 184 | */ 185 | static inline void mavlink_msg_data96_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) 186 | { 187 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 188 | char *buf = (char *)msgbuf; 189 | _mav_put_uint8_t(buf, 0, type); 190 | _mav_put_uint8_t(buf, 1, len); 191 | _mav_put_uint8_t_array(buf, 2, data, 96); 192 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, buf, MAVLINK_MSG_ID_DATA96_MIN_LEN, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); 193 | #else 194 | mavlink_data96_t *packet = (mavlink_data96_t *)msgbuf; 195 | packet->type = type; 196 | packet->len = len; 197 | mav_array_memcpy(packet->data, data, sizeof(uint8_t)*96); 198 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, (const char *)packet, MAVLINK_MSG_ID_DATA96_MIN_LEN, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); 199 | #endif 200 | } 201 | #endif 202 | 203 | #endif 204 | 205 | // MESSAGE DATA96 UNPACKING 206 | 207 | 208 | /** 209 | * @brief Get field type from data96 message 210 | * 211 | * @return data type 212 | */ 213 | static inline uint8_t mavlink_msg_data96_get_type(const mavlink_message_t* msg) 214 | { 215 | return _MAV_RETURN_uint8_t(msg, 0); 216 | } 217 | 218 | /** 219 | * @brief Get field len from data96 message 220 | * 221 | * @return data length 222 | */ 223 | static inline uint8_t mavlink_msg_data96_get_len(const mavlink_message_t* msg) 224 | { 225 | return _MAV_RETURN_uint8_t(msg, 1); 226 | } 227 | 228 | /** 229 | * @brief Get field data from data96 message 230 | * 231 | * @return raw data 232 | */ 233 | static inline uint16_t mavlink_msg_data96_get_data(const mavlink_message_t* msg, uint8_t *data) 234 | { 235 | return _MAV_RETURN_uint8_t_array(msg, data, 96, 2); 236 | } 237 | 238 | /** 239 | * @brief Decode a data96 message into a struct 240 | * 241 | * @param msg The message to decode 242 | * @param data96 C-struct to decode the message contents into 243 | */ 244 | static inline void mavlink_msg_data96_decode(const mavlink_message_t* msg, mavlink_data96_t* data96) 245 | { 246 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 247 | data96->type = mavlink_msg_data96_get_type(msg); 248 | data96->len = mavlink_msg_data96_get_len(msg); 249 | mavlink_msg_data96_get_data(msg, data96->data); 250 | #else 251 | uint8_t len = msg->len < MAVLINK_MSG_ID_DATA96_LEN? msg->len : MAVLINK_MSG_ID_DATA96_LEN; 252 | memset(data96, 0, MAVLINK_MSG_ID_DATA96_LEN); 253 | memcpy(data96, _MAV_PAYLOAD(msg), len); 254 | #endif 255 | } 256 | -------------------------------------------------------------------------------- /src/common/mavlink_msg_statustext.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // MESSAGE STATUSTEXT PACKING 3 | 4 | #define MAVLINK_MSG_ID_STATUSTEXT 253 5 | 6 | MAVPACKED( 7 | typedef struct __mavlink_statustext_t { 8 | uint8_t severity; /*< Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY.*/ 9 | char text[50]; /*< Status text message, without null termination character*/ 10 | }) mavlink_statustext_t; 11 | 12 | #define MAVLINK_MSG_ID_STATUSTEXT_LEN 51 13 | #define MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN 51 14 | #define MAVLINK_MSG_ID_253_LEN 51 15 | #define MAVLINK_MSG_ID_253_MIN_LEN 51 16 | 17 | #define MAVLINK_MSG_ID_STATUSTEXT_CRC 83 18 | #define MAVLINK_MSG_ID_253_CRC 83 19 | 20 | #define MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN 50 21 | 22 | #if MAVLINK_COMMAND_24BIT 23 | #define MAVLINK_MESSAGE_INFO_STATUSTEXT { \ 24 | 253, \ 25 | "STATUSTEXT", \ 26 | 2, \ 27 | { { "severity", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_statustext_t, severity) }, \ 28 | { "text", NULL, MAVLINK_TYPE_CHAR, 50, 1, offsetof(mavlink_statustext_t, text) }, \ 29 | } \ 30 | } 31 | #else 32 | #define MAVLINK_MESSAGE_INFO_STATUSTEXT { \ 33 | "STATUSTEXT", \ 34 | 2, \ 35 | { { "severity", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_statustext_t, severity) }, \ 36 | { "text", NULL, MAVLINK_TYPE_CHAR, 50, 1, offsetof(mavlink_statustext_t, text) }, \ 37 | } \ 38 | } 39 | #endif 40 | 41 | /** 42 | * @brief Pack a statustext message 43 | * @param system_id ID of this system 44 | * @param component_id ID of this component (e.g. 200 for IMU) 45 | * @param msg The MAVLink message to compress the data into 46 | * 47 | * @param severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. 48 | * @param text Status text message, without null termination character 49 | * @return length of the message in bytes (excluding serial stream start sign) 50 | */ 51 | static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 52 | uint8_t severity, const char *text) 53 | { 54 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 55 | char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN]; 56 | _mav_put_uint8_t(buf, 0, severity); 57 | _mav_put_char_array(buf, 1, text, 50); 58 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STATUSTEXT_LEN); 59 | #else 60 | mavlink_statustext_t packet; 61 | packet.severity = severity; 62 | mav_array_memcpy(packet.text, text, sizeof(char)*50); 63 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATUSTEXT_LEN); 64 | #endif 65 | 66 | msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; 67 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); 68 | } 69 | 70 | /** 71 | * @brief Pack a statustext message on a channel 72 | * @param system_id ID of this system 73 | * @param component_id ID of this component (e.g. 200 for IMU) 74 | * @param chan The MAVLink channel this message will be sent over 75 | * @param msg The MAVLink message to compress the data into 76 | * @param severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. 77 | * @param text Status text message, without null termination character 78 | * @return length of the message in bytes (excluding serial stream start sign) 79 | */ 80 | static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 81 | mavlink_message_t* msg, 82 | uint8_t severity,const char *text) 83 | { 84 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 85 | char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN]; 86 | _mav_put_uint8_t(buf, 0, severity); 87 | _mav_put_char_array(buf, 1, text, 50); 88 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STATUSTEXT_LEN); 89 | #else 90 | mavlink_statustext_t packet; 91 | packet.severity = severity; 92 | mav_array_memcpy(packet.text, text, sizeof(char)*50); 93 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATUSTEXT_LEN); 94 | #endif 95 | 96 | msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; 97 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); 98 | } 99 | 100 | /** 101 | * @brief Encode a statustext struct 102 | * 103 | * @param system_id ID of this system 104 | * @param component_id ID of this component (e.g. 200 for IMU) 105 | * @param msg The MAVLink message to compress the data into 106 | * @param statustext C-struct to read the message contents from 107 | */ 108 | static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_statustext_t* statustext) 109 | { 110 | return mavlink_msg_statustext_pack(system_id, component_id, msg, statustext->severity, statustext->text); 111 | } 112 | 113 | /** 114 | * @brief Encode a statustext struct on a channel 115 | * 116 | * @param system_id ID of this system 117 | * @param component_id ID of this component (e.g. 200 for IMU) 118 | * @param chan The MAVLink channel this message will be sent over 119 | * @param msg The MAVLink message to compress the data into 120 | * @param statustext C-struct to read the message contents from 121 | */ 122 | static inline uint16_t mavlink_msg_statustext_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_statustext_t* statustext) 123 | { 124 | return mavlink_msg_statustext_pack_chan(system_id, component_id, chan, msg, statustext->severity, statustext->text); 125 | } 126 | 127 | /** 128 | * @brief Send a statustext message 129 | * @param chan MAVLink channel to send the message 130 | * 131 | * @param severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. 132 | * @param text Status text message, without null termination character 133 | */ 134 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 135 | 136 | static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char *text) 137 | { 138 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 139 | char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN]; 140 | _mav_put_uint8_t(buf, 0, severity); 141 | _mav_put_char_array(buf, 1, text, 50); 142 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); 143 | #else 144 | mavlink_statustext_t packet; 145 | packet.severity = severity; 146 | mav_array_memcpy(packet.text, text, sizeof(char)*50); 147 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)&packet, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); 148 | #endif 149 | } 150 | 151 | /** 152 | * @brief Send a statustext message 153 | * @param chan MAVLink channel to send the message 154 | * @param struct The MAVLink struct to serialize 155 | */ 156 | static inline void mavlink_msg_statustext_send_struct(mavlink_channel_t chan, const mavlink_statustext_t* statustext) 157 | { 158 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 159 | mavlink_msg_statustext_send(chan, statustext->severity, statustext->text); 160 | #else 161 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)statustext, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); 162 | #endif 163 | } 164 | 165 | #if MAVLINK_MSG_ID_STATUSTEXT_LEN <= MAVLINK_MAX_PAYLOAD_LEN 166 | /* 167 | This varient of _send() can be used to save stack space by re-using 168 | memory from the receive buffer. The caller provides a 169 | mavlink_message_t which is the size of a full mavlink message. This 170 | is usually the receive buffer for the channel, and allows a reply to an 171 | incoming message with minimum stack space usage. 172 | */ 173 | static inline void mavlink_msg_statustext_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t severity, const char *text) 174 | { 175 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 176 | char *buf = (char *)msgbuf; 177 | _mav_put_uint8_t(buf, 0, severity); 178 | _mav_put_char_array(buf, 1, text, 50); 179 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); 180 | #else 181 | mavlink_statustext_t *packet = (mavlink_statustext_t *)msgbuf; 182 | packet->severity = severity; 183 | mav_array_memcpy(packet->text, text, sizeof(char)*50); 184 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)packet, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); 185 | #endif 186 | } 187 | #endif 188 | 189 | #endif 190 | 191 | // MESSAGE STATUSTEXT UNPACKING 192 | 193 | 194 | /** 195 | * @brief Get field severity from statustext message 196 | * 197 | * @return Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. 198 | */ 199 | static inline uint8_t mavlink_msg_statustext_get_severity(const mavlink_message_t* msg) 200 | { 201 | return _MAV_RETURN_uint8_t(msg, 0); 202 | } 203 | 204 | /** 205 | * @brief Get field text from statustext message 206 | * 207 | * @return Status text message, without null termination character 208 | */ 209 | static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* msg, char *text) 210 | { 211 | return _MAV_RETURN_char_array(msg, text, 50, 1); 212 | } 213 | 214 | /** 215 | * @brief Decode a statustext message into a struct 216 | * 217 | * @param msg The message to decode 218 | * @param statustext C-struct to decode the message contents into 219 | */ 220 | static inline void mavlink_msg_statustext_decode(const mavlink_message_t* msg, mavlink_statustext_t* statustext) 221 | { 222 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 223 | statustext->severity = mavlink_msg_statustext_get_severity(msg); 224 | mavlink_msg_statustext_get_text(msg, statustext->text); 225 | #else 226 | uint8_t len = msg->len < MAVLINK_MSG_ID_STATUSTEXT_LEN? msg->len : MAVLINK_MSG_ID_STATUSTEXT_LEN; 227 | memset(statustext, 0, MAVLINK_MSG_ID_STATUSTEXT_LEN); 228 | memcpy(statustext, _MAV_PAYLOAD(msg), len); 229 | #endif 230 | } 231 | -------------------------------------------------------------------------------- /src/common/mavlink_msg_debug.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // MESSAGE DEBUG PACKING 3 | 4 | #define MAVLINK_MSG_ID_DEBUG 254 5 | 6 | MAVPACKED( 7 | typedef struct __mavlink_debug_t { 8 | uint32_t time_boot_ms; /*< Timestamp (milliseconds since system boot)*/ 9 | float value; /*< DEBUG value*/ 10 | uint8_t ind; /*< index of debug variable*/ 11 | }) mavlink_debug_t; 12 | 13 | #define MAVLINK_MSG_ID_DEBUG_LEN 9 14 | #define MAVLINK_MSG_ID_DEBUG_MIN_LEN 9 15 | #define MAVLINK_MSG_ID_254_LEN 9 16 | #define MAVLINK_MSG_ID_254_MIN_LEN 9 17 | 18 | #define MAVLINK_MSG_ID_DEBUG_CRC 46 19 | #define MAVLINK_MSG_ID_254_CRC 46 20 | 21 | 22 | 23 | #if MAVLINK_COMMAND_24BIT 24 | #define MAVLINK_MESSAGE_INFO_DEBUG { \ 25 | 254, \ 26 | "DEBUG", \ 27 | 3, \ 28 | { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_debug_t, time_boot_ms) }, \ 29 | { "ind", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_debug_t, ind) }, \ 30 | { "value", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_debug_t, value) }, \ 31 | } \ 32 | } 33 | #else 34 | #define MAVLINK_MESSAGE_INFO_DEBUG { \ 35 | "DEBUG", \ 36 | 3, \ 37 | { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_debug_t, time_boot_ms) }, \ 38 | { "ind", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_debug_t, ind) }, \ 39 | { "value", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_debug_t, value) }, \ 40 | } \ 41 | } 42 | #endif 43 | 44 | /** 45 | * @brief Pack a debug message 46 | * @param system_id ID of this system 47 | * @param component_id ID of this component (e.g. 200 for IMU) 48 | * @param msg The MAVLink message to compress the data into 49 | * 50 | * @param time_boot_ms Timestamp (milliseconds since system boot) 51 | * @param ind index of debug variable 52 | * @param value DEBUG value 53 | * @return length of the message in bytes (excluding serial stream start sign) 54 | */ 55 | static inline uint16_t mavlink_msg_debug_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 56 | uint32_t time_boot_ms, uint8_t ind, float value) 57 | { 58 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 59 | char buf[MAVLINK_MSG_ID_DEBUG_LEN]; 60 | _mav_put_uint32_t(buf, 0, time_boot_ms); 61 | _mav_put_float(buf, 4, value); 62 | _mav_put_uint8_t(buf, 8, ind); 63 | 64 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEBUG_LEN); 65 | #else 66 | mavlink_debug_t packet; 67 | packet.time_boot_ms = time_boot_ms; 68 | packet.value = value; 69 | packet.ind = ind; 70 | 71 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_LEN); 72 | #endif 73 | 74 | msg->msgid = MAVLINK_MSG_ID_DEBUG; 75 | return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_MIN_LEN, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); 76 | } 77 | 78 | /** 79 | * @brief Pack a debug message on a channel 80 | * @param system_id ID of this system 81 | * @param component_id ID of this component (e.g. 200 for IMU) 82 | * @param chan The MAVLink channel this message will be sent over 83 | * @param msg The MAVLink message to compress the data into 84 | * @param time_boot_ms Timestamp (milliseconds since system boot) 85 | * @param ind index of debug variable 86 | * @param value DEBUG value 87 | * @return length of the message in bytes (excluding serial stream start sign) 88 | */ 89 | static inline uint16_t mavlink_msg_debug_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 90 | mavlink_message_t* msg, 91 | uint32_t time_boot_ms,uint8_t ind,float value) 92 | { 93 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 94 | char buf[MAVLINK_MSG_ID_DEBUG_LEN]; 95 | _mav_put_uint32_t(buf, 0, time_boot_ms); 96 | _mav_put_float(buf, 4, value); 97 | _mav_put_uint8_t(buf, 8, ind); 98 | 99 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEBUG_LEN); 100 | #else 101 | mavlink_debug_t packet; 102 | packet.time_boot_ms = time_boot_ms; 103 | packet.value = value; 104 | packet.ind = ind; 105 | 106 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_LEN); 107 | #endif 108 | 109 | msg->msgid = MAVLINK_MSG_ID_DEBUG; 110 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_MIN_LEN, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); 111 | } 112 | 113 | /** 114 | * @brief Encode a debug struct 115 | * 116 | * @param system_id ID of this system 117 | * @param component_id ID of this component (e.g. 200 for IMU) 118 | * @param msg The MAVLink message to compress the data into 119 | * @param debug C-struct to read the message contents from 120 | */ 121 | static inline uint16_t mavlink_msg_debug_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_debug_t* debug) 122 | { 123 | return mavlink_msg_debug_pack(system_id, component_id, msg, debug->time_boot_ms, debug->ind, debug->value); 124 | } 125 | 126 | /** 127 | * @brief Encode a debug struct on a channel 128 | * 129 | * @param system_id ID of this system 130 | * @param component_id ID of this component (e.g. 200 for IMU) 131 | * @param chan The MAVLink channel this message will be sent over 132 | * @param msg The MAVLink message to compress the data into 133 | * @param debug C-struct to read the message contents from 134 | */ 135 | static inline uint16_t mavlink_msg_debug_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_debug_t* debug) 136 | { 137 | return mavlink_msg_debug_pack_chan(system_id, component_id, chan, msg, debug->time_boot_ms, debug->ind, debug->value); 138 | } 139 | 140 | /** 141 | * @brief Send a debug message 142 | * @param chan MAVLink channel to send the message 143 | * 144 | * @param time_boot_ms Timestamp (milliseconds since system boot) 145 | * @param ind index of debug variable 146 | * @param value DEBUG value 147 | */ 148 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 149 | 150 | static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t ind, float value) 151 | { 152 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 153 | char buf[MAVLINK_MSG_ID_DEBUG_LEN]; 154 | _mav_put_uint32_t(buf, 0, time_boot_ms); 155 | _mav_put_float(buf, 4, value); 156 | _mav_put_uint8_t(buf, 8, ind); 157 | 158 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, buf, MAVLINK_MSG_ID_DEBUG_MIN_LEN, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); 159 | #else 160 | mavlink_debug_t packet; 161 | packet.time_boot_ms = time_boot_ms; 162 | packet.value = value; 163 | packet.ind = ind; 164 | 165 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, (const char *)&packet, MAVLINK_MSG_ID_DEBUG_MIN_LEN, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); 166 | #endif 167 | } 168 | 169 | /** 170 | * @brief Send a debug message 171 | * @param chan MAVLink channel to send the message 172 | * @param struct The MAVLink struct to serialize 173 | */ 174 | static inline void mavlink_msg_debug_send_struct(mavlink_channel_t chan, const mavlink_debug_t* debug) 175 | { 176 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 177 | mavlink_msg_debug_send(chan, debug->time_boot_ms, debug->ind, debug->value); 178 | #else 179 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, (const char *)debug, MAVLINK_MSG_ID_DEBUG_MIN_LEN, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); 180 | #endif 181 | } 182 | 183 | #if MAVLINK_MSG_ID_DEBUG_LEN <= MAVLINK_MAX_PAYLOAD_LEN 184 | /* 185 | This varient of _send() can be used to save stack space by re-using 186 | memory from the receive buffer. The caller provides a 187 | mavlink_message_t which is the size of a full mavlink message. This 188 | is usually the receive buffer for the channel, and allows a reply to an 189 | incoming message with minimum stack space usage. 190 | */ 191 | static inline void mavlink_msg_debug_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t ind, float value) 192 | { 193 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 194 | char *buf = (char *)msgbuf; 195 | _mav_put_uint32_t(buf, 0, time_boot_ms); 196 | _mav_put_float(buf, 4, value); 197 | _mav_put_uint8_t(buf, 8, ind); 198 | 199 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, buf, MAVLINK_MSG_ID_DEBUG_MIN_LEN, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); 200 | #else 201 | mavlink_debug_t *packet = (mavlink_debug_t *)msgbuf; 202 | packet->time_boot_ms = time_boot_ms; 203 | packet->value = value; 204 | packet->ind = ind; 205 | 206 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, (const char *)packet, MAVLINK_MSG_ID_DEBUG_MIN_LEN, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); 207 | #endif 208 | } 209 | #endif 210 | 211 | #endif 212 | 213 | // MESSAGE DEBUG UNPACKING 214 | 215 | 216 | /** 217 | * @brief Get field time_boot_ms from debug message 218 | * 219 | * @return Timestamp (milliseconds since system boot) 220 | */ 221 | static inline uint32_t mavlink_msg_debug_get_time_boot_ms(const mavlink_message_t* msg) 222 | { 223 | return _MAV_RETURN_uint32_t(msg, 0); 224 | } 225 | 226 | /** 227 | * @brief Get field ind from debug message 228 | * 229 | * @return index of debug variable 230 | */ 231 | static inline uint8_t mavlink_msg_debug_get_ind(const mavlink_message_t* msg) 232 | { 233 | return _MAV_RETURN_uint8_t(msg, 8); 234 | } 235 | 236 | /** 237 | * @brief Get field value from debug message 238 | * 239 | * @return DEBUG value 240 | */ 241 | static inline float mavlink_msg_debug_get_value(const mavlink_message_t* msg) 242 | { 243 | return _MAV_RETURN_float(msg, 4); 244 | } 245 | 246 | /** 247 | * @brief Decode a debug message into a struct 248 | * 249 | * @param msg The message to decode 250 | * @param debug C-struct to decode the message contents into 251 | */ 252 | static inline void mavlink_msg_debug_decode(const mavlink_message_t* msg, mavlink_debug_t* debug) 253 | { 254 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 255 | debug->time_boot_ms = mavlink_msg_debug_get_time_boot_ms(msg); 256 | debug->value = mavlink_msg_debug_get_value(msg); 257 | debug->ind = mavlink_msg_debug_get_ind(msg); 258 | #else 259 | uint8_t len = msg->len < MAVLINK_MSG_ID_DEBUG_LEN? msg->len : MAVLINK_MSG_ID_DEBUG_LEN; 260 | memset(debug, 0, MAVLINK_MSG_ID_DEBUG_LEN); 261 | memcpy(debug, _MAV_PAYLOAD(msg), len); 262 | #endif 263 | } 264 | --------------------------------------------------------------------------------