├── ArduinoMAVLink └── ArduinoMAVLink.ino ├── README.md └── libraries ├── FastSerial ├── BetterStream.cpp ├── BetterStream.h ├── FastSerial.cpp ├── FastSerial.h ├── WProgram.h ├── examples │ └── FastSerial │ │ ├── .svn │ │ ├── all-wcprops │ │ ├── entries │ │ ├── prop-base │ │ │ └── FastSerial.pde.svn-base │ │ └── text-base │ │ │ └── FastSerial.pde.svn-base │ │ └── FastSerial.pde ├── ftoa_engine.S ├── ftoa_engine.h ├── keywords.txt ├── macros.inc ├── ntz.h ├── ultoa_invert.S ├── vprintf.cpp ├── wiring.h └── xtoa_fast.h └── mavlink ├── .DS_Store ├── README ├── include ├── .DS_Store ├── checksum.h ├── common │ ├── common.h │ ├── mavlink.h │ ├── mavlink_msg_attitude.h │ ├── mavlink_msg_attitude_quaternion.h │ ├── mavlink_msg_auth_key.h │ ├── mavlink_msg_change_operator_control.h │ ├── mavlink_msg_change_operator_control_ack.h │ ├── mavlink_msg_command_ack.h │ ├── mavlink_msg_command_long.h │ ├── mavlink_msg_data_stream.h │ ├── mavlink_msg_debug.h │ ├── mavlink_msg_debug_vect.h │ ├── mavlink_msg_global_position_int.h │ ├── mavlink_msg_global_position_setpoint_int.h │ ├── mavlink_msg_global_vision_position_estimate.h │ ├── mavlink_msg_gps_global_origin.h │ ├── mavlink_msg_gps_raw_int.h │ ├── mavlink_msg_gps_status.h │ ├── mavlink_msg_heartbeat.h │ ├── mavlink_msg_hil_controls.h │ ├── mavlink_msg_hil_rc_inputs_raw.h │ ├── mavlink_msg_hil_state.h │ ├── mavlink_msg_local_position_ned.h │ ├── mavlink_msg_local_position_ned_system_global_offset.h │ ├── mavlink_msg_local_position_setpoint.h │ ├── mavlink_msg_manual_control.h │ ├── mavlink_msg_memory_vect.h │ ├── mavlink_msg_mission_ack.h │ ├── mavlink_msg_mission_clear_all.h │ ├── mavlink_msg_mission_count.h │ ├── mavlink_msg_mission_current.h │ ├── mavlink_msg_mission_item.h │ ├── mavlink_msg_mission_item_reached.h │ ├── mavlink_msg_mission_request.h │ ├── mavlink_msg_mission_request_list.h │ ├── mavlink_msg_mission_request_partial_list.h │ ├── mavlink_msg_mission_set_current.h │ ├── mavlink_msg_mission_write_partial_list.h │ ├── mavlink_msg_named_value_float.h │ ├── mavlink_msg_named_value_int.h │ ├── mavlink_msg_nav_controller_output.h │ ├── mavlink_msg_optical_flow.h │ ├── mavlink_msg_param_request_list.h │ ├── mavlink_msg_param_request_read.h │ ├── mavlink_msg_param_set.h │ ├── mavlink_msg_param_value.h │ ├── mavlink_msg_ping.h │ ├── mavlink_msg_raw_imu.h │ ├── mavlink_msg_raw_pressure.h │ ├── mavlink_msg_rc_channels_override.h │ ├── mavlink_msg_rc_channels_raw.h │ ├── mavlink_msg_rc_channels_scaled.h │ ├── mavlink_msg_request_data_stream.h │ ├── mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h │ ├── mavlink_msg_roll_pitch_yaw_thrust_setpoint.h │ ├── mavlink_msg_safety_allowed_area.h │ ├── mavlink_msg_safety_set_allowed_area.h │ ├── mavlink_msg_scaled_imu.h │ ├── mavlink_msg_scaled_pressure.h │ ├── mavlink_msg_servo_output_raw.h │ ├── mavlink_msg_set_global_position_setpoint_int.h │ ├── mavlink_msg_set_gps_global_origin.h │ ├── mavlink_msg_set_local_position_setpoint.h │ ├── mavlink_msg_set_mode.h │ ├── mavlink_msg_set_quad_motors_setpoint.h │ ├── mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h │ ├── mavlink_msg_set_roll_pitch_yaw_speed_thrust.h │ ├── mavlink_msg_set_roll_pitch_yaw_thrust.h │ ├── mavlink_msg_state_correction.h │ ├── mavlink_msg_statustext.h │ ├── mavlink_msg_sys_status.h │ ├── mavlink_msg_system_time.h │ ├── mavlink_msg_vfr_hud.h │ ├── mavlink_msg_vicon_position_estimate.h │ ├── mavlink_msg_vision_position_estimate.h │ ├── mavlink_msg_vision_speed_estimate.h │ ├── testsuite.h │ └── version.h ├── matrixpilot │ ├── matrixpilot.h │ ├── mavlink.h │ ├── testsuite.h │ └── version.h ├── mavlink.h ├── mavlink_helpers.h ├── mavlink_protobuf_manager.hpp ├── mavlink_types.h ├── minimal │ ├── mavlink.h │ ├── mavlink_msg_heartbeat.h │ ├── minimal.h │ ├── testsuite.h │ └── version.h ├── pixhawk │ ├── mavlink.h │ ├── mavlink_msg_attitude_control.h │ ├── mavlink_msg_brief_feature.h │ ├── mavlink_msg_data_transmission_handshake.h │ ├── mavlink_msg_encapsulated_data.h │ ├── mavlink_msg_image_available.h │ ├── mavlink_msg_image_trigger_control.h │ ├── mavlink_msg_image_triggered.h │ ├── mavlink_msg_marker.h │ ├── mavlink_msg_pattern_detected.h │ ├── mavlink_msg_point_of_interest.h │ ├── mavlink_msg_point_of_interest_connection.h │ ├── mavlink_msg_position_control_setpoint.h │ ├── mavlink_msg_raw_aux.h │ ├── mavlink_msg_set_cam_shutter.h │ ├── mavlink_msg_set_position_control_offset.h │ ├── mavlink_msg_watchdog_command.h │ ├── mavlink_msg_watchdog_heartbeat.h │ ├── mavlink_msg_watchdog_process_info.h │ ├── mavlink_msg_watchdog_process_status.h │ ├── pixhawk.h │ ├── pixhawk.pb.h │ ├── testsuite.h │ └── version.h ├── protocol.h ├── sensesoar │ ├── mavlink.h │ ├── mavlink_msg_cmd_airspeed_ack.h │ ├── mavlink_msg_cmd_airspeed_chng.h │ ├── mavlink_msg_filt_rot_vel.h │ ├── mavlink_msg_llc_out.h │ ├── mavlink_msg_obs_air_temp.h │ ├── mavlink_msg_obs_air_velocity.h │ ├── mavlink_msg_obs_attitude.h │ ├── mavlink_msg_obs_bias.h │ ├── mavlink_msg_obs_position.h │ ├── mavlink_msg_obs_qff.h │ ├── mavlink_msg_obs_velocity.h │ ├── mavlink_msg_obs_wind.h │ ├── mavlink_msg_pm_elec.h │ ├── mavlink_msg_sys_stat.h │ ├── sensesoar.h │ ├── testsuite.h │ └── version.h ├── slugs │ ├── mavlink.h │ ├── mavlink_msg_air_data.h │ ├── mavlink_msg_cpu_load.h │ ├── mavlink_msg_ctrl_srfc_pt.h │ ├── mavlink_msg_data_log.h │ ├── mavlink_msg_diagnostic.h │ ├── mavlink_msg_gps_date_time.h │ ├── mavlink_msg_mid_lvl_cmds.h │ ├── mavlink_msg_sensor_bias.h │ ├── mavlink_msg_slugs_action.h │ ├── mavlink_msg_slugs_navigation.h │ ├── slugs.h │ ├── testsuite.h │ └── version.h ├── test │ ├── mavlink.h │ ├── mavlink_msg_test_types.h │ ├── test.h │ ├── testsuite.h │ └── version.h └── ualberta │ ├── mavlink.h │ ├── mavlink_msg_nav_filter_bias.h │ ├── mavlink_msg_radio_calibration.h │ ├── mavlink_msg_ualberta_sys_status.h │ ├── testsuite.h │ ├── ualberta.h │ └── version.h └── message_definitions ├── ardupilotmega.xml ├── common.xml ├── common_slugs.xml ├── mavlink_standard_proposal.xml ├── pixhawk.xml ├── slugs.xml ├── slugs_ap.xml └── ualberta.xml /ArduinoMAVLink/ArduinoMAVLink.ino: -------------------------------------------------------------------------------- 1 | // Arduino MAVLink test code. 2 | 3 | #include 4 | #include "../mavlink/include/mavlink.h" // Mavlink interface 5 | 6 | 7 | 8 | 9 | FastSerialPort0(Serial); 10 | 11 | 12 | 13 | void setup() { 14 | Serial.begin(57600); 15 | 16 | } 17 | 18 | void loop() { 19 | /* The default UART header for your MCU */ 20 | int sysid = 20; ///< ID 20 for this airplane 21 | int compid = MAV_COMP_ID_IMU; ///< The component sending the message is the IMU, it could be also a Linux process 22 | int type = MAV_TYPE_QUADROTOR; ///< This system is an airplane / fixed wing 23 | 24 | // Define the system type, in this case an airplane 25 | uint8_t system_type = MAV_TYPE_FIXED_WING; 26 | uint8_t autopilot_type = MAV_AUTOPILOT_GENERIC; 27 | 28 | uint8_t system_mode = MAV_MODE_PREFLIGHT; ///< Booting up 29 | uint32_t custom_mode = 0; ///< Custom mode, can be defined by user/adopter 30 | uint8_t system_state = MAV_STATE_STANDBY; ///< System ready for flight 31 | // Initialize the required buffers 32 | mavlink_message_t msg; 33 | uint8_t buf[MAVLINK_MAX_PACKET_LEN]; 34 | 35 | // Pack the message 36 | mavlink_msg_heartbeat_pack(sysid,compid, &msg, type, autopilot_type, system_mode, custom_mode, system_state); 37 | 38 | // Copy the message to the send buffer 39 | uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); 40 | 41 | // Send the message with the standard UART send function 42 | // uart0_send might be named differently depending on 43 | // the individual microcontroller / library in use. 44 | delay(1000); 45 | Serial.write(buf, len); 46 | comm_receive(); 47 | } 48 | 49 | void comm_receive() { 50 | 51 | mavlink_message_t msg; 52 | mavlink_status_t status; 53 | 54 | // COMMUNICATION THROUGH EXTERNAL UART PORT (XBee serial) 55 | 56 | while(Serial.available() > 0 ) 57 | { 58 | uint8_t c = Serial.read(); 59 | // Try to get a new message 60 | if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) { 61 | // Handle message 62 | 63 | switch(msg.msgid) 64 | { 65 | case MAVLINK_MSG_ID_HEARTBEAT: 66 | { 67 | // E.g. read GCS heartbeat and go into 68 | // comm lost mode if timer times out 69 | } 70 | break; 71 | case MAVLINK_MSG_ID_COMMAND_LONG: 72 | // EXECUTE ACTION 73 | break; 74 | default: 75 | //Do nothing 76 | break; 77 | } 78 | } 79 | 80 | // And get the next one 81 | } 82 | } 83 | 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | arduino-mavlink 2 | =============== 3 | 4 | A simple mavlink example for arduino -------------------------------------------------------------------------------- /libraries/FastSerial/BetterStream.cpp: -------------------------------------------------------------------------------- 1 | // -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*- 2 | // 3 | // Copyright (c) 2010 Michael Smith. All rights reserved. 4 | // 5 | // This is free software; you can redistribute it and/or modify it under 6 | // the terms of the GNU Lesser General Public License as published by the 7 | // Free Software Foundation; either version 2.1 of the License, or (at 8 | // your option) any later version. 9 | // 10 | 11 | // 12 | // Enhancements to the Arduino Stream class. 13 | // 14 | 15 | #include "BetterStream.h" 16 | 17 | // Stream extensions//////////////////////////////////////////////////////////// 18 | 19 | void 20 | BetterStream::print_P(const prog_char *s) 21 | { 22 | char c; 23 | 24 | while ('\0' != (c = pgm_read_byte(s++))) 25 | write(c); 26 | } 27 | 28 | void 29 | BetterStream::println_P(const char *s) 30 | { 31 | print_P(s); 32 | println(); 33 | } 34 | 35 | void 36 | BetterStream::printf(const char *fmt, ...) 37 | { 38 | va_list ap; 39 | 40 | va_start(ap, fmt); 41 | _vprintf(0, fmt, ap); 42 | va_end(ap); 43 | } 44 | 45 | void 46 | BetterStream::printf_P(const char *fmt, ...) 47 | { 48 | va_list ap; 49 | 50 | va_start(ap, fmt); 51 | _vprintf(1, fmt, ap); 52 | va_end(ap); 53 | } 54 | 55 | -------------------------------------------------------------------------------- /libraries/FastSerial/BetterStream.h: -------------------------------------------------------------------------------- 1 | // -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*- 2 | // 3 | // Copyright (c) 2010 Michael Smith. All rights reserved. 4 | // 5 | // This is free software; you can redistribute it and/or modify it under 6 | // the terms of the GNU Lesser General Public License as published by the 7 | // Free Software Foundation; either version 2.1 of the License, or (at 8 | // your option) any later version. 9 | // 10 | 11 | #ifndef __BETTERSTREAM_H 12 | #define __BETTERSTREAM_H 13 | 14 | #include 15 | #include 16 | 17 | class BetterStream : public Stream { 18 | public: 19 | BetterStream(void) { 20 | } 21 | 22 | // Stream extensions 23 | void print_P(const char *); 24 | void println_P(const char *); 25 | void printf(const char *, ...) 26 | __attribute__ ((format(__printf__, 2, 3))); 27 | void printf_P(const char *, ...) 28 | __attribute__ ((format(__printf__, 2, 3))); 29 | 30 | private: 31 | void _vprintf(unsigned char, const char *, va_list) 32 | __attribute__ ((format(__printf__, 3, 0))); 33 | }; 34 | 35 | #endif // __BETTERSTREAM_H 36 | 37 | -------------------------------------------------------------------------------- /libraries/FastSerial/WProgram.h: -------------------------------------------------------------------------------- 1 | #ifndef WProgram_h 2 | #define WProgram_h 3 | 4 | #include "wiring.h" 5 | #include 6 | #include "HardwareSerial.h" 7 | 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | typedef uint8_t byte; 19 | 20 | void delay(unsigned long msec); 21 | char *itoa(int __val, char *__s, int __radix); 22 | char *ltoa(long __val, char *__s, int __radix); 23 | char *ultoa(unsigned long __val, char *__s, int __radix); 24 | 25 | #ifdef __cplusplus 26 | } 27 | #endif 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /libraries/FastSerial/examples/FastSerial/.svn/all-wcprops: -------------------------------------------------------------------------------- 1 | K 25 2 | svn:wc:ra_dav:version-url 3 | V 64 4 | /svn/!svn/ver/919/trunk/libraries/FastSerial/examples/FastSerial 5 | END 6 | FastSerial.pde 7 | K 25 8 | svn:wc:ra_dav:version-url 9 | V 79 10 | /svn/!svn/ver/919/trunk/libraries/FastSerial/examples/FastSerial/FastSerial.pde 11 | END 12 | -------------------------------------------------------------------------------- /libraries/FastSerial/examples/FastSerial/.svn/entries: -------------------------------------------------------------------------------- 1 | 10 2 | 3 | dir 4 | 1310 5 | http://arducopter.googlecode.com/svn/trunk/libraries/FastSerial/examples/FastSerial 6 | http://arducopter.googlecode.com/svn 7 | 8 | 9 | 10 | 2010-11-25T02:59:15.997572Z 11 | 919 12 | DrZiplok@gmail.com 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | f9c3cf11-9bcb-44bc-f272-b75c42450872 28 | 29 | FastSerial.pde 30 | file 31 | 32 | 33 | 34 | 35 | 2010-11-28T15:20:25.796875Z 36 | 1e0a863e47af77cc12e6a65775b790cd 37 | 2010-11-25T02:59:15.997572Z 38 | 919 39 | DrZiplok@gmail.com 40 | has-props 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 1660 62 | 63 | -------------------------------------------------------------------------------- /libraries/FastSerial/examples/FastSerial/.svn/prop-base/FastSerial.pde.svn-base: -------------------------------------------------------------------------------- 1 | K 13 2 | svn:eol-style 3 | V 6 4 | native 5 | END 6 | -------------------------------------------------------------------------------- /libraries/FastSerial/examples/FastSerial/.svn/text-base/FastSerial.pde.svn-base: -------------------------------------------------------------------------------- 1 | // -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*- 2 | 3 | // 4 | // Example code for the FastSerial driver. 5 | // 6 | // This code is placed into the public domain. 7 | // 8 | 9 | // 10 | // Include the FastSerial library header. 11 | // 12 | // Note that this causes the standard Arduino Serial* driver to be 13 | // disabled. 14 | // 15 | #include 16 | 17 | #undef PROGMEM 18 | #define PROGMEM __attribute__(( section(".progmem.data") )) 19 | 20 | #undef PSTR 21 | #define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); &__c[0];})) 22 | 23 | // 24 | // Create a FastSerial driver that looks just like the stock Arduino 25 | // driver. 26 | // 27 | FastSerialPort0(Serial); 28 | 29 | // 30 | // To create a driver for a different serial port, on a board that 31 | // supports more than one, use the appropriate macro: 32 | // 33 | //FastSerialPort2(Serial2); 34 | 35 | 36 | void setup(void) 37 | { 38 | // 39 | // Set the speed for our replacement serial port. 40 | // 41 | Serial.begin(38400); 42 | 43 | // 44 | // Test printing things 45 | // 46 | Serial.print("test"); 47 | Serial.println(" begin"); 48 | Serial.println(1000); 49 | Serial.println(1000, 8); 50 | Serial.println(1000, 10); 51 | Serial.println(1000, 16); 52 | Serial.println_P(PSTR("progmem")); 53 | Serial.printf("printf %d %u %#x %p %f %S\n", -1000, 1000, 1000, 1000, 1.2345, PSTR("progmem")); 54 | Serial.printf_P(PSTR("printf_P %d %u %#x %p %f %S\n"), -1000, 1000, 1000, 1000, 1.2345, PSTR("progmem")); 55 | Serial.println("done"); 56 | } 57 | 58 | void 59 | loop(void) 60 | { 61 | int c; 62 | 63 | // 64 | // Perform a simple loopback operation. 65 | // 66 | c = Serial.read(); 67 | if (-1 != c) 68 | Serial.write(c); 69 | } 70 | 71 | -------------------------------------------------------------------------------- /libraries/FastSerial/examples/FastSerial/FastSerial.pde: -------------------------------------------------------------------------------- 1 | // -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*- 2 | 3 | // 4 | // Example code for the FastSerial driver. 5 | // 6 | // This code is placed into the public domain. 7 | // 8 | 9 | // 10 | // Include the FastSerial library header. 11 | // 12 | // Note that this causes the standard Arduino Serial* driver to be 13 | // disabled. 14 | // 15 | #include 16 | 17 | #undef PROGMEM 18 | #define PROGMEM __attribute__(( section(".progmem.data") )) 19 | 20 | #undef PSTR 21 | #define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); &__c[0];})) 22 | 23 | // 24 | // Create a FastSerial driver that looks just like the stock Arduino 25 | // driver. 26 | // 27 | FastSerialPort0(Serial); 28 | 29 | // 30 | // To create a driver for a different serial port, on a board that 31 | // supports more than one, use the appropriate macro: 32 | // 33 | //FastSerialPort2(Serial2); 34 | 35 | 36 | void setup(void) 37 | { 38 | // 39 | // Set the speed for our replacement serial port. 40 | // 41 | Serial.begin(38400); 42 | 43 | // 44 | // Test printing things 45 | // 46 | Serial.print("test"); 47 | Serial.println(" begin"); 48 | Serial.println(1000); 49 | Serial.println(1000, 8); 50 | Serial.println(1000, 10); 51 | Serial.println(1000, 16); 52 | Serial.println_P(PSTR("progmem")); 53 | Serial.printf("printf %d %u %#x %p %f %S\n", -1000, 1000, 1000, 1000, 1.2345, PSTR("progmem")); 54 | Serial.printf_P(PSTR("printf_P %d %u %#x %p %f %S\n"), -1000, 1000, 1000, 1000, 1.2345, PSTR("progmem")); 55 | Serial.println("done"); 56 | } 57 | 58 | void 59 | loop(void) 60 | { 61 | int c; 62 | 63 | // 64 | // Perform a simple loopback operation. 65 | // 66 | c = Serial.read(); 67 | if (-1 != c) 68 | Serial.write(c); 69 | } 70 | 71 | -------------------------------------------------------------------------------- /libraries/FastSerial/ftoa_engine.h: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2005, Dmitry Xmelkov 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in 11 | the documentation and/or other materials provided with the 12 | distribution. 13 | * Neither the name of the copyright holders nor the names of 14 | contributors may be used to endorse or promote products derived 15 | from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | POSSIBILITY OF SUCH DAMAGE. */ 28 | 29 | /* $Id: ftoa_engine.h 1218 2007-02-18 13:18:41Z dmix $ */ 30 | 31 | #ifndef _FTOA_ENGINE_H 32 | #define _FTOA_ENGINE_H 33 | 34 | #ifndef __ASSEMBLER__ 35 | 36 | int __ftoa_engine (double val, char *buf, 37 | unsigned char prec, unsigned char maxdgs); 38 | 39 | #endif 40 | 41 | /* '__ftoa_engine' return next flags (in buf[0]): */ 42 | #define FTOA_MINUS 1 43 | #define FTOA_ZERO 2 44 | #define FTOA_INF 4 45 | #define FTOA_NAN 8 46 | #define FTOA_CARRY 16 /* Carry was to master position. */ 47 | 48 | #endif /* !_FTOA_ENGINE_H */ 49 | -------------------------------------------------------------------------------- /libraries/FastSerial/keywords.txt: -------------------------------------------------------------------------------- 1 | FastSerial KEYWORD1 2 | begin KEYWORD2 3 | end KEYWORD2 4 | available KEYWORD2 5 | read KEYWORD2 6 | flush KEYWORD2 7 | write KEYWORD2 8 | -------------------------------------------------------------------------------- /libraries/FastSerial/ntz.h: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2007, Dmitry Xmelkov 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in 11 | the documentation and/or other materials provided with the 12 | distribution. 13 | * Neither the name of the copyright holders nor the names of 14 | contributors may be used to endorse or promote products derived 15 | from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | POSSIBILITY OF SUCH DAMAGE. */ 28 | 29 | /* $Id: ntz.h 1217 2007-02-18 13:18:05Z dmix $ */ 30 | 31 | #ifndef _NTZ_H 32 | #define _NTZ_H 33 | 34 | /* Number of Tail Zeros: ntz(x)= (ffs(x) ? ffs(x)-1 : 16) 35 | It works with all: cpp, gcc and gas expressions. */ 36 | #define ntz(x) \ 37 | ( (1 & (((x) & 1) == 0)) \ 38 | + (1 & (((x) & 3) == 0)) \ 39 | + (1 & (((x) & 7) == 0)) \ 40 | + (1 & (((x) & 017) == 0)) \ 41 | + (1 & (((x) & 037) == 0)) \ 42 | + (1 & (((x) & 077) == 0)) \ 43 | + (1 & (((x) & 0177) == 0)) \ 44 | + (1 & (((x) & 0377) == 0)) \ 45 | + (1 & (((x) & 0777) == 0)) \ 46 | + (1 & (((x) & 01777) == 0)) \ 47 | + (1 & (((x) & 03777) == 0)) \ 48 | + (1 & (((x) & 07777) == 0)) \ 49 | + (1 & (((x) & 017777) == 0)) \ 50 | + (1 & (((x) & 037777) == 0)) \ 51 | + (1 & (((x) & 077777) == 0)) \ 52 | + (1 & (((x) & 0177777) == 0)) ) 53 | 54 | #endif /* !_NTZ_H */ 55 | -------------------------------------------------------------------------------- /libraries/FastSerial/ultoa_invert.S: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2005,2007 Dmitry Xmelkov 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in 11 | the documentation and/or other materials provided with the 12 | distribution. 13 | * Neither the name of the copyright holders nor the names of 14 | contributors may be used to endorse or promote products derived 15 | from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | POSSIBILITY OF SUCH DAMAGE. */ 28 | 29 | /* $Id: ultoa_invert.S 1944 2009-04-01 23:12:20Z arcanum $ */ 30 | 31 | #ifndef __DOXYGEN__ 32 | 33 | #include "macros.inc" 34 | #include "ntz.h" 35 | #include "xtoa_fast.h" 36 | 37 | /* -------------------------------------------------------------------- 38 | char * __ultoa_invert (unsigned long val, char * str, int base) 39 | 40 | This function is intended for usage as internal printf's one. 41 | It differs from others of `xtoa_fast' family: 42 | * srt[] will NOT 0 terminated. 43 | * Sequence of digits is inverted. 44 | * It returns pointer to first byte after a string. 45 | * Only `XTOA_UPPER' flag is operated. 46 | Notes: 47 | * base: check only 8 and 16, all others are treated as 10. 48 | (internal printf's function). 49 | */ 50 | 51 | /* Input */ 52 | #define v_lo r22 53 | #define v_hi r23 54 | #define v_hlo r24 55 | #define v_hhi r25 56 | #define str_lo r20 57 | #define str_hi r21 58 | #define base r18 59 | #define flags r19 60 | 61 | /* Used */ 62 | #define v_fifth r26 /* val: bits 39..32 */ 63 | #define t_lo r18 /* temporary for shifted `val' */ 64 | #define t_hi r19 65 | #define t_hlo r20 66 | #define t_hhi r21 67 | #define symb r20 /* write to string */ 68 | #define cnt r27 /* shift loop counter, local arg */ 69 | 70 | /* Fixed */ 71 | #define rzero r1 72 | 73 | /* ASSEMBLY_CLIB_SECTION */ 74 | .global __ultoa_invert 75 | .type __ultoa_invert, "function" 76 | 77 | __ultoa_invert: 78 | X_movw ZL, str_lo 79 | clr v_fifth ; needed for all (ultoa_lsr) 80 | cpi base, 8 81 | breq .L_oct 82 | cpi base, 16 83 | breq .L_hex 84 | 85 | ; decimal format 86 | clt ; flag of val == 0 87 | .L_dec_loop: 88 | push v_lo ; to calculate remander 89 | ; val &= ~1 90 | andi v_lo, ~1 91 | ; val += 2 92 | subi v_lo, lo8(-2) 93 | sbci v_hi, hi8(-2) 94 | sbci v_hlo, hlo8(-2) 95 | sbci v_hhi, hhi8(-2) 96 | sbci v_fifth, hhi8(-2) 97 | ; val += val/2 98 | ldi cnt, 1 99 | rcall .L_div_add 100 | ; val += val/16 101 | ldi cnt, 4 102 | rcall .L_div_add 103 | ; val += val/256 104 | add v_lo, v_hi 105 | adc v_hi, v_hlo 106 | adc v_hlo, v_hhi 107 | adc v_hhi, v_fifth 108 | adc v_fifth, rzero 109 | ; val += val/65536 110 | add v_lo, v_hlo 111 | adc v_hi, v_hhi 112 | adc v_hlo, v_fifth 113 | adc v_hhi, rzero 114 | adc v_fifth, rzero 115 | ; val += val >> 32 116 | add v_lo, v_fifth 117 | adc v_hi, rzero 118 | adc v_hlo, rzero 119 | adc v_hhi, rzero 120 | adc v_fifth, rzero 121 | ; division result: val /= 16 122 | rcall .L_lsr_4 ; v_fitth := 0 123 | brne 1f 124 | set ; T := Z flag 125 | 1: 126 | ; rem: val_original - 10*val 127 | pop t_hi 128 | #if defined(__AVR_ENHANCED__) && __AVR_ENHANCED__ 129 | ldi t_lo, 10 130 | mul t_lo, v_lo 131 | clr r1 132 | #else 133 | mov r0, v_lo 134 | lsl r0 135 | sub t_hi, r0 136 | lsl r0 137 | lsl r0 138 | #endif 139 | sub t_hi, r0 140 | ; output digit 141 | subi t_hi, lo8(-'0') 142 | st Z+, t_hi 143 | ; quotient == 0 ? 144 | brtc .L_dec_loop 145 | ; end of string 146 | .L_eos: 147 | X_movw r24, ZL 148 | ret 149 | 150 | ; octal format 151 | .L_oct: 152 | mov symb, v_lo 153 | andi symb, 7 154 | subi symb, lo8(-'0') 155 | st Z+, symb 156 | ldi cnt, 3 157 | rcall .L_lsr 158 | brne .L_oct 159 | rjmp .L_eos 160 | 161 | ; hex format 162 | .L_hex: 163 | mov symb, v_lo 164 | andi symb, 0x0f 165 | subi symb, lo8(-'0') 166 | cpi symb, '9' + 1 167 | brlo 3f 168 | subi symb, lo8('9' + 1 - 'a') 169 | sbrc flags, ntz(XTOA_UPPER) - 8 170 | subi symb, lo8('a' - 'A') 171 | 3: st Z+, symb 172 | rcall .L_lsr_4 173 | brne .L_hex 174 | rjmp .L_eos 175 | 176 | .L_lsr_4: 177 | ldi cnt, 4 178 | .L_lsr: 179 | lsr v_fifth 180 | ror v_hhi 181 | ror v_hlo 182 | ror v_hi 183 | ror v_lo 184 | dec cnt 185 | brne .L_lsr 186 | ; tst 187 | sbiw v_hlo, 0 ; only Z flag is needed 188 | cpc v_lo, rzero 189 | cpc v_hi, rzero 190 | ret 191 | 192 | .L_div_add: 193 | ; copy to temporary 194 | X_movw t_lo, v_lo 195 | X_movw t_hlo, v_hlo 196 | mov r0, v_fifth 197 | ; lsr temporary 198 | 7: lsr r0 199 | ror t_hhi 200 | ror t_hlo 201 | ror t_hi 202 | ror t_lo 203 | dec cnt 204 | brne 7b 205 | ; add 206 | add v_lo, t_lo 207 | adc v_hi, t_hi 208 | adc v_hlo, t_hlo 209 | adc v_hhi, t_hhi 210 | adc v_fifth, r0 ; here r0 == 0 211 | ret 212 | 213 | .size __ultoa_invert, . - __ultoa_invert 214 | .end 215 | 216 | #endif /* !__DOXYGEN__ */ 217 | -------------------------------------------------------------------------------- /libraries/FastSerial/wiring.h: -------------------------------------------------------------------------------- 1 | /* 2 | wiring.h - Partial implementation of the Wiring API for the ATmega8. 3 | Part of Arduino - http://www.arduino.cc/ 4 | 5 | Copyright (c) 2005-2006 David A. Mellis 6 | 7 | This library is free software; you can redistribute it and/or 8 | modify it under the terms of the GNU Lesser General Public 9 | License as published by the Free Software Foundation; either 10 | version 2.1 of the License, or (at your option) any later version. 11 | 12 | This library is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | Lesser General Public License for more details. 16 | 17 | You should have received a copy of the GNU Lesser General 18 | Public License along with this library; if not, write to the 19 | Free Software Foundation, Inc., 59 Temple Place, Suite 330, 20 | Boston, MA 02111-1307 USA 21 | 22 | $Id$ 23 | */ 24 | 25 | #ifndef Wiring_h 26 | #define Wiring_h 27 | 28 | #include 29 | #include 30 | #include "binary.h" 31 | 32 | #ifdef __cplusplus 33 | extern "C"{ 34 | #endif 35 | 36 | #define HIGH 0x1 37 | #define LOW 0x0 38 | 39 | #define INPUT 0x0 40 | #define OUTPUT 0x1 41 | 42 | #define true 0x1 43 | #define false 0x0 44 | 45 | #define PI 3.1415926535897932384626433832795 46 | #define HALF_PI 1.5707963267948966192313216916398 47 | #define TWO_PI 6.283185307179586476925286766559 48 | #define DEG_TO_RAD 0.017453292519943295769236907684886 49 | #define RAD_TO_DEG 57.295779513082320876798154814105 50 | 51 | #define SERIAL 0x0 52 | #define DISPLAY 0x1 53 | 54 | #define LSBFIRST 0 55 | #define MSBFIRST 1 56 | 57 | #define CHANGE 1 58 | #define FALLING 2 59 | #define RISING 3 60 | 61 | #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) || defined(DESKTOP_BUILD) 62 | #define INTERNAL1V1 2 63 | #define INTERNAL2V56 3 64 | #else 65 | #define INTERNAL 3 66 | #endif 67 | #define DEFAULT 1 68 | #define EXTERNAL 0 69 | 70 | // undefine stdlib's abs if encountered 71 | #ifdef abs 72 | #undef abs 73 | #endif 74 | 75 | #define min(a,b) ((a)<(b)?(a):(b)) 76 | #define max(a,b) ((a)>(b)?(a):(b)) 77 | #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 78 | //#define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) 79 | #define radians(deg) ((deg)*DEG_TO_RAD) 80 | #define degrees(rad) ((rad)*RAD_TO_DEG) 81 | #define sq(x) ((x)*(x)) 82 | 83 | #define interrupts() sei() 84 | #define noInterrupts() cli() 85 | 86 | #define clockCyclesPerMicrosecond() ( F_CPU / 1000000L ) 87 | #define clockCyclesToMicroseconds(a) ( ((a) * 1000L) / (F_CPU / 1000L) ) 88 | #define microsecondsToClockCycles(a) ( ((a) * (F_CPU / 1000L)) / 1000L ) 89 | 90 | #define lowByte(w) ((uint8_t) ((w) & 0xff)) 91 | #define highByte(w) ((uint8_t) ((w) >> 8)) 92 | 93 | #define bitRead(value, bit) (((value) >> (bit)) & 0x01) 94 | #define bitSet(value, bit) ((value) |= (1UL << (bit))) 95 | #define bitClear(value, bit) ((value) &= ~(1UL << (bit))) 96 | #define bitWrite(value, bit, bitvalue) (bitvalue ? bitSet(value, bit) : bitClear(value, bit)) 97 | #define bit_is_set(value, bit) (((value) & bit) != 0) 98 | 99 | typedef unsigned int word; 100 | 101 | #define bit(b) (1UL << (b)) 102 | 103 | typedef uint8_t boolean; 104 | typedef uint8_t byte; 105 | 106 | void init(void); 107 | 108 | void pinMode(uint8_t, uint8_t); 109 | void digitalWrite(uint8_t, uint8_t); 110 | int digitalRead(uint8_t); 111 | void analogReference(uint8_t mode); 112 | void analogWrite(uint8_t, int); 113 | 114 | unsigned long millis(void); 115 | unsigned long micros(void); 116 | void delay(unsigned long); 117 | void delayMicroseconds(unsigned int us); 118 | unsigned long pulseIn(uint8_t pin, uint8_t state, unsigned long timeout); 119 | 120 | void shiftOut(uint8_t dataPin, uint8_t clockPin, uint8_t bitOrder, uint8_t val); 121 | uint8_t shiftIn(uint8_t dataPin, uint8_t clockPin, uint8_t bitOrder); 122 | 123 | void attachInterrupt(uint8_t, void (*)(void), int mode); 124 | void detachInterrupt(uint8_t); 125 | 126 | int abs(int v); 127 | 128 | #ifdef __cplusplus 129 | } // extern "C" 130 | #endif 131 | 132 | void setup(void); 133 | void loop(void); 134 | long map(long , long , long , long , long ); 135 | 136 | #endif 137 | -------------------------------------------------------------------------------- /libraries/FastSerial/xtoa_fast.h: -------------------------------------------------------------------------------- 1 | /* 2 | Adapted from avr-libc: 3 | 4 | Copyright (c) 2005, Dmitry Xmelkov 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in 14 | the documentation and/or other materials provided with the 15 | distribution. 16 | * Neither the name of the copyright holders nor the names of 17 | contributors may be used to endorse or promote products derived 18 | from this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 24 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. */ 31 | 32 | /* $Id: xtoa_fast.h 1223 2007-02-18 13:33:09Z dmix $ */ 33 | 34 | #ifndef _XTOA_FAST_H_ 35 | #define _XTOA_FAST_H_ 36 | 37 | #ifndef __ASSEMBLER__ 38 | 39 | /* Internal function for use from `printf'. */ 40 | char * __ultoa_invert (unsigned long val, char *s, int base); 41 | 42 | #endif /* ifndef __ASSEMBLER__ */ 43 | 44 | /* Next flags are to use with `base'. Unused fields are reserved. */ 45 | #define XTOA_PREFIX 0x0100 /* put prefix for octal or hex */ 46 | #define XTOA_UPPER 0x0200 /* use upper case letters */ 47 | 48 | #endif /* _XTOA_FAST_H_ */ 49 | -------------------------------------------------------------------------------- /libraries/mavlink/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alaney/arduino-mavlink/4d71dc5eecd4dda262237081a07c07963e8136ef/libraries/mavlink/.DS_Store -------------------------------------------------------------------------------- /libraries/mavlink/README: -------------------------------------------------------------------------------- 1 | MAVLink Micro Air Vehicle Message Marshalling Library 2 | 3 | This is a library for lightweight communication between 4 | Micro Air Vehicles (swarm) and/or ground control stations. 5 | 6 | It serializes C-structs for serial channels and can be used with 7 | any type of radio modem. 8 | 9 | To generate/update packets, select mavlink_standard_message.xml 10 | in the QGroundControl station settings view, select mavlink/include as 11 | the output directory and click on "Save and Generate". 12 | You will find the newly generated/updated message_xx.h files in 13 | the mavlink/include/generated folder. 14 | 15 | To use MAVLink, #include the file, not the individual 16 | message files. In some cases you will have to add the main folder to the include search path as well. To be safe, we recommend these flags: 17 | 18 | gcc -I mavlink/include -I mavlink/include/ 19 | 20 | For more information, please visit: 21 | 22 | http://pixhawk.ethz.ch/wiki/software/mavlink/ 23 | 24 | (c) 2009-2011 Lorenz Meier / PIXHAWK Team 25 | 26 | -------------------------------------------------------------------------------- /libraries/mavlink/include/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alaney/arduino-mavlink/4d71dc5eecd4dda262237081a07c07963e8136ef/libraries/mavlink/include/.DS_Store -------------------------------------------------------------------------------- /libraries/mavlink/include/checksum.h: -------------------------------------------------------------------------------- 1 | #ifdef __cplusplus 2 | extern "C" { 3 | #endif 4 | 5 | #ifndef _CHECKSUM_H_ 6 | #define _CHECKSUM_H_ 7 | 8 | 9 | /** 10 | * 11 | * CALCULATE THE CHECKSUM 12 | * 13 | */ 14 | 15 | #define X25_INIT_CRC 0xffff 16 | #define X25_VALIDATE_CRC 0xf0b8 17 | 18 | /** 19 | * @brief Accumulate the X.25 CRC by adding one char at a time. 20 | * 21 | * The checksum function adds the hash of one char at a time to the 22 | * 16 bit checksum (uint16_t). 23 | * 24 | * @param data new char to hash 25 | * @param crcAccum the already accumulated checksum 26 | **/ 27 | #ifndef HAVE_CRC_ACCUMULATE 28 | static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) 29 | { 30 | /*Accumulate one byte of data into the CRC*/ 31 | uint8_t tmp; 32 | 33 | tmp = data ^ (uint8_t)(*crcAccum &0xff); 34 | tmp ^= (tmp<<4); 35 | *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); 36 | } 37 | #endif 38 | 39 | /** 40 | * @brief Initiliaze the buffer for the X.25 CRC 41 | * 42 | * @param crcAccum the 16 bit X.25 CRC 43 | */ 44 | static inline void crc_init(uint16_t* crcAccum) 45 | { 46 | *crcAccum = X25_INIT_CRC; 47 | } 48 | 49 | 50 | /** 51 | * @brief Calculates the X.25 checksum on a byte buffer 52 | * 53 | * @param pBuffer buffer containing the byte array to hash 54 | * @param length length of the byte array 55 | * @return the checksum over the buffer bytes 56 | **/ 57 | static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length) 58 | { 59 | uint16_t crcTmp; 60 | crc_init(&crcTmp); 61 | while (length--) { 62 | crc_accumulate(*pBuffer++, &crcTmp); 63 | } 64 | return crcTmp; 65 | } 66 | 67 | /** 68 | * @brief Accumulate the X.25 CRC by adding an array of bytes 69 | * 70 | * The checksum function adds the hash of one char at a time to the 71 | * 16 bit checksum (uint16_t). 72 | * 73 | * @param data new bytes to hash 74 | * @param crcAccum the already accumulated checksum 75 | **/ 76 | static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint8_t length) 77 | { 78 | const uint8_t *p = (const uint8_t *)pBuffer; 79 | while (length--) { 80 | crc_accumulate(*p++, crcAccum); 81 | } 82 | } 83 | 84 | 85 | 86 | 87 | #endif /* _CHECKSUM_H_ */ 88 | 89 | #ifdef __cplusplus 90 | } 91 | #endif 92 | -------------------------------------------------------------------------------- /libraries/mavlink/include/common/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from common.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_H 6 | #define MAVLINK_H 7 | 8 | #ifndef MAVLINK_STX 9 | #define MAVLINK_STX 254 10 | #endif 11 | 12 | #ifndef MAVLINK_ENDIAN 13 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 14 | #endif 15 | 16 | #ifndef MAVLINK_ALIGNED_FIELDS 17 | #define MAVLINK_ALIGNED_FIELDS 1 18 | #endif 19 | 20 | #ifndef MAVLINK_CRC_EXTRA 21 | #define MAVLINK_CRC_EXTRA 1 22 | #endif 23 | 24 | #include "version.h" 25 | #include "common.h" 26 | 27 | #endif // MAVLINK_H 28 | -------------------------------------------------------------------------------- /libraries/mavlink/include/common/mavlink_msg_auth_key.h: -------------------------------------------------------------------------------- 1 | // MESSAGE AUTH_KEY PACKING 2 | 3 | #define MAVLINK_MSG_ID_AUTH_KEY 7 4 | 5 | typedef struct __mavlink_auth_key_t 6 | { 7 | char key[32]; ///< key 8 | } mavlink_auth_key_t; 9 | 10 | #define MAVLINK_MSG_ID_AUTH_KEY_LEN 32 11 | #define MAVLINK_MSG_ID_7_LEN 32 12 | 13 | #define MAVLINK_MSG_AUTH_KEY_FIELD_KEY_LEN 32 14 | 15 | #define MAVLINK_MESSAGE_INFO_AUTH_KEY { \ 16 | "AUTH_KEY", \ 17 | 1, \ 18 | { { "key", NULL, MAVLINK_TYPE_CHAR, 32, 0, offsetof(mavlink_auth_key_t, key) }, \ 19 | } \ 20 | } 21 | 22 | 23 | /** 24 | * @brief Pack a auth_key message 25 | * @param system_id ID of this system 26 | * @param component_id ID of this component (e.g. 200 for IMU) 27 | * @param msg The MAVLink message to compress the data into 28 | * 29 | * @param key key 30 | * @return length of the message in bytes (excluding serial stream start sign) 31 | */ 32 | static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 33 | const char *key) 34 | { 35 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 36 | char buf[32]; 37 | 38 | _mav_put_char_array(buf, 0, key, 32); 39 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32); 40 | #else 41 | mavlink_auth_key_t packet; 42 | 43 | mav_array_memcpy(packet.key, key, sizeof(char)*32); 44 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 32); 45 | #endif 46 | 47 | msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; 48 | return mavlink_finalize_message(msg, system_id, component_id, 32, 119); 49 | } 50 | 51 | /** 52 | * @brief Pack a auth_key message on a channel 53 | * @param system_id ID of this system 54 | * @param component_id ID of this component (e.g. 200 for IMU) 55 | * @param chan The MAVLink channel this message was sent over 56 | * @param msg The MAVLink message to compress the data into 57 | * @param key key 58 | * @return length of the message in bytes (excluding serial stream start sign) 59 | */ 60 | static inline uint16_t mavlink_msg_auth_key_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 61 | mavlink_message_t* msg, 62 | const char *key) 63 | { 64 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 65 | char buf[32]; 66 | 67 | _mav_put_char_array(buf, 0, key, 32); 68 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32); 69 | #else 70 | mavlink_auth_key_t packet; 71 | 72 | mav_array_memcpy(packet.key, key, sizeof(char)*32); 73 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 32); 74 | #endif 75 | 76 | msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; 77 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 119); 78 | } 79 | 80 | /** 81 | * @brief Encode a auth_key struct into a message 82 | * 83 | * @param system_id ID of this system 84 | * @param component_id ID of this component (e.g. 200 for IMU) 85 | * @param msg The MAVLink message to compress the data into 86 | * @param auth_key C-struct to read the message contents from 87 | */ 88 | 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) 89 | { 90 | return mavlink_msg_auth_key_pack(system_id, component_id, msg, auth_key->key); 91 | } 92 | 93 | /** 94 | * @brief Send a auth_key message 95 | * @param chan MAVLink channel to send the message 96 | * 97 | * @param key key 98 | */ 99 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 100 | 101 | static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char *key) 102 | { 103 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 104 | char buf[32]; 105 | 106 | _mav_put_char_array(buf, 0, key, 32); 107 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, 32, 119); 108 | #else 109 | mavlink_auth_key_t packet; 110 | 111 | mav_array_memcpy(packet.key, key, sizeof(char)*32); 112 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)&packet, 32, 119); 113 | #endif 114 | } 115 | 116 | #endif 117 | 118 | // MESSAGE AUTH_KEY UNPACKING 119 | 120 | 121 | /** 122 | * @brief Get field key from auth_key message 123 | * 124 | * @return key 125 | */ 126 | static inline uint16_t mavlink_msg_auth_key_get_key(const mavlink_message_t* msg, char *key) 127 | { 128 | return _MAV_RETURN_char_array(msg, key, 32, 0); 129 | } 130 | 131 | /** 132 | * @brief Decode a auth_key message into a struct 133 | * 134 | * @param msg The message to decode 135 | * @param auth_key C-struct to decode the message contents into 136 | */ 137 | static inline void mavlink_msg_auth_key_decode(const mavlink_message_t* msg, mavlink_auth_key_t* auth_key) 138 | { 139 | #if MAVLINK_NEED_BYTE_SWAP 140 | mavlink_msg_auth_key_get_key(msg, auth_key->key); 141 | #else 142 | memcpy(auth_key, _MAV_PAYLOAD(msg), 32); 143 | #endif 144 | } 145 | -------------------------------------------------------------------------------- /libraries/mavlink/include/common/mavlink_msg_command_ack.h: -------------------------------------------------------------------------------- 1 | // MESSAGE COMMAND_ACK PACKING 2 | 3 | #define MAVLINK_MSG_ID_COMMAND_ACK 77 4 | 5 | typedef struct __mavlink_command_ack_t 6 | { 7 | uint16_t command; ///< Command ID, as defined by MAV_CMD enum. 8 | uint8_t result; ///< See MAV_RESULT enum 9 | } mavlink_command_ack_t; 10 | 11 | #define MAVLINK_MSG_ID_COMMAND_ACK_LEN 3 12 | #define MAVLINK_MSG_ID_77_LEN 3 13 | 14 | 15 | 16 | #define MAVLINK_MESSAGE_INFO_COMMAND_ACK { \ 17 | "COMMAND_ACK", \ 18 | 2, \ 19 | { { "command", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_command_ack_t, command) }, \ 20 | { "result", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_command_ack_t, result) }, \ 21 | } \ 22 | } 23 | 24 | 25 | /** 26 | * @brief Pack a command_ack message 27 | * @param system_id ID of this system 28 | * @param component_id ID of this component (e.g. 200 for IMU) 29 | * @param msg The MAVLink message to compress the data into 30 | * 31 | * @param command Command ID, as defined by MAV_CMD enum. 32 | * @param result See MAV_RESULT enum 33 | * @return length of the message in bytes (excluding serial stream start sign) 34 | */ 35 | static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 36 | uint16_t command, uint8_t result) 37 | { 38 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 39 | char buf[3]; 40 | _mav_put_uint16_t(buf, 0, command); 41 | _mav_put_uint8_t(buf, 2, result); 42 | 43 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 3); 44 | #else 45 | mavlink_command_ack_t packet; 46 | packet.command = command; 47 | packet.result = result; 48 | 49 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 3); 50 | #endif 51 | 52 | msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; 53 | return mavlink_finalize_message(msg, system_id, component_id, 3, 143); 54 | } 55 | 56 | /** 57 | * @brief Pack a command_ack message on a channel 58 | * @param system_id ID of this system 59 | * @param component_id ID of this component (e.g. 200 for IMU) 60 | * @param chan The MAVLink channel this message was sent over 61 | * @param msg The MAVLink message to compress the data into 62 | * @param command Command ID, as defined by MAV_CMD enum. 63 | * @param result See MAV_RESULT enum 64 | * @return length of the message in bytes (excluding serial stream start sign) 65 | */ 66 | static inline uint16_t mavlink_msg_command_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 67 | mavlink_message_t* msg, 68 | uint16_t command,uint8_t result) 69 | { 70 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 71 | char buf[3]; 72 | _mav_put_uint16_t(buf, 0, command); 73 | _mav_put_uint8_t(buf, 2, result); 74 | 75 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 3); 76 | #else 77 | mavlink_command_ack_t packet; 78 | packet.command = command; 79 | packet.result = result; 80 | 81 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 3); 82 | #endif 83 | 84 | msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; 85 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 143); 86 | } 87 | 88 | /** 89 | * @brief Encode a command_ack struct into a message 90 | * 91 | * @param system_id ID of this system 92 | * @param component_id ID of this component (e.g. 200 for IMU) 93 | * @param msg The MAVLink message to compress the data into 94 | * @param command_ack C-struct to read the message contents from 95 | */ 96 | static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_command_ack_t* command_ack) 97 | { 98 | return mavlink_msg_command_ack_pack(system_id, component_id, msg, command_ack->command, command_ack->result); 99 | } 100 | 101 | /** 102 | * @brief Send a command_ack message 103 | * @param chan MAVLink channel to send the message 104 | * 105 | * @param command Command ID, as defined by MAV_CMD enum. 106 | * @param result See MAV_RESULT enum 107 | */ 108 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 109 | 110 | static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, uint16_t command, uint8_t result) 111 | { 112 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 113 | char buf[3]; 114 | _mav_put_uint16_t(buf, 0, command); 115 | _mav_put_uint8_t(buf, 2, result); 116 | 117 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, buf, 3, 143); 118 | #else 119 | mavlink_command_ack_t packet; 120 | packet.command = command; 121 | packet.result = result; 122 | 123 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)&packet, 3, 143); 124 | #endif 125 | } 126 | 127 | #endif 128 | 129 | // MESSAGE COMMAND_ACK UNPACKING 130 | 131 | 132 | /** 133 | * @brief Get field command from command_ack message 134 | * 135 | * @return Command ID, as defined by MAV_CMD enum. 136 | */ 137 | static inline uint16_t mavlink_msg_command_ack_get_command(const mavlink_message_t* msg) 138 | { 139 | return _MAV_RETURN_uint16_t(msg, 0); 140 | } 141 | 142 | /** 143 | * @brief Get field result from command_ack message 144 | * 145 | * @return See MAV_RESULT enum 146 | */ 147 | static inline uint8_t mavlink_msg_command_ack_get_result(const mavlink_message_t* msg) 148 | { 149 | return _MAV_RETURN_uint8_t(msg, 2); 150 | } 151 | 152 | /** 153 | * @brief Decode a command_ack message into a struct 154 | * 155 | * @param msg The message to decode 156 | * @param command_ack C-struct to decode the message contents into 157 | */ 158 | static inline void mavlink_msg_command_ack_decode(const mavlink_message_t* msg, mavlink_command_ack_t* command_ack) 159 | { 160 | #if MAVLINK_NEED_BYTE_SWAP 161 | command_ack->command = mavlink_msg_command_ack_get_command(msg); 162 | command_ack->result = mavlink_msg_command_ack_get_result(msg); 163 | #else 164 | memcpy(command_ack, _MAV_PAYLOAD(msg), 3); 165 | #endif 166 | } 167 | -------------------------------------------------------------------------------- /libraries/mavlink/include/common/mavlink_msg_debug.h: -------------------------------------------------------------------------------- 1 | // MESSAGE DEBUG PACKING 2 | 3 | #define MAVLINK_MSG_ID_DEBUG 254 4 | 5 | typedef struct __mavlink_debug_t 6 | { 7 | uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) 8 | float value; ///< DEBUG value 9 | uint8_t ind; ///< index of debug variable 10 | } mavlink_debug_t; 11 | 12 | #define MAVLINK_MSG_ID_DEBUG_LEN 9 13 | #define MAVLINK_MSG_ID_254_LEN 9 14 | 15 | 16 | 17 | #define MAVLINK_MESSAGE_INFO_DEBUG { \ 18 | "DEBUG", \ 19 | 3, \ 20 | { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_debug_t, time_boot_ms) }, \ 21 | { "value", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_debug_t, value) }, \ 22 | { "ind", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_debug_t, ind) }, \ 23 | } \ 24 | } 25 | 26 | 27 | /** 28 | * @brief Pack a debug message 29 | * @param system_id ID of this system 30 | * @param component_id ID of this component (e.g. 200 for IMU) 31 | * @param msg The MAVLink message to compress the data into 32 | * 33 | * @param time_boot_ms Timestamp (milliseconds since system boot) 34 | * @param ind index of debug variable 35 | * @param value DEBUG value 36 | * @return length of the message in bytes (excluding serial stream start sign) 37 | */ 38 | static inline uint16_t mavlink_msg_debug_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 39 | uint32_t time_boot_ms, uint8_t ind, float value) 40 | { 41 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 42 | char buf[9]; 43 | _mav_put_uint32_t(buf, 0, time_boot_ms); 44 | _mav_put_float(buf, 4, value); 45 | _mav_put_uint8_t(buf, 8, ind); 46 | 47 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 9); 48 | #else 49 | mavlink_debug_t packet; 50 | packet.time_boot_ms = time_boot_ms; 51 | packet.value = value; 52 | packet.ind = ind; 53 | 54 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 9); 55 | #endif 56 | 57 | msg->msgid = MAVLINK_MSG_ID_DEBUG; 58 | return mavlink_finalize_message(msg, system_id, component_id, 9, 46); 59 | } 60 | 61 | /** 62 | * @brief Pack a debug message on a channel 63 | * @param system_id ID of this system 64 | * @param component_id ID of this component (e.g. 200 for IMU) 65 | * @param chan The MAVLink channel this message was sent over 66 | * @param msg The MAVLink message to compress the data into 67 | * @param time_boot_ms Timestamp (milliseconds since system boot) 68 | * @param ind index of debug variable 69 | * @param value DEBUG value 70 | * @return length of the message in bytes (excluding serial stream start sign) 71 | */ 72 | static inline uint16_t mavlink_msg_debug_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 73 | mavlink_message_t* msg, 74 | uint32_t time_boot_ms,uint8_t ind,float value) 75 | { 76 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 77 | char buf[9]; 78 | _mav_put_uint32_t(buf, 0, time_boot_ms); 79 | _mav_put_float(buf, 4, value); 80 | _mav_put_uint8_t(buf, 8, ind); 81 | 82 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 9); 83 | #else 84 | mavlink_debug_t packet; 85 | packet.time_boot_ms = time_boot_ms; 86 | packet.value = value; 87 | packet.ind = ind; 88 | 89 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 9); 90 | #endif 91 | 92 | msg->msgid = MAVLINK_MSG_ID_DEBUG; 93 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 9, 46); 94 | } 95 | 96 | /** 97 | * @brief Encode a debug struct into a message 98 | * 99 | * @param system_id ID of this system 100 | * @param component_id ID of this component (e.g. 200 for IMU) 101 | * @param msg The MAVLink message to compress the data into 102 | * @param debug C-struct to read the message contents from 103 | */ 104 | 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) 105 | { 106 | return mavlink_msg_debug_pack(system_id, component_id, msg, debug->time_boot_ms, debug->ind, debug->value); 107 | } 108 | 109 | /** 110 | * @brief Send a debug message 111 | * @param chan MAVLink channel to send the message 112 | * 113 | * @param time_boot_ms Timestamp (milliseconds since system boot) 114 | * @param ind index of debug variable 115 | * @param value DEBUG value 116 | */ 117 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 118 | 119 | static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t ind, float value) 120 | { 121 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 122 | char buf[9]; 123 | _mav_put_uint32_t(buf, 0, time_boot_ms); 124 | _mav_put_float(buf, 4, value); 125 | _mav_put_uint8_t(buf, 8, ind); 126 | 127 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, buf, 9, 46); 128 | #else 129 | mavlink_debug_t packet; 130 | packet.time_boot_ms = time_boot_ms; 131 | packet.value = value; 132 | packet.ind = ind; 133 | 134 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, (const char *)&packet, 9, 46); 135 | #endif 136 | } 137 | 138 | #endif 139 | 140 | // MESSAGE DEBUG UNPACKING 141 | 142 | 143 | /** 144 | * @brief Get field time_boot_ms from debug message 145 | * 146 | * @return Timestamp (milliseconds since system boot) 147 | */ 148 | static inline uint32_t mavlink_msg_debug_get_time_boot_ms(const mavlink_message_t* msg) 149 | { 150 | return _MAV_RETURN_uint32_t(msg, 0); 151 | } 152 | 153 | /** 154 | * @brief Get field ind from debug message 155 | * 156 | * @return index of debug variable 157 | */ 158 | static inline uint8_t mavlink_msg_debug_get_ind(const mavlink_message_t* msg) 159 | { 160 | return _MAV_RETURN_uint8_t(msg, 8); 161 | } 162 | 163 | /** 164 | * @brief Get field value from debug message 165 | * 166 | * @return DEBUG value 167 | */ 168 | static inline float mavlink_msg_debug_get_value(const mavlink_message_t* msg) 169 | { 170 | return _MAV_RETURN_float(msg, 4); 171 | } 172 | 173 | /** 174 | * @brief Decode a debug message into a struct 175 | * 176 | * @param msg The message to decode 177 | * @param debug C-struct to decode the message contents into 178 | */ 179 | static inline void mavlink_msg_debug_decode(const mavlink_message_t* msg, mavlink_debug_t* debug) 180 | { 181 | #if MAVLINK_NEED_BYTE_SWAP 182 | debug->time_boot_ms = mavlink_msg_debug_get_time_boot_ms(msg); 183 | debug->value = mavlink_msg_debug_get_value(msg); 184 | debug->ind = mavlink_msg_debug_get_ind(msg); 185 | #else 186 | memcpy(debug, _MAV_PAYLOAD(msg), 9); 187 | #endif 188 | } 189 | -------------------------------------------------------------------------------- /libraries/mavlink/include/common/mavlink_msg_mission_clear_all.h: -------------------------------------------------------------------------------- 1 | // MESSAGE MISSION_CLEAR_ALL PACKING 2 | 3 | #define MAVLINK_MSG_ID_MISSION_CLEAR_ALL 45 4 | 5 | typedef struct __mavlink_mission_clear_all_t 6 | { 7 | uint8_t target_system; ///< System ID 8 | uint8_t target_component; ///< Component ID 9 | } mavlink_mission_clear_all_t; 10 | 11 | #define MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN 2 12 | #define MAVLINK_MSG_ID_45_LEN 2 13 | 14 | 15 | 16 | #define MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL { \ 17 | "MISSION_CLEAR_ALL", \ 18 | 2, \ 19 | { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_clear_all_t, target_system) }, \ 20 | { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_clear_all_t, target_component) }, \ 21 | } \ 22 | } 23 | 24 | 25 | /** 26 | * @brief Pack a mission_clear_all message 27 | * @param system_id ID of this system 28 | * @param component_id ID of this component (e.g. 200 for IMU) 29 | * @param msg The MAVLink message to compress the data into 30 | * 31 | * @param target_system System ID 32 | * @param target_component Component ID 33 | * @return length of the message in bytes (excluding serial stream start sign) 34 | */ 35 | static inline uint16_t mavlink_msg_mission_clear_all_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 36 | uint8_t target_system, uint8_t target_component) 37 | { 38 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 39 | char buf[2]; 40 | _mav_put_uint8_t(buf, 0, target_system); 41 | _mav_put_uint8_t(buf, 1, target_component); 42 | 43 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 2); 44 | #else 45 | mavlink_mission_clear_all_t packet; 46 | packet.target_system = target_system; 47 | packet.target_component = target_component; 48 | 49 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); 50 | #endif 51 | 52 | msg->msgid = MAVLINK_MSG_ID_MISSION_CLEAR_ALL; 53 | return mavlink_finalize_message(msg, system_id, component_id, 2, 232); 54 | } 55 | 56 | /** 57 | * @brief Pack a mission_clear_all message on a channel 58 | * @param system_id ID of this system 59 | * @param component_id ID of this component (e.g. 200 for IMU) 60 | * @param chan The MAVLink channel this message was sent over 61 | * @param msg The MAVLink message to compress the data into 62 | * @param target_system System ID 63 | * @param target_component Component ID 64 | * @return length of the message in bytes (excluding serial stream start sign) 65 | */ 66 | static inline uint16_t mavlink_msg_mission_clear_all_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 67 | mavlink_message_t* msg, 68 | uint8_t target_system,uint8_t target_component) 69 | { 70 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 71 | char buf[2]; 72 | _mav_put_uint8_t(buf, 0, target_system); 73 | _mav_put_uint8_t(buf, 1, target_component); 74 | 75 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 2); 76 | #else 77 | mavlink_mission_clear_all_t packet; 78 | packet.target_system = target_system; 79 | packet.target_component = target_component; 80 | 81 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); 82 | #endif 83 | 84 | msg->msgid = MAVLINK_MSG_ID_MISSION_CLEAR_ALL; 85 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 232); 86 | } 87 | 88 | /** 89 | * @brief Encode a mission_clear_all struct into a message 90 | * 91 | * @param system_id ID of this system 92 | * @param component_id ID of this component (e.g. 200 for IMU) 93 | * @param msg The MAVLink message to compress the data into 94 | * @param mission_clear_all C-struct to read the message contents from 95 | */ 96 | static inline uint16_t mavlink_msg_mission_clear_all_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_clear_all_t* mission_clear_all) 97 | { 98 | return mavlink_msg_mission_clear_all_pack(system_id, component_id, msg, mission_clear_all->target_system, mission_clear_all->target_component); 99 | } 100 | 101 | /** 102 | * @brief Send a mission_clear_all message 103 | * @param chan MAVLink channel to send the message 104 | * 105 | * @param target_system System ID 106 | * @param target_component Component ID 107 | */ 108 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 109 | 110 | static inline void mavlink_msg_mission_clear_all_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) 111 | { 112 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 113 | char buf[2]; 114 | _mav_put_uint8_t(buf, 0, target_system); 115 | _mav_put_uint8_t(buf, 1, target_component); 116 | 117 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, buf, 2, 232); 118 | #else 119 | mavlink_mission_clear_all_t packet; 120 | packet.target_system = target_system; 121 | packet.target_component = target_component; 122 | 123 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, (const char *)&packet, 2, 232); 124 | #endif 125 | } 126 | 127 | #endif 128 | 129 | // MESSAGE MISSION_CLEAR_ALL UNPACKING 130 | 131 | 132 | /** 133 | * @brief Get field target_system from mission_clear_all message 134 | * 135 | * @return System ID 136 | */ 137 | static inline uint8_t mavlink_msg_mission_clear_all_get_target_system(const mavlink_message_t* msg) 138 | { 139 | return _MAV_RETURN_uint8_t(msg, 0); 140 | } 141 | 142 | /** 143 | * @brief Get field target_component from mission_clear_all message 144 | * 145 | * @return Component ID 146 | */ 147 | static inline uint8_t mavlink_msg_mission_clear_all_get_target_component(const mavlink_message_t* msg) 148 | { 149 | return _MAV_RETURN_uint8_t(msg, 1); 150 | } 151 | 152 | /** 153 | * @brief Decode a mission_clear_all message into a struct 154 | * 155 | * @param msg The message to decode 156 | * @param mission_clear_all C-struct to decode the message contents into 157 | */ 158 | static inline void mavlink_msg_mission_clear_all_decode(const mavlink_message_t* msg, mavlink_mission_clear_all_t* mission_clear_all) 159 | { 160 | #if MAVLINK_NEED_BYTE_SWAP 161 | mission_clear_all->target_system = mavlink_msg_mission_clear_all_get_target_system(msg); 162 | mission_clear_all->target_component = mavlink_msg_mission_clear_all_get_target_component(msg); 163 | #else 164 | memcpy(mission_clear_all, _MAV_PAYLOAD(msg), 2); 165 | #endif 166 | } 167 | -------------------------------------------------------------------------------- /libraries/mavlink/include/common/mavlink_msg_mission_current.h: -------------------------------------------------------------------------------- 1 | // MESSAGE MISSION_CURRENT PACKING 2 | 3 | #define MAVLINK_MSG_ID_MISSION_CURRENT 42 4 | 5 | typedef struct __mavlink_mission_current_t 6 | { 7 | uint16_t seq; ///< Sequence 8 | } mavlink_mission_current_t; 9 | 10 | #define MAVLINK_MSG_ID_MISSION_CURRENT_LEN 2 11 | #define MAVLINK_MSG_ID_42_LEN 2 12 | 13 | 14 | 15 | #define MAVLINK_MESSAGE_INFO_MISSION_CURRENT { \ 16 | "MISSION_CURRENT", \ 17 | 1, \ 18 | { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_current_t, seq) }, \ 19 | } \ 20 | } 21 | 22 | 23 | /** 24 | * @brief Pack a mission_current message 25 | * @param system_id ID of this system 26 | * @param component_id ID of this component (e.g. 200 for IMU) 27 | * @param msg The MAVLink message to compress the data into 28 | * 29 | * @param seq Sequence 30 | * @return length of the message in bytes (excluding serial stream start sign) 31 | */ 32 | static inline uint16_t mavlink_msg_mission_current_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 33 | uint16_t seq) 34 | { 35 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 36 | char buf[2]; 37 | _mav_put_uint16_t(buf, 0, seq); 38 | 39 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 2); 40 | #else 41 | mavlink_mission_current_t packet; 42 | packet.seq = seq; 43 | 44 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); 45 | #endif 46 | 47 | msg->msgid = MAVLINK_MSG_ID_MISSION_CURRENT; 48 | return mavlink_finalize_message(msg, system_id, component_id, 2, 28); 49 | } 50 | 51 | /** 52 | * @brief Pack a mission_current message on a channel 53 | * @param system_id ID of this system 54 | * @param component_id ID of this component (e.g. 200 for IMU) 55 | * @param chan The MAVLink channel this message was sent over 56 | * @param msg The MAVLink message to compress the data into 57 | * @param seq Sequence 58 | * @return length of the message in bytes (excluding serial stream start sign) 59 | */ 60 | static inline uint16_t mavlink_msg_mission_current_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 61 | mavlink_message_t* msg, 62 | uint16_t seq) 63 | { 64 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 65 | char buf[2]; 66 | _mav_put_uint16_t(buf, 0, seq); 67 | 68 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 2); 69 | #else 70 | mavlink_mission_current_t packet; 71 | packet.seq = seq; 72 | 73 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); 74 | #endif 75 | 76 | msg->msgid = MAVLINK_MSG_ID_MISSION_CURRENT; 77 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 28); 78 | } 79 | 80 | /** 81 | * @brief Encode a mission_current struct into a message 82 | * 83 | * @param system_id ID of this system 84 | * @param component_id ID of this component (e.g. 200 for IMU) 85 | * @param msg The MAVLink message to compress the data into 86 | * @param mission_current C-struct to read the message contents from 87 | */ 88 | 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) 89 | { 90 | return mavlink_msg_mission_current_pack(system_id, component_id, msg, mission_current->seq); 91 | } 92 | 93 | /** 94 | * @brief Send a mission_current message 95 | * @param chan MAVLink channel to send the message 96 | * 97 | * @param seq Sequence 98 | */ 99 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 100 | 101 | static inline void mavlink_msg_mission_current_send(mavlink_channel_t chan, uint16_t seq) 102 | { 103 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 104 | char buf[2]; 105 | _mav_put_uint16_t(buf, 0, seq); 106 | 107 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, buf, 2, 28); 108 | #else 109 | mavlink_mission_current_t packet; 110 | packet.seq = seq; 111 | 112 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)&packet, 2, 28); 113 | #endif 114 | } 115 | 116 | #endif 117 | 118 | // MESSAGE MISSION_CURRENT UNPACKING 119 | 120 | 121 | /** 122 | * @brief Get field seq from mission_current message 123 | * 124 | * @return Sequence 125 | */ 126 | static inline uint16_t mavlink_msg_mission_current_get_seq(const mavlink_message_t* msg) 127 | { 128 | return _MAV_RETURN_uint16_t(msg, 0); 129 | } 130 | 131 | /** 132 | * @brief Decode a mission_current message into a struct 133 | * 134 | * @param msg The message to decode 135 | * @param mission_current C-struct to decode the message contents into 136 | */ 137 | static inline void mavlink_msg_mission_current_decode(const mavlink_message_t* msg, mavlink_mission_current_t* mission_current) 138 | { 139 | #if MAVLINK_NEED_BYTE_SWAP 140 | mission_current->seq = mavlink_msg_mission_current_get_seq(msg); 141 | #else 142 | memcpy(mission_current, _MAV_PAYLOAD(msg), 2); 143 | #endif 144 | } 145 | -------------------------------------------------------------------------------- /libraries/mavlink/include/common/mavlink_msg_mission_item_reached.h: -------------------------------------------------------------------------------- 1 | // MESSAGE MISSION_ITEM_REACHED PACKING 2 | 3 | #define MAVLINK_MSG_ID_MISSION_ITEM_REACHED 46 4 | 5 | typedef struct __mavlink_mission_item_reached_t 6 | { 7 | uint16_t seq; ///< Sequence 8 | } mavlink_mission_item_reached_t; 9 | 10 | #define MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN 2 11 | #define MAVLINK_MSG_ID_46_LEN 2 12 | 13 | 14 | 15 | #define MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED { \ 16 | "MISSION_ITEM_REACHED", \ 17 | 1, \ 18 | { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_item_reached_t, seq) }, \ 19 | } \ 20 | } 21 | 22 | 23 | /** 24 | * @brief Pack a mission_item_reached message 25 | * @param system_id ID of this system 26 | * @param component_id ID of this component (e.g. 200 for IMU) 27 | * @param msg The MAVLink message to compress the data into 28 | * 29 | * @param seq Sequence 30 | * @return length of the message in bytes (excluding serial stream start sign) 31 | */ 32 | static inline uint16_t mavlink_msg_mission_item_reached_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 33 | uint16_t seq) 34 | { 35 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 36 | char buf[2]; 37 | _mav_put_uint16_t(buf, 0, seq); 38 | 39 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 2); 40 | #else 41 | mavlink_mission_item_reached_t packet; 42 | packet.seq = seq; 43 | 44 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); 45 | #endif 46 | 47 | msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM_REACHED; 48 | return mavlink_finalize_message(msg, system_id, component_id, 2, 11); 49 | } 50 | 51 | /** 52 | * @brief Pack a mission_item_reached message on a channel 53 | * @param system_id ID of this system 54 | * @param component_id ID of this component (e.g. 200 for IMU) 55 | * @param chan The MAVLink channel this message was sent over 56 | * @param msg The MAVLink message to compress the data into 57 | * @param seq Sequence 58 | * @return length of the message in bytes (excluding serial stream start sign) 59 | */ 60 | static inline uint16_t mavlink_msg_mission_item_reached_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 61 | mavlink_message_t* msg, 62 | uint16_t seq) 63 | { 64 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 65 | char buf[2]; 66 | _mav_put_uint16_t(buf, 0, seq); 67 | 68 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 2); 69 | #else 70 | mavlink_mission_item_reached_t packet; 71 | packet.seq = seq; 72 | 73 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); 74 | #endif 75 | 76 | msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM_REACHED; 77 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 11); 78 | } 79 | 80 | /** 81 | * @brief Encode a mission_item_reached struct into a message 82 | * 83 | * @param system_id ID of this system 84 | * @param component_id ID of this component (e.g. 200 for IMU) 85 | * @param msg The MAVLink message to compress the data into 86 | * @param mission_item_reached C-struct to read the message contents from 87 | */ 88 | 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) 89 | { 90 | return mavlink_msg_mission_item_reached_pack(system_id, component_id, msg, mission_item_reached->seq); 91 | } 92 | 93 | /** 94 | * @brief Send a mission_item_reached message 95 | * @param chan MAVLink channel to send the message 96 | * 97 | * @param seq Sequence 98 | */ 99 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 100 | 101 | static inline void mavlink_msg_mission_item_reached_send(mavlink_channel_t chan, uint16_t seq) 102 | { 103 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 104 | char buf[2]; 105 | _mav_put_uint16_t(buf, 0, seq); 106 | 107 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, buf, 2, 11); 108 | #else 109 | mavlink_mission_item_reached_t packet; 110 | packet.seq = seq; 111 | 112 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, (const char *)&packet, 2, 11); 113 | #endif 114 | } 115 | 116 | #endif 117 | 118 | // MESSAGE MISSION_ITEM_REACHED UNPACKING 119 | 120 | 121 | /** 122 | * @brief Get field seq from mission_item_reached message 123 | * 124 | * @return Sequence 125 | */ 126 | static inline uint16_t mavlink_msg_mission_item_reached_get_seq(const mavlink_message_t* msg) 127 | { 128 | return _MAV_RETURN_uint16_t(msg, 0); 129 | } 130 | 131 | /** 132 | * @brief Decode a mission_item_reached message into a struct 133 | * 134 | * @param msg The message to decode 135 | * @param mission_item_reached C-struct to decode the message contents into 136 | */ 137 | static inline void mavlink_msg_mission_item_reached_decode(const mavlink_message_t* msg, mavlink_mission_item_reached_t* mission_item_reached) 138 | { 139 | #if MAVLINK_NEED_BYTE_SWAP 140 | mission_item_reached->seq = mavlink_msg_mission_item_reached_get_seq(msg); 141 | #else 142 | memcpy(mission_item_reached, _MAV_PAYLOAD(msg), 2); 143 | #endif 144 | } 145 | -------------------------------------------------------------------------------- /libraries/mavlink/include/common/mavlink_msg_mission_request_list.h: -------------------------------------------------------------------------------- 1 | // MESSAGE MISSION_REQUEST_LIST PACKING 2 | 3 | #define MAVLINK_MSG_ID_MISSION_REQUEST_LIST 43 4 | 5 | typedef struct __mavlink_mission_request_list_t 6 | { 7 | uint8_t target_system; ///< System ID 8 | uint8_t target_component; ///< Component ID 9 | } mavlink_mission_request_list_t; 10 | 11 | #define MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN 2 12 | #define MAVLINK_MSG_ID_43_LEN 2 13 | 14 | 15 | 16 | #define MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST { \ 17 | "MISSION_REQUEST_LIST", \ 18 | 2, \ 19 | { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_request_list_t, target_system) }, \ 20 | { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_request_list_t, target_component) }, \ 21 | } \ 22 | } 23 | 24 | 25 | /** 26 | * @brief Pack a mission_request_list message 27 | * @param system_id ID of this system 28 | * @param component_id ID of this component (e.g. 200 for IMU) 29 | * @param msg The MAVLink message to compress the data into 30 | * 31 | * @param target_system System ID 32 | * @param target_component Component ID 33 | * @return length of the message in bytes (excluding serial stream start sign) 34 | */ 35 | static inline uint16_t mavlink_msg_mission_request_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 36 | uint8_t target_system, uint8_t target_component) 37 | { 38 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 39 | char buf[2]; 40 | _mav_put_uint8_t(buf, 0, target_system); 41 | _mav_put_uint8_t(buf, 1, target_component); 42 | 43 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 2); 44 | #else 45 | mavlink_mission_request_list_t packet; 46 | packet.target_system = target_system; 47 | packet.target_component = target_component; 48 | 49 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); 50 | #endif 51 | 52 | msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST_LIST; 53 | return mavlink_finalize_message(msg, system_id, component_id, 2, 132); 54 | } 55 | 56 | /** 57 | * @brief Pack a mission_request_list message on a channel 58 | * @param system_id ID of this system 59 | * @param component_id ID of this component (e.g. 200 for IMU) 60 | * @param chan The MAVLink channel this message was sent over 61 | * @param msg The MAVLink message to compress the data into 62 | * @param target_system System ID 63 | * @param target_component Component ID 64 | * @return length of the message in bytes (excluding serial stream start sign) 65 | */ 66 | static inline uint16_t mavlink_msg_mission_request_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 67 | mavlink_message_t* msg, 68 | uint8_t target_system,uint8_t target_component) 69 | { 70 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 71 | char buf[2]; 72 | _mav_put_uint8_t(buf, 0, target_system); 73 | _mav_put_uint8_t(buf, 1, target_component); 74 | 75 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 2); 76 | #else 77 | mavlink_mission_request_list_t packet; 78 | packet.target_system = target_system; 79 | packet.target_component = target_component; 80 | 81 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); 82 | #endif 83 | 84 | msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST_LIST; 85 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 132); 86 | } 87 | 88 | /** 89 | * @brief Encode a mission_request_list struct into a message 90 | * 91 | * @param system_id ID of this system 92 | * @param component_id ID of this component (e.g. 200 for IMU) 93 | * @param msg The MAVLink message to compress the data into 94 | * @param mission_request_list C-struct to read the message contents from 95 | */ 96 | static inline uint16_t mavlink_msg_mission_request_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_request_list_t* mission_request_list) 97 | { 98 | return mavlink_msg_mission_request_list_pack(system_id, component_id, msg, mission_request_list->target_system, mission_request_list->target_component); 99 | } 100 | 101 | /** 102 | * @brief Send a mission_request_list message 103 | * @param chan MAVLink channel to send the message 104 | * 105 | * @param target_system System ID 106 | * @param target_component Component ID 107 | */ 108 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 109 | 110 | static inline void mavlink_msg_mission_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) 111 | { 112 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 113 | char buf[2]; 114 | _mav_put_uint8_t(buf, 0, target_system); 115 | _mav_put_uint8_t(buf, 1, target_component); 116 | 117 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, buf, 2, 132); 118 | #else 119 | mavlink_mission_request_list_t packet; 120 | packet.target_system = target_system; 121 | packet.target_component = target_component; 122 | 123 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, (const char *)&packet, 2, 132); 124 | #endif 125 | } 126 | 127 | #endif 128 | 129 | // MESSAGE MISSION_REQUEST_LIST UNPACKING 130 | 131 | 132 | /** 133 | * @brief Get field target_system from mission_request_list message 134 | * 135 | * @return System ID 136 | */ 137 | static inline uint8_t mavlink_msg_mission_request_list_get_target_system(const mavlink_message_t* msg) 138 | { 139 | return _MAV_RETURN_uint8_t(msg, 0); 140 | } 141 | 142 | /** 143 | * @brief Get field target_component from mission_request_list message 144 | * 145 | * @return Component ID 146 | */ 147 | static inline uint8_t mavlink_msg_mission_request_list_get_target_component(const mavlink_message_t* msg) 148 | { 149 | return _MAV_RETURN_uint8_t(msg, 1); 150 | } 151 | 152 | /** 153 | * @brief Decode a mission_request_list message into a struct 154 | * 155 | * @param msg The message to decode 156 | * @param mission_request_list C-struct to decode the message contents into 157 | */ 158 | static inline void mavlink_msg_mission_request_list_decode(const mavlink_message_t* msg, mavlink_mission_request_list_t* mission_request_list) 159 | { 160 | #if MAVLINK_NEED_BYTE_SWAP 161 | mission_request_list->target_system = mavlink_msg_mission_request_list_get_target_system(msg); 162 | mission_request_list->target_component = mavlink_msg_mission_request_list_get_target_component(msg); 163 | #else 164 | memcpy(mission_request_list, _MAV_PAYLOAD(msg), 2); 165 | #endif 166 | } 167 | -------------------------------------------------------------------------------- /libraries/mavlink/include/common/mavlink_msg_param_request_list.h: -------------------------------------------------------------------------------- 1 | // MESSAGE PARAM_REQUEST_LIST PACKING 2 | 3 | #define MAVLINK_MSG_ID_PARAM_REQUEST_LIST 21 4 | 5 | typedef struct __mavlink_param_request_list_t 6 | { 7 | uint8_t target_system; ///< System ID 8 | uint8_t target_component; ///< Component ID 9 | } mavlink_param_request_list_t; 10 | 11 | #define MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN 2 12 | #define MAVLINK_MSG_ID_21_LEN 2 13 | 14 | 15 | 16 | #define MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST { \ 17 | "PARAM_REQUEST_LIST", \ 18 | 2, \ 19 | { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_param_request_list_t, target_system) }, \ 20 | { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_param_request_list_t, target_component) }, \ 21 | } \ 22 | } 23 | 24 | 25 | /** 26 | * @brief Pack a param_request_list message 27 | * @param system_id ID of this system 28 | * @param component_id ID of this component (e.g. 200 for IMU) 29 | * @param msg The MAVLink message to compress the data into 30 | * 31 | * @param target_system System ID 32 | * @param target_component Component ID 33 | * @return length of the message in bytes (excluding serial stream start sign) 34 | */ 35 | static inline uint16_t mavlink_msg_param_request_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 36 | uint8_t target_system, uint8_t target_component) 37 | { 38 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 39 | char buf[2]; 40 | _mav_put_uint8_t(buf, 0, target_system); 41 | _mav_put_uint8_t(buf, 1, target_component); 42 | 43 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 2); 44 | #else 45 | mavlink_param_request_list_t packet; 46 | packet.target_system = target_system; 47 | packet.target_component = target_component; 48 | 49 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); 50 | #endif 51 | 52 | msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; 53 | return mavlink_finalize_message(msg, system_id, component_id, 2, 159); 54 | } 55 | 56 | /** 57 | * @brief Pack a param_request_list message on a channel 58 | * @param system_id ID of this system 59 | * @param component_id ID of this component (e.g. 200 for IMU) 60 | * @param chan The MAVLink channel this message was sent over 61 | * @param msg The MAVLink message to compress the data into 62 | * @param target_system System ID 63 | * @param target_component Component ID 64 | * @return length of the message in bytes (excluding serial stream start sign) 65 | */ 66 | static inline uint16_t mavlink_msg_param_request_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 67 | mavlink_message_t* msg, 68 | uint8_t target_system,uint8_t target_component) 69 | { 70 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 71 | char buf[2]; 72 | _mav_put_uint8_t(buf, 0, target_system); 73 | _mav_put_uint8_t(buf, 1, target_component); 74 | 75 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 2); 76 | #else 77 | mavlink_param_request_list_t packet; 78 | packet.target_system = target_system; 79 | packet.target_component = target_component; 80 | 81 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); 82 | #endif 83 | 84 | msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; 85 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 159); 86 | } 87 | 88 | /** 89 | * @brief Encode a param_request_list struct into a message 90 | * 91 | * @param system_id ID of this system 92 | * @param component_id ID of this component (e.g. 200 for IMU) 93 | * @param msg The MAVLink message to compress the data into 94 | * @param param_request_list C-struct to read the message contents from 95 | */ 96 | static inline uint16_t mavlink_msg_param_request_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_request_list_t* param_request_list) 97 | { 98 | return mavlink_msg_param_request_list_pack(system_id, component_id, msg, param_request_list->target_system, param_request_list->target_component); 99 | } 100 | 101 | /** 102 | * @brief Send a param_request_list message 103 | * @param chan MAVLink channel to send the message 104 | * 105 | * @param target_system System ID 106 | * @param target_component Component ID 107 | */ 108 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 109 | 110 | static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) 111 | { 112 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 113 | char buf[2]; 114 | _mav_put_uint8_t(buf, 0, target_system); 115 | _mav_put_uint8_t(buf, 1, target_component); 116 | 117 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, buf, 2, 159); 118 | #else 119 | mavlink_param_request_list_t packet; 120 | packet.target_system = target_system; 121 | packet.target_component = target_component; 122 | 123 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)&packet, 2, 159); 124 | #endif 125 | } 126 | 127 | #endif 128 | 129 | // MESSAGE PARAM_REQUEST_LIST UNPACKING 130 | 131 | 132 | /** 133 | * @brief Get field target_system from param_request_list message 134 | * 135 | * @return System ID 136 | */ 137 | static inline uint8_t mavlink_msg_param_request_list_get_target_system(const mavlink_message_t* msg) 138 | { 139 | return _MAV_RETURN_uint8_t(msg, 0); 140 | } 141 | 142 | /** 143 | * @brief Get field target_component from param_request_list message 144 | * 145 | * @return Component ID 146 | */ 147 | static inline uint8_t mavlink_msg_param_request_list_get_target_component(const mavlink_message_t* msg) 148 | { 149 | return _MAV_RETURN_uint8_t(msg, 1); 150 | } 151 | 152 | /** 153 | * @brief Decode a param_request_list message into a struct 154 | * 155 | * @param msg The message to decode 156 | * @param param_request_list C-struct to decode the message contents into 157 | */ 158 | static inline void mavlink_msg_param_request_list_decode(const mavlink_message_t* msg, mavlink_param_request_list_t* param_request_list) 159 | { 160 | #if MAVLINK_NEED_BYTE_SWAP 161 | param_request_list->target_system = mavlink_msg_param_request_list_get_target_system(msg); 162 | param_request_list->target_component = mavlink_msg_param_request_list_get_target_component(msg); 163 | #else 164 | memcpy(param_request_list, _MAV_PAYLOAD(msg), 2); 165 | #endif 166 | } 167 | -------------------------------------------------------------------------------- /libraries/mavlink/include/common/mavlink_msg_statustext.h: -------------------------------------------------------------------------------- 1 | // MESSAGE STATUSTEXT PACKING 2 | 3 | #define MAVLINK_MSG_ID_STATUSTEXT 253 4 | 5 | typedef struct __mavlink_statustext_t 6 | { 7 | uint8_t severity; ///< Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. 8 | char text[50]; ///< Status text message, without null termination character 9 | } mavlink_statustext_t; 10 | 11 | #define MAVLINK_MSG_ID_STATUSTEXT_LEN 51 12 | #define MAVLINK_MSG_ID_253_LEN 51 13 | 14 | #define MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN 50 15 | 16 | #define MAVLINK_MESSAGE_INFO_STATUSTEXT { \ 17 | "STATUSTEXT", \ 18 | 2, \ 19 | { { "severity", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_statustext_t, severity) }, \ 20 | { "text", NULL, MAVLINK_TYPE_CHAR, 50, 1, offsetof(mavlink_statustext_t, text) }, \ 21 | } \ 22 | } 23 | 24 | 25 | /** 26 | * @brief Pack a statustext message 27 | * @param system_id ID of this system 28 | * @param component_id ID of this component (e.g. 200 for IMU) 29 | * @param msg The MAVLink message to compress the data into 30 | * 31 | * @param severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. 32 | * @param text Status text message, without null termination character 33 | * @return length of the message in bytes (excluding serial stream start sign) 34 | */ 35 | static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 36 | uint8_t severity, const char *text) 37 | { 38 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 39 | char buf[51]; 40 | _mav_put_uint8_t(buf, 0, severity); 41 | _mav_put_char_array(buf, 1, text, 50); 42 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 51); 43 | #else 44 | mavlink_statustext_t packet; 45 | packet.severity = severity; 46 | mav_array_memcpy(packet.text, text, sizeof(char)*50); 47 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 51); 48 | #endif 49 | 50 | msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; 51 | return mavlink_finalize_message(msg, system_id, component_id, 51, 83); 52 | } 53 | 54 | /** 55 | * @brief Pack a statustext message on a channel 56 | * @param system_id ID of this system 57 | * @param component_id ID of this component (e.g. 200 for IMU) 58 | * @param chan The MAVLink channel this message was sent over 59 | * @param msg The MAVLink message to compress the data into 60 | * @param severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. 61 | * @param text Status text message, without null termination character 62 | * @return length of the message in bytes (excluding serial stream start sign) 63 | */ 64 | static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 65 | mavlink_message_t* msg, 66 | uint8_t severity,const char *text) 67 | { 68 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 69 | char buf[51]; 70 | _mav_put_uint8_t(buf, 0, severity); 71 | _mav_put_char_array(buf, 1, text, 50); 72 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 51); 73 | #else 74 | mavlink_statustext_t packet; 75 | packet.severity = severity; 76 | mav_array_memcpy(packet.text, text, sizeof(char)*50); 77 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 51); 78 | #endif 79 | 80 | msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; 81 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 51, 83); 82 | } 83 | 84 | /** 85 | * @brief Encode a statustext struct into a message 86 | * 87 | * @param system_id ID of this system 88 | * @param component_id ID of this component (e.g. 200 for IMU) 89 | * @param msg The MAVLink message to compress the data into 90 | * @param statustext C-struct to read the message contents from 91 | */ 92 | 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) 93 | { 94 | return mavlink_msg_statustext_pack(system_id, component_id, msg, statustext->severity, statustext->text); 95 | } 96 | 97 | /** 98 | * @brief Send a statustext message 99 | * @param chan MAVLink channel to send the message 100 | * 101 | * @param severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. 102 | * @param text Status text message, without null termination character 103 | */ 104 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 105 | 106 | static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char *text) 107 | { 108 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 109 | char buf[51]; 110 | _mav_put_uint8_t(buf, 0, severity); 111 | _mav_put_char_array(buf, 1, text, 50); 112 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, 51, 83); 113 | #else 114 | mavlink_statustext_t packet; 115 | packet.severity = severity; 116 | mav_array_memcpy(packet.text, text, sizeof(char)*50); 117 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)&packet, 51, 83); 118 | #endif 119 | } 120 | 121 | #endif 122 | 123 | // MESSAGE STATUSTEXT UNPACKING 124 | 125 | 126 | /** 127 | * @brief Get field severity from statustext message 128 | * 129 | * @return Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. 130 | */ 131 | static inline uint8_t mavlink_msg_statustext_get_severity(const mavlink_message_t* msg) 132 | { 133 | return _MAV_RETURN_uint8_t(msg, 0); 134 | } 135 | 136 | /** 137 | * @brief Get field text from statustext message 138 | * 139 | * @return Status text message, without null termination character 140 | */ 141 | static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* msg, char *text) 142 | { 143 | return _MAV_RETURN_char_array(msg, text, 50, 1); 144 | } 145 | 146 | /** 147 | * @brief Decode a statustext message into a struct 148 | * 149 | * @param msg The message to decode 150 | * @param statustext C-struct to decode the message contents into 151 | */ 152 | static inline void mavlink_msg_statustext_decode(const mavlink_message_t* msg, mavlink_statustext_t* statustext) 153 | { 154 | #if MAVLINK_NEED_BYTE_SWAP 155 | statustext->severity = mavlink_msg_statustext_get_severity(msg); 156 | mavlink_msg_statustext_get_text(msg, statustext->text); 157 | #else 158 | memcpy(statustext, _MAV_PAYLOAD(msg), 51); 159 | #endif 160 | } 161 | -------------------------------------------------------------------------------- /libraries/mavlink/include/common/mavlink_msg_system_time.h: -------------------------------------------------------------------------------- 1 | // MESSAGE SYSTEM_TIME PACKING 2 | 3 | #define MAVLINK_MSG_ID_SYSTEM_TIME 2 4 | 5 | typedef struct __mavlink_system_time_t 6 | { 7 | uint64_t time_unix_usec; ///< Timestamp of the master clock in microseconds since UNIX epoch. 8 | uint32_t time_boot_ms; ///< Timestamp of the component clock since boot time in milliseconds. 9 | } mavlink_system_time_t; 10 | 11 | #define MAVLINK_MSG_ID_SYSTEM_TIME_LEN 12 12 | #define MAVLINK_MSG_ID_2_LEN 12 13 | 14 | 15 | 16 | #define MAVLINK_MESSAGE_INFO_SYSTEM_TIME { \ 17 | "SYSTEM_TIME", \ 18 | 2, \ 19 | { { "time_unix_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_system_time_t, time_unix_usec) }, \ 20 | { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_system_time_t, time_boot_ms) }, \ 21 | } \ 22 | } 23 | 24 | 25 | /** 26 | * @brief Pack a system_time message 27 | * @param system_id ID of this system 28 | * @param component_id ID of this component (e.g. 200 for IMU) 29 | * @param msg The MAVLink message to compress the data into 30 | * 31 | * @param time_unix_usec Timestamp of the master clock in microseconds since UNIX epoch. 32 | * @param time_boot_ms Timestamp of the component clock since boot time in milliseconds. 33 | * @return length of the message in bytes (excluding serial stream start sign) 34 | */ 35 | static inline uint16_t mavlink_msg_system_time_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 36 | uint64_t time_unix_usec, uint32_t time_boot_ms) 37 | { 38 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 39 | char buf[12]; 40 | _mav_put_uint64_t(buf, 0, time_unix_usec); 41 | _mav_put_uint32_t(buf, 8, time_boot_ms); 42 | 43 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); 44 | #else 45 | mavlink_system_time_t packet; 46 | packet.time_unix_usec = time_unix_usec; 47 | packet.time_boot_ms = time_boot_ms; 48 | 49 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); 50 | #endif 51 | 52 | msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; 53 | return mavlink_finalize_message(msg, system_id, component_id, 12, 137); 54 | } 55 | 56 | /** 57 | * @brief Pack a system_time message on a channel 58 | * @param system_id ID of this system 59 | * @param component_id ID of this component (e.g. 200 for IMU) 60 | * @param chan The MAVLink channel this message was sent over 61 | * @param msg The MAVLink message to compress the data into 62 | * @param time_unix_usec Timestamp of the master clock in microseconds since UNIX epoch. 63 | * @param time_boot_ms Timestamp of the component clock since boot time in milliseconds. 64 | * @return length of the message in bytes (excluding serial stream start sign) 65 | */ 66 | static inline uint16_t mavlink_msg_system_time_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 67 | mavlink_message_t* msg, 68 | uint64_t time_unix_usec,uint32_t time_boot_ms) 69 | { 70 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 71 | char buf[12]; 72 | _mav_put_uint64_t(buf, 0, time_unix_usec); 73 | _mav_put_uint32_t(buf, 8, time_boot_ms); 74 | 75 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); 76 | #else 77 | mavlink_system_time_t packet; 78 | packet.time_unix_usec = time_unix_usec; 79 | packet.time_boot_ms = time_boot_ms; 80 | 81 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); 82 | #endif 83 | 84 | msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; 85 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 137); 86 | } 87 | 88 | /** 89 | * @brief Encode a system_time struct into a message 90 | * 91 | * @param system_id ID of this system 92 | * @param component_id ID of this component (e.g. 200 for IMU) 93 | * @param msg The MAVLink message to compress the data into 94 | * @param system_time C-struct to read the message contents from 95 | */ 96 | static inline uint16_t mavlink_msg_system_time_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_system_time_t* system_time) 97 | { 98 | return mavlink_msg_system_time_pack(system_id, component_id, msg, system_time->time_unix_usec, system_time->time_boot_ms); 99 | } 100 | 101 | /** 102 | * @brief Send a system_time message 103 | * @param chan MAVLink channel to send the message 104 | * 105 | * @param time_unix_usec Timestamp of the master clock in microseconds since UNIX epoch. 106 | * @param time_boot_ms Timestamp of the component clock since boot time in milliseconds. 107 | */ 108 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 109 | 110 | static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t time_unix_usec, uint32_t time_boot_ms) 111 | { 112 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 113 | char buf[12]; 114 | _mav_put_uint64_t(buf, 0, time_unix_usec); 115 | _mav_put_uint32_t(buf, 8, time_boot_ms); 116 | 117 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, buf, 12, 137); 118 | #else 119 | mavlink_system_time_t packet; 120 | packet.time_unix_usec = time_unix_usec; 121 | packet.time_boot_ms = time_boot_ms; 122 | 123 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, (const char *)&packet, 12, 137); 124 | #endif 125 | } 126 | 127 | #endif 128 | 129 | // MESSAGE SYSTEM_TIME UNPACKING 130 | 131 | 132 | /** 133 | * @brief Get field time_unix_usec from system_time message 134 | * 135 | * @return Timestamp of the master clock in microseconds since UNIX epoch. 136 | */ 137 | static inline uint64_t mavlink_msg_system_time_get_time_unix_usec(const mavlink_message_t* msg) 138 | { 139 | return _MAV_RETURN_uint64_t(msg, 0); 140 | } 141 | 142 | /** 143 | * @brief Get field time_boot_ms from system_time message 144 | * 145 | * @return Timestamp of the component clock since boot time in milliseconds. 146 | */ 147 | static inline uint32_t mavlink_msg_system_time_get_time_boot_ms(const mavlink_message_t* msg) 148 | { 149 | return _MAV_RETURN_uint32_t(msg, 8); 150 | } 151 | 152 | /** 153 | * @brief Decode a system_time message into a struct 154 | * 155 | * @param msg The message to decode 156 | * @param system_time C-struct to decode the message contents into 157 | */ 158 | static inline void mavlink_msg_system_time_decode(const mavlink_message_t* msg, mavlink_system_time_t* system_time) 159 | { 160 | #if MAVLINK_NEED_BYTE_SWAP 161 | system_time->time_unix_usec = mavlink_msg_system_time_get_time_unix_usec(msg); 162 | system_time->time_boot_ms = mavlink_msg_system_time_get_time_boot_ms(msg); 163 | #else 164 | memcpy(system_time, _MAV_PAYLOAD(msg), 12); 165 | #endif 166 | } 167 | -------------------------------------------------------------------------------- /libraries/mavlink/include/common/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from common.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_VERSION_H 6 | #define MAVLINK_VERSION_H 7 | 8 | #define MAVLINK_BUILD_DATE "Fri Apr 20 12:22:54 2012" 9 | #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" 10 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101 11 | 12 | #endif // MAVLINK_VERSION_H 13 | -------------------------------------------------------------------------------- /libraries/mavlink/include/matrixpilot/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from matrixpilot.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_H 6 | #define MAVLINK_H 7 | 8 | #ifndef MAVLINK_STX 9 | #define MAVLINK_STX 254 10 | #endif 11 | 12 | #ifndef MAVLINK_ENDIAN 13 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 14 | #endif 15 | 16 | #ifndef MAVLINK_ALIGNED_FIELDS 17 | #define MAVLINK_ALIGNED_FIELDS 1 18 | #endif 19 | 20 | #ifndef MAVLINK_CRC_EXTRA 21 | #define MAVLINK_CRC_EXTRA 1 22 | #endif 23 | 24 | #include "version.h" 25 | #include "matrixpilot.h" 26 | 27 | #endif // MAVLINK_H 28 | -------------------------------------------------------------------------------- /libraries/mavlink/include/matrixpilot/testsuite.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol testsuite generated from matrixpilot.xml 3 | * @see http://qgroundcontrol.org/mavlink/ 4 | */ 5 | #ifndef MATRIXPILOT_TESTSUITE_H 6 | #define MATRIXPILOT_TESTSUITE_H 7 | 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | #ifndef MAVLINK_TEST_ALL 13 | #define MAVLINK_TEST_ALL 14 | static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); 15 | static void mavlink_test_matrixpilot(uint8_t, uint8_t, mavlink_message_t *last_msg); 16 | 17 | static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) 18 | { 19 | mavlink_test_common(system_id, component_id, last_msg); 20 | mavlink_test_matrixpilot(system_id, component_id, last_msg); 21 | } 22 | #endif 23 | 24 | #include "../common/testsuite.h" 25 | 26 | 27 | 28 | static void mavlink_test_matrixpilot(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) 29 | { 30 | 31 | } 32 | 33 | #ifdef __cplusplus 34 | } 35 | #endif // __cplusplus 36 | #endif // MATRIXPILOT_TESTSUITE_H 37 | -------------------------------------------------------------------------------- /libraries/mavlink/include/matrixpilot/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from matrixpilot.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_VERSION_H 6 | #define MAVLINK_VERSION_H 7 | 8 | #define MAVLINK_BUILD_DATE "Fri Apr 20 12:22:49 2012" 9 | #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" 10 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101 11 | 12 | #endif // MAVLINK_VERSION_H 13 | -------------------------------------------------------------------------------- /libraries/mavlink/include/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from ardupilotmega.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_H 6 | #define MAVLINK_H 7 | 8 | #ifndef MAVLINK_STX 9 | #define MAVLINK_STX 254 10 | #endif 11 | 12 | #ifndef MAVLINK_ENDIAN 13 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 14 | #endif 15 | 16 | #ifndef MAVLINK_ALIGNED_FIELDS 17 | #define MAVLINK_ALIGNED_FIELDS 1 18 | #endif 19 | 20 | #ifndef MAVLINK_CRC_EXTRA 21 | #define MAVLINK_CRC_EXTRA 1 22 | #endif 23 | 24 | #include "common/common.h" 25 | #include "protocol.h" 26 | 27 | 28 | #endif // MAVLINK_H 29 | -------------------------------------------------------------------------------- /libraries/mavlink/include/minimal/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from minimal.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_H 6 | #define MAVLINK_H 7 | 8 | #ifndef MAVLINK_STX 9 | #define MAVLINK_STX 254 10 | #endif 11 | 12 | #ifndef MAVLINK_ENDIAN 13 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 14 | #endif 15 | 16 | #ifndef MAVLINK_ALIGNED_FIELDS 17 | #define MAVLINK_ALIGNED_FIELDS 1 18 | #endif 19 | 20 | #ifndef MAVLINK_CRC_EXTRA 21 | #define MAVLINK_CRC_EXTRA 1 22 | #endif 23 | 24 | #include "version.h" 25 | #include "minimal.h" 26 | 27 | #endif // MAVLINK_H 28 | -------------------------------------------------------------------------------- /libraries/mavlink/include/minimal/testsuite.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol testsuite generated from minimal.xml 3 | * @see http://qgroundcontrol.org/mavlink/ 4 | */ 5 | #ifndef MINIMAL_TESTSUITE_H 6 | #define MINIMAL_TESTSUITE_H 7 | 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | #ifndef MAVLINK_TEST_ALL 13 | #define MAVLINK_TEST_ALL 14 | 15 | static void mavlink_test_minimal(uint8_t, uint8_t, mavlink_message_t *last_msg); 16 | 17 | static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) 18 | { 19 | 20 | mavlink_test_minimal(system_id, component_id, last_msg); 21 | } 22 | #endif 23 | 24 | 25 | 26 | 27 | static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) 28 | { 29 | mavlink_message_t msg; 30 | uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; 31 | uint16_t i; 32 | mavlink_heartbeat_t packet_in = { 33 | 963497464, 34 | 17, 35 | 84, 36 | 151, 37 | 218, 38 | 2, 39 | }; 40 | mavlink_heartbeat_t packet1, packet2; 41 | memset(&packet1, 0, sizeof(packet1)); 42 | packet1.custom_mode = packet_in.custom_mode; 43 | packet1.type = packet_in.type; 44 | packet1.autopilot = packet_in.autopilot; 45 | packet1.base_mode = packet_in.base_mode; 46 | packet1.system_status = packet_in.system_status; 47 | packet1.mavlink_version = packet_in.mavlink_version; 48 | 49 | 50 | 51 | memset(&packet2, 0, sizeof(packet2)); 52 | mavlink_msg_heartbeat_encode(system_id, component_id, &msg, &packet1); 53 | mavlink_msg_heartbeat_decode(&msg, &packet2); 54 | MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); 55 | 56 | memset(&packet2, 0, sizeof(packet2)); 57 | mavlink_msg_heartbeat_pack(system_id, component_id, &msg , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status ); 58 | mavlink_msg_heartbeat_decode(&msg, &packet2); 59 | MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); 60 | 61 | memset(&packet2, 0, sizeof(packet2)); 62 | mavlink_msg_heartbeat_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status ); 63 | mavlink_msg_heartbeat_decode(&msg, &packet2); 64 | MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); 65 | 66 | memset(&packet2, 0, sizeof(packet2)); 67 | mavlink_msg_to_send_buffer(buffer, &msg); 68 | for (i=0; imsgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; 51 | return mavlink_finalize_message(msg, system_id, component_id, 255, 223); 52 | } 53 | 54 | /** 55 | * @brief Pack a encapsulated_data message on a channel 56 | * @param system_id ID of this system 57 | * @param component_id ID of this component (e.g. 200 for IMU) 58 | * @param chan The MAVLink channel this message was sent over 59 | * @param msg The MAVLink message to compress the data into 60 | * @param seqnr sequence number (starting with 0 on every transmission) 61 | * @param data image data bytes 62 | * @return length of the message in bytes (excluding serial stream start sign) 63 | */ 64 | static inline uint16_t mavlink_msg_encapsulated_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 65 | mavlink_message_t* msg, 66 | uint16_t seqnr,const uint8_t *data) 67 | { 68 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 69 | char buf[255]; 70 | _mav_put_uint16_t(buf, 0, seqnr); 71 | _mav_put_uint8_t_array(buf, 2, data, 253); 72 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 255); 73 | #else 74 | mavlink_encapsulated_data_t packet; 75 | packet.seqnr = seqnr; 76 | mav_array_memcpy(packet.data, data, sizeof(uint8_t)*253); 77 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 255); 78 | #endif 79 | 80 | msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; 81 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 255, 223); 82 | } 83 | 84 | /** 85 | * @brief Encode a encapsulated_data struct into a message 86 | * 87 | * @param system_id ID of this system 88 | * @param component_id ID of this component (e.g. 200 for IMU) 89 | * @param msg The MAVLink message to compress the data into 90 | * @param encapsulated_data C-struct to read the message contents from 91 | */ 92 | static inline uint16_t mavlink_msg_encapsulated_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_encapsulated_data_t* encapsulated_data) 93 | { 94 | return mavlink_msg_encapsulated_data_pack(system_id, component_id, msg, encapsulated_data->seqnr, encapsulated_data->data); 95 | } 96 | 97 | /** 98 | * @brief Send a encapsulated_data message 99 | * @param chan MAVLink channel to send the message 100 | * 101 | * @param seqnr sequence number (starting with 0 on every transmission) 102 | * @param data image data bytes 103 | */ 104 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 105 | 106 | static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, uint16_t seqnr, const uint8_t *data) 107 | { 108 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 109 | char buf[255]; 110 | _mav_put_uint16_t(buf, 0, seqnr); 111 | _mav_put_uint8_t_array(buf, 2, data, 253); 112 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, buf, 255, 223); 113 | #else 114 | mavlink_encapsulated_data_t packet; 115 | packet.seqnr = seqnr; 116 | mav_array_memcpy(packet.data, data, sizeof(uint8_t)*253); 117 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, (const char *)&packet, 255, 223); 118 | #endif 119 | } 120 | 121 | #endif 122 | 123 | // MESSAGE ENCAPSULATED_DATA UNPACKING 124 | 125 | 126 | /** 127 | * @brief Get field seqnr from encapsulated_data message 128 | * 129 | * @return sequence number (starting with 0 on every transmission) 130 | */ 131 | static inline uint16_t mavlink_msg_encapsulated_data_get_seqnr(const mavlink_message_t* msg) 132 | { 133 | return _MAV_RETURN_uint16_t(msg, 0); 134 | } 135 | 136 | /** 137 | * @brief Get field data from encapsulated_data message 138 | * 139 | * @return image data bytes 140 | */ 141 | static inline uint16_t mavlink_msg_encapsulated_data_get_data(const mavlink_message_t* msg, uint8_t *data) 142 | { 143 | return _MAV_RETURN_uint8_t_array(msg, data, 253, 2); 144 | } 145 | 146 | /** 147 | * @brief Decode a encapsulated_data message into a struct 148 | * 149 | * @param msg The message to decode 150 | * @param encapsulated_data C-struct to decode the message contents into 151 | */ 152 | static inline void mavlink_msg_encapsulated_data_decode(const mavlink_message_t* msg, mavlink_encapsulated_data_t* encapsulated_data) 153 | { 154 | #if MAVLINK_NEED_BYTE_SWAP 155 | encapsulated_data->seqnr = mavlink_msg_encapsulated_data_get_seqnr(msg); 156 | mavlink_msg_encapsulated_data_get_data(msg, encapsulated_data->data); 157 | #else 158 | memcpy(encapsulated_data, _MAV_PAYLOAD(msg), 255); 159 | #endif 160 | } 161 | -------------------------------------------------------------------------------- /libraries/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h: -------------------------------------------------------------------------------- 1 | // MESSAGE IMAGE_TRIGGER_CONTROL PACKING 2 | 3 | #define MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL 153 4 | 5 | typedef struct __mavlink_image_trigger_control_t 6 | { 7 | uint8_t enable; ///< 0 to disable, 1 to enable 8 | } mavlink_image_trigger_control_t; 9 | 10 | #define MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN 1 11 | #define MAVLINK_MSG_ID_153_LEN 1 12 | 13 | 14 | 15 | #define MAVLINK_MESSAGE_INFO_IMAGE_TRIGGER_CONTROL { \ 16 | "IMAGE_TRIGGER_CONTROL", \ 17 | 1, \ 18 | { { "enable", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_image_trigger_control_t, enable) }, \ 19 | } \ 20 | } 21 | 22 | 23 | /** 24 | * @brief Pack a image_trigger_control message 25 | * @param system_id ID of this system 26 | * @param component_id ID of this component (e.g. 200 for IMU) 27 | * @param msg The MAVLink message to compress the data into 28 | * 29 | * @param enable 0 to disable, 1 to enable 30 | * @return length of the message in bytes (excluding serial stream start sign) 31 | */ 32 | static inline uint16_t mavlink_msg_image_trigger_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 33 | uint8_t enable) 34 | { 35 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 36 | char buf[1]; 37 | _mav_put_uint8_t(buf, 0, enable); 38 | 39 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 1); 40 | #else 41 | mavlink_image_trigger_control_t packet; 42 | packet.enable = enable; 43 | 44 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 1); 45 | #endif 46 | 47 | msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; 48 | return mavlink_finalize_message(msg, system_id, component_id, 1, 95); 49 | } 50 | 51 | /** 52 | * @brief Pack a image_trigger_control message on a channel 53 | * @param system_id ID of this system 54 | * @param component_id ID of this component (e.g. 200 for IMU) 55 | * @param chan The MAVLink channel this message was sent over 56 | * @param msg The MAVLink message to compress the data into 57 | * @param enable 0 to disable, 1 to enable 58 | * @return length of the message in bytes (excluding serial stream start sign) 59 | */ 60 | static inline uint16_t mavlink_msg_image_trigger_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 61 | mavlink_message_t* msg, 62 | uint8_t enable) 63 | { 64 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 65 | char buf[1]; 66 | _mav_put_uint8_t(buf, 0, enable); 67 | 68 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 1); 69 | #else 70 | mavlink_image_trigger_control_t packet; 71 | packet.enable = enable; 72 | 73 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 1); 74 | #endif 75 | 76 | msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; 77 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 1, 95); 78 | } 79 | 80 | /** 81 | * @brief Encode a image_trigger_control struct into a message 82 | * 83 | * @param system_id ID of this system 84 | * @param component_id ID of this component (e.g. 200 for IMU) 85 | * @param msg The MAVLink message to compress the data into 86 | * @param image_trigger_control C-struct to read the message contents from 87 | */ 88 | static inline uint16_t mavlink_msg_image_trigger_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_image_trigger_control_t* image_trigger_control) 89 | { 90 | return mavlink_msg_image_trigger_control_pack(system_id, component_id, msg, image_trigger_control->enable); 91 | } 92 | 93 | /** 94 | * @brief Send a image_trigger_control message 95 | * @param chan MAVLink channel to send the message 96 | * 97 | * @param enable 0 to disable, 1 to enable 98 | */ 99 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 100 | 101 | static inline void mavlink_msg_image_trigger_control_send(mavlink_channel_t chan, uint8_t enable) 102 | { 103 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 104 | char buf[1]; 105 | _mav_put_uint8_t(buf, 0, enable); 106 | 107 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, buf, 1, 95); 108 | #else 109 | mavlink_image_trigger_control_t packet; 110 | packet.enable = enable; 111 | 112 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, (const char *)&packet, 1, 95); 113 | #endif 114 | } 115 | 116 | #endif 117 | 118 | // MESSAGE IMAGE_TRIGGER_CONTROL UNPACKING 119 | 120 | 121 | /** 122 | * @brief Get field enable from image_trigger_control message 123 | * 124 | * @return 0 to disable, 1 to enable 125 | */ 126 | static inline uint8_t mavlink_msg_image_trigger_control_get_enable(const mavlink_message_t* msg) 127 | { 128 | return _MAV_RETURN_uint8_t(msg, 0); 129 | } 130 | 131 | /** 132 | * @brief Decode a image_trigger_control message into a struct 133 | * 134 | * @param msg The message to decode 135 | * @param image_trigger_control C-struct to decode the message contents into 136 | */ 137 | static inline void mavlink_msg_image_trigger_control_decode(const mavlink_message_t* msg, mavlink_image_trigger_control_t* image_trigger_control) 138 | { 139 | #if MAVLINK_NEED_BYTE_SWAP 140 | image_trigger_control->enable = mavlink_msg_image_trigger_control_get_enable(msg); 141 | #else 142 | memcpy(image_trigger_control, _MAV_PAYLOAD(msg), 1); 143 | #endif 144 | } 145 | -------------------------------------------------------------------------------- /libraries/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h: -------------------------------------------------------------------------------- 1 | // MESSAGE WATCHDOG_HEARTBEAT PACKING 2 | 3 | #define MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT 180 4 | 5 | typedef struct __mavlink_watchdog_heartbeat_t 6 | { 7 | uint16_t watchdog_id; ///< Watchdog ID 8 | uint16_t process_count; ///< Number of processes 9 | } mavlink_watchdog_heartbeat_t; 10 | 11 | #define MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN 4 12 | #define MAVLINK_MSG_ID_180_LEN 4 13 | 14 | 15 | 16 | #define MAVLINK_MESSAGE_INFO_WATCHDOG_HEARTBEAT { \ 17 | "WATCHDOG_HEARTBEAT", \ 18 | 2, \ 19 | { { "watchdog_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_watchdog_heartbeat_t, watchdog_id) }, \ 20 | { "process_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_watchdog_heartbeat_t, process_count) }, \ 21 | } \ 22 | } 23 | 24 | 25 | /** 26 | * @brief Pack a watchdog_heartbeat message 27 | * @param system_id ID of this system 28 | * @param component_id ID of this component (e.g. 200 for IMU) 29 | * @param msg The MAVLink message to compress the data into 30 | * 31 | * @param watchdog_id Watchdog ID 32 | * @param process_count Number of processes 33 | * @return length of the message in bytes (excluding serial stream start sign) 34 | */ 35 | static inline uint16_t mavlink_msg_watchdog_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 36 | uint16_t watchdog_id, uint16_t process_count) 37 | { 38 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 39 | char buf[4]; 40 | _mav_put_uint16_t(buf, 0, watchdog_id); 41 | _mav_put_uint16_t(buf, 2, process_count); 42 | 43 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 4); 44 | #else 45 | mavlink_watchdog_heartbeat_t packet; 46 | packet.watchdog_id = watchdog_id; 47 | packet.process_count = process_count; 48 | 49 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 4); 50 | #endif 51 | 52 | msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; 53 | return mavlink_finalize_message(msg, system_id, component_id, 4, 153); 54 | } 55 | 56 | /** 57 | * @brief Pack a watchdog_heartbeat message on a channel 58 | * @param system_id ID of this system 59 | * @param component_id ID of this component (e.g. 200 for IMU) 60 | * @param chan The MAVLink channel this message was sent over 61 | * @param msg The MAVLink message to compress the data into 62 | * @param watchdog_id Watchdog ID 63 | * @param process_count Number of processes 64 | * @return length of the message in bytes (excluding serial stream start sign) 65 | */ 66 | static inline uint16_t mavlink_msg_watchdog_heartbeat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 67 | mavlink_message_t* msg, 68 | uint16_t watchdog_id,uint16_t process_count) 69 | { 70 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 71 | char buf[4]; 72 | _mav_put_uint16_t(buf, 0, watchdog_id); 73 | _mav_put_uint16_t(buf, 2, process_count); 74 | 75 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 4); 76 | #else 77 | mavlink_watchdog_heartbeat_t packet; 78 | packet.watchdog_id = watchdog_id; 79 | packet.process_count = process_count; 80 | 81 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 4); 82 | #endif 83 | 84 | msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; 85 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 153); 86 | } 87 | 88 | /** 89 | * @brief Encode a watchdog_heartbeat struct into a message 90 | * 91 | * @param system_id ID of this system 92 | * @param component_id ID of this component (e.g. 200 for IMU) 93 | * @param msg The MAVLink message to compress the data into 94 | * @param watchdog_heartbeat C-struct to read the message contents from 95 | */ 96 | static inline uint16_t mavlink_msg_watchdog_heartbeat_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_watchdog_heartbeat_t* watchdog_heartbeat) 97 | { 98 | return mavlink_msg_watchdog_heartbeat_pack(system_id, component_id, msg, watchdog_heartbeat->watchdog_id, watchdog_heartbeat->process_count); 99 | } 100 | 101 | /** 102 | * @brief Send a watchdog_heartbeat message 103 | * @param chan MAVLink channel to send the message 104 | * 105 | * @param watchdog_id Watchdog ID 106 | * @param process_count Number of processes 107 | */ 108 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 109 | 110 | static inline void mavlink_msg_watchdog_heartbeat_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_count) 111 | { 112 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 113 | char buf[4]; 114 | _mav_put_uint16_t(buf, 0, watchdog_id); 115 | _mav_put_uint16_t(buf, 2, process_count); 116 | 117 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, buf, 4, 153); 118 | #else 119 | mavlink_watchdog_heartbeat_t packet; 120 | packet.watchdog_id = watchdog_id; 121 | packet.process_count = process_count; 122 | 123 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, (const char *)&packet, 4, 153); 124 | #endif 125 | } 126 | 127 | #endif 128 | 129 | // MESSAGE WATCHDOG_HEARTBEAT UNPACKING 130 | 131 | 132 | /** 133 | * @brief Get field watchdog_id from watchdog_heartbeat message 134 | * 135 | * @return Watchdog ID 136 | */ 137 | static inline uint16_t mavlink_msg_watchdog_heartbeat_get_watchdog_id(const mavlink_message_t* msg) 138 | { 139 | return _MAV_RETURN_uint16_t(msg, 0); 140 | } 141 | 142 | /** 143 | * @brief Get field process_count from watchdog_heartbeat message 144 | * 145 | * @return Number of processes 146 | */ 147 | static inline uint16_t mavlink_msg_watchdog_heartbeat_get_process_count(const mavlink_message_t* msg) 148 | { 149 | return _MAV_RETURN_uint16_t(msg, 2); 150 | } 151 | 152 | /** 153 | * @brief Decode a watchdog_heartbeat message into a struct 154 | * 155 | * @param msg The message to decode 156 | * @param watchdog_heartbeat C-struct to decode the message contents into 157 | */ 158 | static inline void mavlink_msg_watchdog_heartbeat_decode(const mavlink_message_t* msg, mavlink_watchdog_heartbeat_t* watchdog_heartbeat) 159 | { 160 | #if MAVLINK_NEED_BYTE_SWAP 161 | watchdog_heartbeat->watchdog_id = mavlink_msg_watchdog_heartbeat_get_watchdog_id(msg); 162 | watchdog_heartbeat->process_count = mavlink_msg_watchdog_heartbeat_get_process_count(msg); 163 | #else 164 | memcpy(watchdog_heartbeat, _MAV_PAYLOAD(msg), 4); 165 | #endif 166 | } 167 | -------------------------------------------------------------------------------- /libraries/mavlink/include/pixhawk/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from pixhawk.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_VERSION_H 6 | #define MAVLINK_VERSION_H 7 | 8 | #define MAVLINK_BUILD_DATE "Fri Apr 20 12:22:50 2012" 9 | #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" 10 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 11 | 12 | #endif // MAVLINK_VERSION_H 13 | -------------------------------------------------------------------------------- /libraries/mavlink/include/sensesoar/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from sensesoar.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_H 6 | #define MAVLINK_H 7 | 8 | #ifndef MAVLINK_STX 9 | #define MAVLINK_STX 254 10 | #endif 11 | 12 | #ifndef MAVLINK_ENDIAN 13 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 14 | #endif 15 | 16 | #ifndef MAVLINK_ALIGNED_FIELDS 17 | #define MAVLINK_ALIGNED_FIELDS 1 18 | #endif 19 | 20 | #ifndef MAVLINK_CRC_EXTRA 21 | #define MAVLINK_CRC_EXTRA 1 22 | #endif 23 | 24 | #include "version.h" 25 | #include "sensesoar.h" 26 | 27 | #endif // MAVLINK_H 28 | -------------------------------------------------------------------------------- /libraries/mavlink/include/sensesoar/mavlink_msg_cmd_airspeed_ack.h: -------------------------------------------------------------------------------- 1 | // MESSAGE CMD_AIRSPEED_ACK PACKING 2 | 3 | #define MAVLINK_MSG_ID_CMD_AIRSPEED_ACK 194 4 | 5 | typedef struct __mavlink_cmd_airspeed_ack_t 6 | { 7 | float spCmd; ///< commanded airspeed 8 | uint8_t ack; ///< 0:ack, 1:nack 9 | } mavlink_cmd_airspeed_ack_t; 10 | 11 | #define MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN 5 12 | #define MAVLINK_MSG_ID_194_LEN 5 13 | 14 | 15 | 16 | #define MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_ACK { \ 17 | "CMD_AIRSPEED_ACK", \ 18 | 2, \ 19 | { { "spCmd", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_cmd_airspeed_ack_t, spCmd) }, \ 20 | { "ack", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_cmd_airspeed_ack_t, ack) }, \ 21 | } \ 22 | } 23 | 24 | 25 | /** 26 | * @brief Pack a cmd_airspeed_ack message 27 | * @param system_id ID of this system 28 | * @param component_id ID of this component (e.g. 200 for IMU) 29 | * @param msg The MAVLink message to compress the data into 30 | * 31 | * @param spCmd commanded airspeed 32 | * @param ack 0:ack, 1:nack 33 | * @return length of the message in bytes (excluding serial stream start sign) 34 | */ 35 | static inline uint16_t mavlink_msg_cmd_airspeed_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 36 | float spCmd, uint8_t ack) 37 | { 38 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 39 | char buf[5]; 40 | _mav_put_float(buf, 0, spCmd); 41 | _mav_put_uint8_t(buf, 4, ack); 42 | 43 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 5); 44 | #else 45 | mavlink_cmd_airspeed_ack_t packet; 46 | packet.spCmd = spCmd; 47 | packet.ack = ack; 48 | 49 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 5); 50 | #endif 51 | 52 | msg->msgid = MAVLINK_MSG_ID_CMD_AIRSPEED_ACK; 53 | return mavlink_finalize_message(msg, system_id, component_id, 5, 243); 54 | } 55 | 56 | /** 57 | * @brief Pack a cmd_airspeed_ack message on a channel 58 | * @param system_id ID of this system 59 | * @param component_id ID of this component (e.g. 200 for IMU) 60 | * @param chan The MAVLink channel this message was sent over 61 | * @param msg The MAVLink message to compress the data into 62 | * @param spCmd commanded airspeed 63 | * @param ack 0:ack, 1:nack 64 | * @return length of the message in bytes (excluding serial stream start sign) 65 | */ 66 | static inline uint16_t mavlink_msg_cmd_airspeed_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 67 | mavlink_message_t* msg, 68 | float spCmd,uint8_t ack) 69 | { 70 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 71 | char buf[5]; 72 | _mav_put_float(buf, 0, spCmd); 73 | _mav_put_uint8_t(buf, 4, ack); 74 | 75 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 5); 76 | #else 77 | mavlink_cmd_airspeed_ack_t packet; 78 | packet.spCmd = spCmd; 79 | packet.ack = ack; 80 | 81 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 5); 82 | #endif 83 | 84 | msg->msgid = MAVLINK_MSG_ID_CMD_AIRSPEED_ACK; 85 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 5, 243); 86 | } 87 | 88 | /** 89 | * @brief Encode a cmd_airspeed_ack struct into a message 90 | * 91 | * @param system_id ID of this system 92 | * @param component_id ID of this component (e.g. 200 for IMU) 93 | * @param msg The MAVLink message to compress the data into 94 | * @param cmd_airspeed_ack C-struct to read the message contents from 95 | */ 96 | static inline uint16_t mavlink_msg_cmd_airspeed_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_cmd_airspeed_ack_t* cmd_airspeed_ack) 97 | { 98 | return mavlink_msg_cmd_airspeed_ack_pack(system_id, component_id, msg, cmd_airspeed_ack->spCmd, cmd_airspeed_ack->ack); 99 | } 100 | 101 | /** 102 | * @brief Send a cmd_airspeed_ack message 103 | * @param chan MAVLink channel to send the message 104 | * 105 | * @param spCmd commanded airspeed 106 | * @param ack 0:ack, 1:nack 107 | */ 108 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 109 | 110 | static inline void mavlink_msg_cmd_airspeed_ack_send(mavlink_channel_t chan, float spCmd, uint8_t ack) 111 | { 112 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 113 | char buf[5]; 114 | _mav_put_float(buf, 0, spCmd); 115 | _mav_put_uint8_t(buf, 4, ack); 116 | 117 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, buf, 5, 243); 118 | #else 119 | mavlink_cmd_airspeed_ack_t packet; 120 | packet.spCmd = spCmd; 121 | packet.ack = ack; 122 | 123 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, (const char *)&packet, 5, 243); 124 | #endif 125 | } 126 | 127 | #endif 128 | 129 | // MESSAGE CMD_AIRSPEED_ACK UNPACKING 130 | 131 | 132 | /** 133 | * @brief Get field spCmd from cmd_airspeed_ack message 134 | * 135 | * @return commanded airspeed 136 | */ 137 | static inline float mavlink_msg_cmd_airspeed_ack_get_spCmd(const mavlink_message_t* msg) 138 | { 139 | return _MAV_RETURN_float(msg, 0); 140 | } 141 | 142 | /** 143 | * @brief Get field ack from cmd_airspeed_ack message 144 | * 145 | * @return 0:ack, 1:nack 146 | */ 147 | static inline uint8_t mavlink_msg_cmd_airspeed_ack_get_ack(const mavlink_message_t* msg) 148 | { 149 | return _MAV_RETURN_uint8_t(msg, 4); 150 | } 151 | 152 | /** 153 | * @brief Decode a cmd_airspeed_ack message into a struct 154 | * 155 | * @param msg The message to decode 156 | * @param cmd_airspeed_ack C-struct to decode the message contents into 157 | */ 158 | static inline void mavlink_msg_cmd_airspeed_ack_decode(const mavlink_message_t* msg, mavlink_cmd_airspeed_ack_t* cmd_airspeed_ack) 159 | { 160 | #if MAVLINK_NEED_BYTE_SWAP 161 | cmd_airspeed_ack->spCmd = mavlink_msg_cmd_airspeed_ack_get_spCmd(msg); 162 | cmd_airspeed_ack->ack = mavlink_msg_cmd_airspeed_ack_get_ack(msg); 163 | #else 164 | memcpy(cmd_airspeed_ack, _MAV_PAYLOAD(msg), 5); 165 | #endif 166 | } 167 | -------------------------------------------------------------------------------- /libraries/mavlink/include/sensesoar/mavlink_msg_cmd_airspeed_chng.h: -------------------------------------------------------------------------------- 1 | // MESSAGE CMD_AIRSPEED_CHNG PACKING 2 | 3 | #define MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG 192 4 | 5 | typedef struct __mavlink_cmd_airspeed_chng_t 6 | { 7 | float spCmd; ///< commanded airspeed 8 | uint8_t target; ///< Target ID 9 | } mavlink_cmd_airspeed_chng_t; 10 | 11 | #define MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN 5 12 | #define MAVLINK_MSG_ID_192_LEN 5 13 | 14 | 15 | 16 | #define MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_CHNG { \ 17 | "CMD_AIRSPEED_CHNG", \ 18 | 2, \ 19 | { { "spCmd", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_cmd_airspeed_chng_t, spCmd) }, \ 20 | { "target", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_cmd_airspeed_chng_t, target) }, \ 21 | } \ 22 | } 23 | 24 | 25 | /** 26 | * @brief Pack a cmd_airspeed_chng message 27 | * @param system_id ID of this system 28 | * @param component_id ID of this component (e.g. 200 for IMU) 29 | * @param msg The MAVLink message to compress the data into 30 | * 31 | * @param target Target ID 32 | * @param spCmd commanded airspeed 33 | * @return length of the message in bytes (excluding serial stream start sign) 34 | */ 35 | static inline uint16_t mavlink_msg_cmd_airspeed_chng_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 36 | uint8_t target, float spCmd) 37 | { 38 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 39 | char buf[5]; 40 | _mav_put_float(buf, 0, spCmd); 41 | _mav_put_uint8_t(buf, 4, target); 42 | 43 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 5); 44 | #else 45 | mavlink_cmd_airspeed_chng_t packet; 46 | packet.spCmd = spCmd; 47 | packet.target = target; 48 | 49 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 5); 50 | #endif 51 | 52 | msg->msgid = MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG; 53 | return mavlink_finalize_message(msg, system_id, component_id, 5, 209); 54 | } 55 | 56 | /** 57 | * @brief Pack a cmd_airspeed_chng message on a channel 58 | * @param system_id ID of this system 59 | * @param component_id ID of this component (e.g. 200 for IMU) 60 | * @param chan The MAVLink channel this message was sent over 61 | * @param msg The MAVLink message to compress the data into 62 | * @param target Target ID 63 | * @param spCmd commanded airspeed 64 | * @return length of the message in bytes (excluding serial stream start sign) 65 | */ 66 | static inline uint16_t mavlink_msg_cmd_airspeed_chng_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 67 | mavlink_message_t* msg, 68 | uint8_t target,float spCmd) 69 | { 70 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 71 | char buf[5]; 72 | _mav_put_float(buf, 0, spCmd); 73 | _mav_put_uint8_t(buf, 4, target); 74 | 75 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 5); 76 | #else 77 | mavlink_cmd_airspeed_chng_t packet; 78 | packet.spCmd = spCmd; 79 | packet.target = target; 80 | 81 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 5); 82 | #endif 83 | 84 | msg->msgid = MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG; 85 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 5, 209); 86 | } 87 | 88 | /** 89 | * @brief Encode a cmd_airspeed_chng struct into a message 90 | * 91 | * @param system_id ID of this system 92 | * @param component_id ID of this component (e.g. 200 for IMU) 93 | * @param msg The MAVLink message to compress the data into 94 | * @param cmd_airspeed_chng C-struct to read the message contents from 95 | */ 96 | static inline uint16_t mavlink_msg_cmd_airspeed_chng_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_cmd_airspeed_chng_t* cmd_airspeed_chng) 97 | { 98 | return mavlink_msg_cmd_airspeed_chng_pack(system_id, component_id, msg, cmd_airspeed_chng->target, cmd_airspeed_chng->spCmd); 99 | } 100 | 101 | /** 102 | * @brief Send a cmd_airspeed_chng message 103 | * @param chan MAVLink channel to send the message 104 | * 105 | * @param target Target ID 106 | * @param spCmd commanded airspeed 107 | */ 108 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 109 | 110 | static inline void mavlink_msg_cmd_airspeed_chng_send(mavlink_channel_t chan, uint8_t target, float spCmd) 111 | { 112 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 113 | char buf[5]; 114 | _mav_put_float(buf, 0, spCmd); 115 | _mav_put_uint8_t(buf, 4, target); 116 | 117 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, buf, 5, 209); 118 | #else 119 | mavlink_cmd_airspeed_chng_t packet; 120 | packet.spCmd = spCmd; 121 | packet.target = target; 122 | 123 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, (const char *)&packet, 5, 209); 124 | #endif 125 | } 126 | 127 | #endif 128 | 129 | // MESSAGE CMD_AIRSPEED_CHNG UNPACKING 130 | 131 | 132 | /** 133 | * @brief Get field target from cmd_airspeed_chng message 134 | * 135 | * @return Target ID 136 | */ 137 | static inline uint8_t mavlink_msg_cmd_airspeed_chng_get_target(const mavlink_message_t* msg) 138 | { 139 | return _MAV_RETURN_uint8_t(msg, 4); 140 | } 141 | 142 | /** 143 | * @brief Get field spCmd from cmd_airspeed_chng message 144 | * 145 | * @return commanded airspeed 146 | */ 147 | static inline float mavlink_msg_cmd_airspeed_chng_get_spCmd(const mavlink_message_t* msg) 148 | { 149 | return _MAV_RETURN_float(msg, 0); 150 | } 151 | 152 | /** 153 | * @brief Decode a cmd_airspeed_chng message into a struct 154 | * 155 | * @param msg The message to decode 156 | * @param cmd_airspeed_chng C-struct to decode the message contents into 157 | */ 158 | static inline void mavlink_msg_cmd_airspeed_chng_decode(const mavlink_message_t* msg, mavlink_cmd_airspeed_chng_t* cmd_airspeed_chng) 159 | { 160 | #if MAVLINK_NEED_BYTE_SWAP 161 | cmd_airspeed_chng->spCmd = mavlink_msg_cmd_airspeed_chng_get_spCmd(msg); 162 | cmd_airspeed_chng->target = mavlink_msg_cmd_airspeed_chng_get_target(msg); 163 | #else 164 | memcpy(cmd_airspeed_chng, _MAV_PAYLOAD(msg), 5); 165 | #endif 166 | } 167 | -------------------------------------------------------------------------------- /libraries/mavlink/include/sensesoar/mavlink_msg_filt_rot_vel.h: -------------------------------------------------------------------------------- 1 | // MESSAGE FILT_ROT_VEL PACKING 2 | 3 | #define MAVLINK_MSG_ID_FILT_ROT_VEL 184 4 | 5 | typedef struct __mavlink_filt_rot_vel_t 6 | { 7 | float rotVel[3]; ///< rotational velocity 8 | } mavlink_filt_rot_vel_t; 9 | 10 | #define MAVLINK_MSG_ID_FILT_ROT_VEL_LEN 12 11 | #define MAVLINK_MSG_ID_184_LEN 12 12 | 13 | #define MAVLINK_MSG_FILT_ROT_VEL_FIELD_ROTVEL_LEN 3 14 | 15 | #define MAVLINK_MESSAGE_INFO_FILT_ROT_VEL { \ 16 | "FILT_ROT_VEL", \ 17 | 1, \ 18 | { { "rotVel", NULL, MAVLINK_TYPE_FLOAT, 3, 0, offsetof(mavlink_filt_rot_vel_t, rotVel) }, \ 19 | } \ 20 | } 21 | 22 | 23 | /** 24 | * @brief Pack a filt_rot_vel message 25 | * @param system_id ID of this system 26 | * @param component_id ID of this component (e.g. 200 for IMU) 27 | * @param msg The MAVLink message to compress the data into 28 | * 29 | * @param rotVel rotational velocity 30 | * @return length of the message in bytes (excluding serial stream start sign) 31 | */ 32 | static inline uint16_t mavlink_msg_filt_rot_vel_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 33 | const float *rotVel) 34 | { 35 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 36 | char buf[12]; 37 | 38 | _mav_put_float_array(buf, 0, rotVel, 3); 39 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); 40 | #else 41 | mavlink_filt_rot_vel_t packet; 42 | 43 | mav_array_memcpy(packet.rotVel, rotVel, sizeof(float)*3); 44 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); 45 | #endif 46 | 47 | msg->msgid = MAVLINK_MSG_ID_FILT_ROT_VEL; 48 | return mavlink_finalize_message(msg, system_id, component_id, 12, 79); 49 | } 50 | 51 | /** 52 | * @brief Pack a filt_rot_vel message on a channel 53 | * @param system_id ID of this system 54 | * @param component_id ID of this component (e.g. 200 for IMU) 55 | * @param chan The MAVLink channel this message was sent over 56 | * @param msg The MAVLink message to compress the data into 57 | * @param rotVel rotational velocity 58 | * @return length of the message in bytes (excluding serial stream start sign) 59 | */ 60 | static inline uint16_t mavlink_msg_filt_rot_vel_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 61 | mavlink_message_t* msg, 62 | const float *rotVel) 63 | { 64 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 65 | char buf[12]; 66 | 67 | _mav_put_float_array(buf, 0, rotVel, 3); 68 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); 69 | #else 70 | mavlink_filt_rot_vel_t packet; 71 | 72 | mav_array_memcpy(packet.rotVel, rotVel, sizeof(float)*3); 73 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); 74 | #endif 75 | 76 | msg->msgid = MAVLINK_MSG_ID_FILT_ROT_VEL; 77 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 79); 78 | } 79 | 80 | /** 81 | * @brief Encode a filt_rot_vel struct into a message 82 | * 83 | * @param system_id ID of this system 84 | * @param component_id ID of this component (e.g. 200 for IMU) 85 | * @param msg The MAVLink message to compress the data into 86 | * @param filt_rot_vel C-struct to read the message contents from 87 | */ 88 | static inline uint16_t mavlink_msg_filt_rot_vel_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_filt_rot_vel_t* filt_rot_vel) 89 | { 90 | return mavlink_msg_filt_rot_vel_pack(system_id, component_id, msg, filt_rot_vel->rotVel); 91 | } 92 | 93 | /** 94 | * @brief Send a filt_rot_vel message 95 | * @param chan MAVLink channel to send the message 96 | * 97 | * @param rotVel rotational velocity 98 | */ 99 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 100 | 101 | static inline void mavlink_msg_filt_rot_vel_send(mavlink_channel_t chan, const float *rotVel) 102 | { 103 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 104 | char buf[12]; 105 | 106 | _mav_put_float_array(buf, 0, rotVel, 3); 107 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, buf, 12, 79); 108 | #else 109 | mavlink_filt_rot_vel_t packet; 110 | 111 | mav_array_memcpy(packet.rotVel, rotVel, sizeof(float)*3); 112 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, (const char *)&packet, 12, 79); 113 | #endif 114 | } 115 | 116 | #endif 117 | 118 | // MESSAGE FILT_ROT_VEL UNPACKING 119 | 120 | 121 | /** 122 | * @brief Get field rotVel from filt_rot_vel message 123 | * 124 | * @return rotational velocity 125 | */ 126 | static inline uint16_t mavlink_msg_filt_rot_vel_get_rotVel(const mavlink_message_t* msg, float *rotVel) 127 | { 128 | return _MAV_RETURN_float_array(msg, rotVel, 3, 0); 129 | } 130 | 131 | /** 132 | * @brief Decode a filt_rot_vel message into a struct 133 | * 134 | * @param msg The message to decode 135 | * @param filt_rot_vel C-struct to decode the message contents into 136 | */ 137 | static inline void mavlink_msg_filt_rot_vel_decode(const mavlink_message_t* msg, mavlink_filt_rot_vel_t* filt_rot_vel) 138 | { 139 | #if MAVLINK_NEED_BYTE_SWAP 140 | mavlink_msg_filt_rot_vel_get_rotVel(msg, filt_rot_vel->rotVel); 141 | #else 142 | memcpy(filt_rot_vel, _MAV_PAYLOAD(msg), 12); 143 | #endif 144 | } 145 | -------------------------------------------------------------------------------- /libraries/mavlink/include/sensesoar/mavlink_msg_llc_out.h: -------------------------------------------------------------------------------- 1 | // MESSAGE LLC_OUT PACKING 2 | 3 | #define MAVLINK_MSG_ID_LLC_OUT 186 4 | 5 | typedef struct __mavlink_llc_out_t 6 | { 7 | int16_t servoOut[4]; ///< Servo signal 8 | int16_t MotorOut[2]; ///< motor signal 9 | } mavlink_llc_out_t; 10 | 11 | #define MAVLINK_MSG_ID_LLC_OUT_LEN 12 12 | #define MAVLINK_MSG_ID_186_LEN 12 13 | 14 | #define MAVLINK_MSG_LLC_OUT_FIELD_SERVOOUT_LEN 4 15 | #define MAVLINK_MSG_LLC_OUT_FIELD_MOTOROUT_LEN 2 16 | 17 | #define MAVLINK_MESSAGE_INFO_LLC_OUT { \ 18 | "LLC_OUT", \ 19 | 2, \ 20 | { { "servoOut", NULL, MAVLINK_TYPE_INT16_T, 4, 0, offsetof(mavlink_llc_out_t, servoOut) }, \ 21 | { "MotorOut", NULL, MAVLINK_TYPE_INT16_T, 2, 8, offsetof(mavlink_llc_out_t, MotorOut) }, \ 22 | } \ 23 | } 24 | 25 | 26 | /** 27 | * @brief Pack a llc_out message 28 | * @param system_id ID of this system 29 | * @param component_id ID of this component (e.g. 200 for IMU) 30 | * @param msg The MAVLink message to compress the data into 31 | * 32 | * @param servoOut Servo signal 33 | * @param MotorOut motor signal 34 | * @return length of the message in bytes (excluding serial stream start sign) 35 | */ 36 | static inline uint16_t mavlink_msg_llc_out_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 37 | const int16_t *servoOut, const int16_t *MotorOut) 38 | { 39 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 40 | char buf[12]; 41 | 42 | _mav_put_int16_t_array(buf, 0, servoOut, 4); 43 | _mav_put_int16_t_array(buf, 8, MotorOut, 2); 44 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); 45 | #else 46 | mavlink_llc_out_t packet; 47 | 48 | mav_array_memcpy(packet.servoOut, servoOut, sizeof(int16_t)*4); 49 | mav_array_memcpy(packet.MotorOut, MotorOut, sizeof(int16_t)*2); 50 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); 51 | #endif 52 | 53 | msg->msgid = MAVLINK_MSG_ID_LLC_OUT; 54 | return mavlink_finalize_message(msg, system_id, component_id, 12, 5); 55 | } 56 | 57 | /** 58 | * @brief Pack a llc_out message on a channel 59 | * @param system_id ID of this system 60 | * @param component_id ID of this component (e.g. 200 for IMU) 61 | * @param chan The MAVLink channel this message was sent over 62 | * @param msg The MAVLink message to compress the data into 63 | * @param servoOut Servo signal 64 | * @param MotorOut motor signal 65 | * @return length of the message in bytes (excluding serial stream start sign) 66 | */ 67 | static inline uint16_t mavlink_msg_llc_out_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 68 | mavlink_message_t* msg, 69 | const int16_t *servoOut,const int16_t *MotorOut) 70 | { 71 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 72 | char buf[12]; 73 | 74 | _mav_put_int16_t_array(buf, 0, servoOut, 4); 75 | _mav_put_int16_t_array(buf, 8, MotorOut, 2); 76 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); 77 | #else 78 | mavlink_llc_out_t packet; 79 | 80 | mav_array_memcpy(packet.servoOut, servoOut, sizeof(int16_t)*4); 81 | mav_array_memcpy(packet.MotorOut, MotorOut, sizeof(int16_t)*2); 82 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); 83 | #endif 84 | 85 | msg->msgid = MAVLINK_MSG_ID_LLC_OUT; 86 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 5); 87 | } 88 | 89 | /** 90 | * @brief Encode a llc_out struct into a message 91 | * 92 | * @param system_id ID of this system 93 | * @param component_id ID of this component (e.g. 200 for IMU) 94 | * @param msg The MAVLink message to compress the data into 95 | * @param llc_out C-struct to read the message contents from 96 | */ 97 | static inline uint16_t mavlink_msg_llc_out_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_llc_out_t* llc_out) 98 | { 99 | return mavlink_msg_llc_out_pack(system_id, component_id, msg, llc_out->servoOut, llc_out->MotorOut); 100 | } 101 | 102 | /** 103 | * @brief Send a llc_out message 104 | * @param chan MAVLink channel to send the message 105 | * 106 | * @param servoOut Servo signal 107 | * @param MotorOut motor signal 108 | */ 109 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 110 | 111 | static inline void mavlink_msg_llc_out_send(mavlink_channel_t chan, const int16_t *servoOut, const int16_t *MotorOut) 112 | { 113 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 114 | char buf[12]; 115 | 116 | _mav_put_int16_t_array(buf, 0, servoOut, 4); 117 | _mav_put_int16_t_array(buf, 8, MotorOut, 2); 118 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, buf, 12, 5); 119 | #else 120 | mavlink_llc_out_t packet; 121 | 122 | mav_array_memcpy(packet.servoOut, servoOut, sizeof(int16_t)*4); 123 | mav_array_memcpy(packet.MotorOut, MotorOut, sizeof(int16_t)*2); 124 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, (const char *)&packet, 12, 5); 125 | #endif 126 | } 127 | 128 | #endif 129 | 130 | // MESSAGE LLC_OUT UNPACKING 131 | 132 | 133 | /** 134 | * @brief Get field servoOut from llc_out message 135 | * 136 | * @return Servo signal 137 | */ 138 | static inline uint16_t mavlink_msg_llc_out_get_servoOut(const mavlink_message_t* msg, int16_t *servoOut) 139 | { 140 | return _MAV_RETURN_int16_t_array(msg, servoOut, 4, 0); 141 | } 142 | 143 | /** 144 | * @brief Get field MotorOut from llc_out message 145 | * 146 | * @return motor signal 147 | */ 148 | static inline uint16_t mavlink_msg_llc_out_get_MotorOut(const mavlink_message_t* msg, int16_t *MotorOut) 149 | { 150 | return _MAV_RETURN_int16_t_array(msg, MotorOut, 2, 8); 151 | } 152 | 153 | /** 154 | * @brief Decode a llc_out message into a struct 155 | * 156 | * @param msg The message to decode 157 | * @param llc_out C-struct to decode the message contents into 158 | */ 159 | static inline void mavlink_msg_llc_out_decode(const mavlink_message_t* msg, mavlink_llc_out_t* llc_out) 160 | { 161 | #if MAVLINK_NEED_BYTE_SWAP 162 | mavlink_msg_llc_out_get_servoOut(msg, llc_out->servoOut); 163 | mavlink_msg_llc_out_get_MotorOut(msg, llc_out->MotorOut); 164 | #else 165 | memcpy(llc_out, _MAV_PAYLOAD(msg), 12); 166 | #endif 167 | } 168 | -------------------------------------------------------------------------------- /libraries/mavlink/include/sensesoar/mavlink_msg_obs_air_temp.h: -------------------------------------------------------------------------------- 1 | // MESSAGE OBS_AIR_TEMP PACKING 2 | 3 | #define MAVLINK_MSG_ID_OBS_AIR_TEMP 183 4 | 5 | typedef struct __mavlink_obs_air_temp_t 6 | { 7 | float airT; ///< Air Temperatur 8 | } mavlink_obs_air_temp_t; 9 | 10 | #define MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN 4 11 | #define MAVLINK_MSG_ID_183_LEN 4 12 | 13 | 14 | 15 | #define MAVLINK_MESSAGE_INFO_OBS_AIR_TEMP { \ 16 | "OBS_AIR_TEMP", \ 17 | 1, \ 18 | { { "airT", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_obs_air_temp_t, airT) }, \ 19 | } \ 20 | } 21 | 22 | 23 | /** 24 | * @brief Pack a obs_air_temp message 25 | * @param system_id ID of this system 26 | * @param component_id ID of this component (e.g. 200 for IMU) 27 | * @param msg The MAVLink message to compress the data into 28 | * 29 | * @param airT Air Temperatur 30 | * @return length of the message in bytes (excluding serial stream start sign) 31 | */ 32 | static inline uint16_t mavlink_msg_obs_air_temp_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 33 | float airT) 34 | { 35 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 36 | char buf[4]; 37 | _mav_put_float(buf, 0, airT); 38 | 39 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 4); 40 | #else 41 | mavlink_obs_air_temp_t packet; 42 | packet.airT = airT; 43 | 44 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 4); 45 | #endif 46 | 47 | msg->msgid = MAVLINK_MSG_ID_OBS_AIR_TEMP; 48 | return mavlink_finalize_message(msg, system_id, component_id, 4, 248); 49 | } 50 | 51 | /** 52 | * @brief Pack a obs_air_temp message on a channel 53 | * @param system_id ID of this system 54 | * @param component_id ID of this component (e.g. 200 for IMU) 55 | * @param chan The MAVLink channel this message was sent over 56 | * @param msg The MAVLink message to compress the data into 57 | * @param airT Air Temperatur 58 | * @return length of the message in bytes (excluding serial stream start sign) 59 | */ 60 | static inline uint16_t mavlink_msg_obs_air_temp_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 61 | mavlink_message_t* msg, 62 | float airT) 63 | { 64 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 65 | char buf[4]; 66 | _mav_put_float(buf, 0, airT); 67 | 68 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 4); 69 | #else 70 | mavlink_obs_air_temp_t packet; 71 | packet.airT = airT; 72 | 73 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 4); 74 | #endif 75 | 76 | msg->msgid = MAVLINK_MSG_ID_OBS_AIR_TEMP; 77 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 248); 78 | } 79 | 80 | /** 81 | * @brief Encode a obs_air_temp struct into a message 82 | * 83 | * @param system_id ID of this system 84 | * @param component_id ID of this component (e.g. 200 for IMU) 85 | * @param msg The MAVLink message to compress the data into 86 | * @param obs_air_temp C-struct to read the message contents from 87 | */ 88 | static inline uint16_t mavlink_msg_obs_air_temp_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_air_temp_t* obs_air_temp) 89 | { 90 | return mavlink_msg_obs_air_temp_pack(system_id, component_id, msg, obs_air_temp->airT); 91 | } 92 | 93 | /** 94 | * @brief Send a obs_air_temp message 95 | * @param chan MAVLink channel to send the message 96 | * 97 | * @param airT Air Temperatur 98 | */ 99 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 100 | 101 | static inline void mavlink_msg_obs_air_temp_send(mavlink_channel_t chan, float airT) 102 | { 103 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 104 | char buf[4]; 105 | _mav_put_float(buf, 0, airT); 106 | 107 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, buf, 4, 248); 108 | #else 109 | mavlink_obs_air_temp_t packet; 110 | packet.airT = airT; 111 | 112 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, (const char *)&packet, 4, 248); 113 | #endif 114 | } 115 | 116 | #endif 117 | 118 | // MESSAGE OBS_AIR_TEMP UNPACKING 119 | 120 | 121 | /** 122 | * @brief Get field airT from obs_air_temp message 123 | * 124 | * @return Air Temperatur 125 | */ 126 | static inline float mavlink_msg_obs_air_temp_get_airT(const mavlink_message_t* msg) 127 | { 128 | return _MAV_RETURN_float(msg, 0); 129 | } 130 | 131 | /** 132 | * @brief Decode a obs_air_temp message into a struct 133 | * 134 | * @param msg The message to decode 135 | * @param obs_air_temp C-struct to decode the message contents into 136 | */ 137 | static inline void mavlink_msg_obs_air_temp_decode(const mavlink_message_t* msg, mavlink_obs_air_temp_t* obs_air_temp) 138 | { 139 | #if MAVLINK_NEED_BYTE_SWAP 140 | obs_air_temp->airT = mavlink_msg_obs_air_temp_get_airT(msg); 141 | #else 142 | memcpy(obs_air_temp, _MAV_PAYLOAD(msg), 4); 143 | #endif 144 | } 145 | -------------------------------------------------------------------------------- /libraries/mavlink/include/sensesoar/mavlink_msg_obs_air_velocity.h: -------------------------------------------------------------------------------- 1 | // MESSAGE OBS_AIR_VELOCITY PACKING 2 | 3 | #define MAVLINK_MSG_ID_OBS_AIR_VELOCITY 178 4 | 5 | typedef struct __mavlink_obs_air_velocity_t 6 | { 7 | float magnitude; ///< Air speed 8 | float aoa; ///< angle of attack 9 | float slip; ///< slip angle 10 | } mavlink_obs_air_velocity_t; 11 | 12 | #define MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN 12 13 | #define MAVLINK_MSG_ID_178_LEN 12 14 | 15 | 16 | 17 | #define MAVLINK_MESSAGE_INFO_OBS_AIR_VELOCITY { \ 18 | "OBS_AIR_VELOCITY", \ 19 | 3, \ 20 | { { "magnitude", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_obs_air_velocity_t, magnitude) }, \ 21 | { "aoa", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_obs_air_velocity_t, aoa) }, \ 22 | { "slip", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_obs_air_velocity_t, slip) }, \ 23 | } \ 24 | } 25 | 26 | 27 | /** 28 | * @brief Pack a obs_air_velocity message 29 | * @param system_id ID of this system 30 | * @param component_id ID of this component (e.g. 200 for IMU) 31 | * @param msg The MAVLink message to compress the data into 32 | * 33 | * @param magnitude Air speed 34 | * @param aoa angle of attack 35 | * @param slip slip angle 36 | * @return length of the message in bytes (excluding serial stream start sign) 37 | */ 38 | static inline uint16_t mavlink_msg_obs_air_velocity_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 39 | float magnitude, float aoa, float slip) 40 | { 41 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 42 | char buf[12]; 43 | _mav_put_float(buf, 0, magnitude); 44 | _mav_put_float(buf, 4, aoa); 45 | _mav_put_float(buf, 8, slip); 46 | 47 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); 48 | #else 49 | mavlink_obs_air_velocity_t packet; 50 | packet.magnitude = magnitude; 51 | packet.aoa = aoa; 52 | packet.slip = slip; 53 | 54 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); 55 | #endif 56 | 57 | msg->msgid = MAVLINK_MSG_ID_OBS_AIR_VELOCITY; 58 | return mavlink_finalize_message(msg, system_id, component_id, 12, 32); 59 | } 60 | 61 | /** 62 | * @brief Pack a obs_air_velocity message on a channel 63 | * @param system_id ID of this system 64 | * @param component_id ID of this component (e.g. 200 for IMU) 65 | * @param chan The MAVLink channel this message was sent over 66 | * @param msg The MAVLink message to compress the data into 67 | * @param magnitude Air speed 68 | * @param aoa angle of attack 69 | * @param slip slip angle 70 | * @return length of the message in bytes (excluding serial stream start sign) 71 | */ 72 | static inline uint16_t mavlink_msg_obs_air_velocity_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 73 | mavlink_message_t* msg, 74 | float magnitude,float aoa,float slip) 75 | { 76 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 77 | char buf[12]; 78 | _mav_put_float(buf, 0, magnitude); 79 | _mav_put_float(buf, 4, aoa); 80 | _mav_put_float(buf, 8, slip); 81 | 82 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); 83 | #else 84 | mavlink_obs_air_velocity_t packet; 85 | packet.magnitude = magnitude; 86 | packet.aoa = aoa; 87 | packet.slip = slip; 88 | 89 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); 90 | #endif 91 | 92 | msg->msgid = MAVLINK_MSG_ID_OBS_AIR_VELOCITY; 93 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 32); 94 | } 95 | 96 | /** 97 | * @brief Encode a obs_air_velocity struct into a message 98 | * 99 | * @param system_id ID of this system 100 | * @param component_id ID of this component (e.g. 200 for IMU) 101 | * @param msg The MAVLink message to compress the data into 102 | * @param obs_air_velocity C-struct to read the message contents from 103 | */ 104 | static inline uint16_t mavlink_msg_obs_air_velocity_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_air_velocity_t* obs_air_velocity) 105 | { 106 | return mavlink_msg_obs_air_velocity_pack(system_id, component_id, msg, obs_air_velocity->magnitude, obs_air_velocity->aoa, obs_air_velocity->slip); 107 | } 108 | 109 | /** 110 | * @brief Send a obs_air_velocity message 111 | * @param chan MAVLink channel to send the message 112 | * 113 | * @param magnitude Air speed 114 | * @param aoa angle of attack 115 | * @param slip slip angle 116 | */ 117 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 118 | 119 | static inline void mavlink_msg_obs_air_velocity_send(mavlink_channel_t chan, float magnitude, float aoa, float slip) 120 | { 121 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 122 | char buf[12]; 123 | _mav_put_float(buf, 0, magnitude); 124 | _mav_put_float(buf, 4, aoa); 125 | _mav_put_float(buf, 8, slip); 126 | 127 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, buf, 12, 32); 128 | #else 129 | mavlink_obs_air_velocity_t packet; 130 | packet.magnitude = magnitude; 131 | packet.aoa = aoa; 132 | packet.slip = slip; 133 | 134 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, (const char *)&packet, 12, 32); 135 | #endif 136 | } 137 | 138 | #endif 139 | 140 | // MESSAGE OBS_AIR_VELOCITY UNPACKING 141 | 142 | 143 | /** 144 | * @brief Get field magnitude from obs_air_velocity message 145 | * 146 | * @return Air speed 147 | */ 148 | static inline float mavlink_msg_obs_air_velocity_get_magnitude(const mavlink_message_t* msg) 149 | { 150 | return _MAV_RETURN_float(msg, 0); 151 | } 152 | 153 | /** 154 | * @brief Get field aoa from obs_air_velocity message 155 | * 156 | * @return angle of attack 157 | */ 158 | static inline float mavlink_msg_obs_air_velocity_get_aoa(const mavlink_message_t* msg) 159 | { 160 | return _MAV_RETURN_float(msg, 4); 161 | } 162 | 163 | /** 164 | * @brief Get field slip from obs_air_velocity message 165 | * 166 | * @return slip angle 167 | */ 168 | static inline float mavlink_msg_obs_air_velocity_get_slip(const mavlink_message_t* msg) 169 | { 170 | return _MAV_RETURN_float(msg, 8); 171 | } 172 | 173 | /** 174 | * @brief Decode a obs_air_velocity message into a struct 175 | * 176 | * @param msg The message to decode 177 | * @param obs_air_velocity C-struct to decode the message contents into 178 | */ 179 | static inline void mavlink_msg_obs_air_velocity_decode(const mavlink_message_t* msg, mavlink_obs_air_velocity_t* obs_air_velocity) 180 | { 181 | #if MAVLINK_NEED_BYTE_SWAP 182 | obs_air_velocity->magnitude = mavlink_msg_obs_air_velocity_get_magnitude(msg); 183 | obs_air_velocity->aoa = mavlink_msg_obs_air_velocity_get_aoa(msg); 184 | obs_air_velocity->slip = mavlink_msg_obs_air_velocity_get_slip(msg); 185 | #else 186 | memcpy(obs_air_velocity, _MAV_PAYLOAD(msg), 12); 187 | #endif 188 | } 189 | -------------------------------------------------------------------------------- /libraries/mavlink/include/sensesoar/mavlink_msg_obs_attitude.h: -------------------------------------------------------------------------------- 1 | // MESSAGE OBS_ATTITUDE PACKING 2 | 3 | #define MAVLINK_MSG_ID_OBS_ATTITUDE 174 4 | 5 | typedef struct __mavlink_obs_attitude_t 6 | { 7 | double quat[4]; ///< Quaternion re;im 8 | } mavlink_obs_attitude_t; 9 | 10 | #define MAVLINK_MSG_ID_OBS_ATTITUDE_LEN 32 11 | #define MAVLINK_MSG_ID_174_LEN 32 12 | 13 | #define MAVLINK_MSG_OBS_ATTITUDE_FIELD_QUAT_LEN 4 14 | 15 | #define MAVLINK_MESSAGE_INFO_OBS_ATTITUDE { \ 16 | "OBS_ATTITUDE", \ 17 | 1, \ 18 | { { "quat", NULL, MAVLINK_TYPE_DOUBLE, 4, 0, offsetof(mavlink_obs_attitude_t, quat) }, \ 19 | } \ 20 | } 21 | 22 | 23 | /** 24 | * @brief Pack a obs_attitude message 25 | * @param system_id ID of this system 26 | * @param component_id ID of this component (e.g. 200 for IMU) 27 | * @param msg The MAVLink message to compress the data into 28 | * 29 | * @param quat Quaternion re;im 30 | * @return length of the message in bytes (excluding serial stream start sign) 31 | */ 32 | static inline uint16_t mavlink_msg_obs_attitude_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 33 | const double *quat) 34 | { 35 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 36 | char buf[32]; 37 | 38 | _mav_put_double_array(buf, 0, quat, 4); 39 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32); 40 | #else 41 | mavlink_obs_attitude_t packet; 42 | 43 | mav_array_memcpy(packet.quat, quat, sizeof(double)*4); 44 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 32); 45 | #endif 46 | 47 | msg->msgid = MAVLINK_MSG_ID_OBS_ATTITUDE; 48 | return mavlink_finalize_message(msg, system_id, component_id, 32, 146); 49 | } 50 | 51 | /** 52 | * @brief Pack a obs_attitude message on a channel 53 | * @param system_id ID of this system 54 | * @param component_id ID of this component (e.g. 200 for IMU) 55 | * @param chan The MAVLink channel this message was sent over 56 | * @param msg The MAVLink message to compress the data into 57 | * @param quat Quaternion re;im 58 | * @return length of the message in bytes (excluding serial stream start sign) 59 | */ 60 | static inline uint16_t mavlink_msg_obs_attitude_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 61 | mavlink_message_t* msg, 62 | const double *quat) 63 | { 64 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 65 | char buf[32]; 66 | 67 | _mav_put_double_array(buf, 0, quat, 4); 68 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32); 69 | #else 70 | mavlink_obs_attitude_t packet; 71 | 72 | mav_array_memcpy(packet.quat, quat, sizeof(double)*4); 73 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 32); 74 | #endif 75 | 76 | msg->msgid = MAVLINK_MSG_ID_OBS_ATTITUDE; 77 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 146); 78 | } 79 | 80 | /** 81 | * @brief Encode a obs_attitude struct into a message 82 | * 83 | * @param system_id ID of this system 84 | * @param component_id ID of this component (e.g. 200 for IMU) 85 | * @param msg The MAVLink message to compress the data into 86 | * @param obs_attitude C-struct to read the message contents from 87 | */ 88 | static inline uint16_t mavlink_msg_obs_attitude_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_attitude_t* obs_attitude) 89 | { 90 | return mavlink_msg_obs_attitude_pack(system_id, component_id, msg, obs_attitude->quat); 91 | } 92 | 93 | /** 94 | * @brief Send a obs_attitude message 95 | * @param chan MAVLink channel to send the message 96 | * 97 | * @param quat Quaternion re;im 98 | */ 99 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 100 | 101 | static inline void mavlink_msg_obs_attitude_send(mavlink_channel_t chan, const double *quat) 102 | { 103 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 104 | char buf[32]; 105 | 106 | _mav_put_double_array(buf, 0, quat, 4); 107 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, buf, 32, 146); 108 | #else 109 | mavlink_obs_attitude_t packet; 110 | 111 | mav_array_memcpy(packet.quat, quat, sizeof(double)*4); 112 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, (const char *)&packet, 32, 146); 113 | #endif 114 | } 115 | 116 | #endif 117 | 118 | // MESSAGE OBS_ATTITUDE UNPACKING 119 | 120 | 121 | /** 122 | * @brief Get field quat from obs_attitude message 123 | * 124 | * @return Quaternion re;im 125 | */ 126 | static inline uint16_t mavlink_msg_obs_attitude_get_quat(const mavlink_message_t* msg, double *quat) 127 | { 128 | return _MAV_RETURN_double_array(msg, quat, 4, 0); 129 | } 130 | 131 | /** 132 | * @brief Decode a obs_attitude message into a struct 133 | * 134 | * @param msg The message to decode 135 | * @param obs_attitude C-struct to decode the message contents into 136 | */ 137 | static inline void mavlink_msg_obs_attitude_decode(const mavlink_message_t* msg, mavlink_obs_attitude_t* obs_attitude) 138 | { 139 | #if MAVLINK_NEED_BYTE_SWAP 140 | mavlink_msg_obs_attitude_get_quat(msg, obs_attitude->quat); 141 | #else 142 | memcpy(obs_attitude, _MAV_PAYLOAD(msg), 32); 143 | #endif 144 | } 145 | -------------------------------------------------------------------------------- /libraries/mavlink/include/sensesoar/mavlink_msg_obs_bias.h: -------------------------------------------------------------------------------- 1 | // MESSAGE OBS_BIAS PACKING 2 | 3 | #define MAVLINK_MSG_ID_OBS_BIAS 180 4 | 5 | typedef struct __mavlink_obs_bias_t 6 | { 7 | float accBias[3]; ///< accelerometer bias 8 | float gyroBias[3]; ///< gyroscope bias 9 | } mavlink_obs_bias_t; 10 | 11 | #define MAVLINK_MSG_ID_OBS_BIAS_LEN 24 12 | #define MAVLINK_MSG_ID_180_LEN 24 13 | 14 | #define MAVLINK_MSG_OBS_BIAS_FIELD_ACCBIAS_LEN 3 15 | #define MAVLINK_MSG_OBS_BIAS_FIELD_GYROBIAS_LEN 3 16 | 17 | #define MAVLINK_MESSAGE_INFO_OBS_BIAS { \ 18 | "OBS_BIAS", \ 19 | 2, \ 20 | { { "accBias", NULL, MAVLINK_TYPE_FLOAT, 3, 0, offsetof(mavlink_obs_bias_t, accBias) }, \ 21 | { "gyroBias", NULL, MAVLINK_TYPE_FLOAT, 3, 12, offsetof(mavlink_obs_bias_t, gyroBias) }, \ 22 | } \ 23 | } 24 | 25 | 26 | /** 27 | * @brief Pack a obs_bias message 28 | * @param system_id ID of this system 29 | * @param component_id ID of this component (e.g. 200 for IMU) 30 | * @param msg The MAVLink message to compress the data into 31 | * 32 | * @param accBias accelerometer bias 33 | * @param gyroBias gyroscope bias 34 | * @return length of the message in bytes (excluding serial stream start sign) 35 | */ 36 | static inline uint16_t mavlink_msg_obs_bias_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 37 | const float *accBias, const float *gyroBias) 38 | { 39 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 40 | char buf[24]; 41 | 42 | _mav_put_float_array(buf, 0, accBias, 3); 43 | _mav_put_float_array(buf, 12, gyroBias, 3); 44 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 24); 45 | #else 46 | mavlink_obs_bias_t packet; 47 | 48 | mav_array_memcpy(packet.accBias, accBias, sizeof(float)*3); 49 | mav_array_memcpy(packet.gyroBias, gyroBias, sizeof(float)*3); 50 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 24); 51 | #endif 52 | 53 | msg->msgid = MAVLINK_MSG_ID_OBS_BIAS; 54 | return mavlink_finalize_message(msg, system_id, component_id, 24, 159); 55 | } 56 | 57 | /** 58 | * @brief Pack a obs_bias message on a channel 59 | * @param system_id ID of this system 60 | * @param component_id ID of this component (e.g. 200 for IMU) 61 | * @param chan The MAVLink channel this message was sent over 62 | * @param msg The MAVLink message to compress the data into 63 | * @param accBias accelerometer bias 64 | * @param gyroBias gyroscope bias 65 | * @return length of the message in bytes (excluding serial stream start sign) 66 | */ 67 | static inline uint16_t mavlink_msg_obs_bias_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 68 | mavlink_message_t* msg, 69 | const float *accBias,const float *gyroBias) 70 | { 71 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 72 | char buf[24]; 73 | 74 | _mav_put_float_array(buf, 0, accBias, 3); 75 | _mav_put_float_array(buf, 12, gyroBias, 3); 76 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 24); 77 | #else 78 | mavlink_obs_bias_t packet; 79 | 80 | mav_array_memcpy(packet.accBias, accBias, sizeof(float)*3); 81 | mav_array_memcpy(packet.gyroBias, gyroBias, sizeof(float)*3); 82 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 24); 83 | #endif 84 | 85 | msg->msgid = MAVLINK_MSG_ID_OBS_BIAS; 86 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 24, 159); 87 | } 88 | 89 | /** 90 | * @brief Encode a obs_bias struct into a message 91 | * 92 | * @param system_id ID of this system 93 | * @param component_id ID of this component (e.g. 200 for IMU) 94 | * @param msg The MAVLink message to compress the data into 95 | * @param obs_bias C-struct to read the message contents from 96 | */ 97 | static inline uint16_t mavlink_msg_obs_bias_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_bias_t* obs_bias) 98 | { 99 | return mavlink_msg_obs_bias_pack(system_id, component_id, msg, obs_bias->accBias, obs_bias->gyroBias); 100 | } 101 | 102 | /** 103 | * @brief Send a obs_bias message 104 | * @param chan MAVLink channel to send the message 105 | * 106 | * @param accBias accelerometer bias 107 | * @param gyroBias gyroscope bias 108 | */ 109 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 110 | 111 | static inline void mavlink_msg_obs_bias_send(mavlink_channel_t chan, const float *accBias, const float *gyroBias) 112 | { 113 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 114 | char buf[24]; 115 | 116 | _mav_put_float_array(buf, 0, accBias, 3); 117 | _mav_put_float_array(buf, 12, gyroBias, 3); 118 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, buf, 24, 159); 119 | #else 120 | mavlink_obs_bias_t packet; 121 | 122 | mav_array_memcpy(packet.accBias, accBias, sizeof(float)*3); 123 | mav_array_memcpy(packet.gyroBias, gyroBias, sizeof(float)*3); 124 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, (const char *)&packet, 24, 159); 125 | #endif 126 | } 127 | 128 | #endif 129 | 130 | // MESSAGE OBS_BIAS UNPACKING 131 | 132 | 133 | /** 134 | * @brief Get field accBias from obs_bias message 135 | * 136 | * @return accelerometer bias 137 | */ 138 | static inline uint16_t mavlink_msg_obs_bias_get_accBias(const mavlink_message_t* msg, float *accBias) 139 | { 140 | return _MAV_RETURN_float_array(msg, accBias, 3, 0); 141 | } 142 | 143 | /** 144 | * @brief Get field gyroBias from obs_bias message 145 | * 146 | * @return gyroscope bias 147 | */ 148 | static inline uint16_t mavlink_msg_obs_bias_get_gyroBias(const mavlink_message_t* msg, float *gyroBias) 149 | { 150 | return _MAV_RETURN_float_array(msg, gyroBias, 3, 12); 151 | } 152 | 153 | /** 154 | * @brief Decode a obs_bias message into a struct 155 | * 156 | * @param msg The message to decode 157 | * @param obs_bias C-struct to decode the message contents into 158 | */ 159 | static inline void mavlink_msg_obs_bias_decode(const mavlink_message_t* msg, mavlink_obs_bias_t* obs_bias) 160 | { 161 | #if MAVLINK_NEED_BYTE_SWAP 162 | mavlink_msg_obs_bias_get_accBias(msg, obs_bias->accBias); 163 | mavlink_msg_obs_bias_get_gyroBias(msg, obs_bias->gyroBias); 164 | #else 165 | memcpy(obs_bias, _MAV_PAYLOAD(msg), 24); 166 | #endif 167 | } 168 | -------------------------------------------------------------------------------- /libraries/mavlink/include/sensesoar/mavlink_msg_obs_position.h: -------------------------------------------------------------------------------- 1 | // MESSAGE OBS_POSITION PACKING 2 | 3 | #define MAVLINK_MSG_ID_OBS_POSITION 170 4 | 5 | typedef struct __mavlink_obs_position_t 6 | { 7 | int32_t lon; ///< Longitude expressed in 1E7 8 | int32_t lat; ///< Latitude expressed in 1E7 9 | int32_t alt; ///< Altitude expressed in milimeters 10 | } mavlink_obs_position_t; 11 | 12 | #define MAVLINK_MSG_ID_OBS_POSITION_LEN 12 13 | #define MAVLINK_MSG_ID_170_LEN 12 14 | 15 | 16 | 17 | #define MAVLINK_MESSAGE_INFO_OBS_POSITION { \ 18 | "OBS_POSITION", \ 19 | 3, \ 20 | { { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_obs_position_t, lon) }, \ 21 | { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_obs_position_t, lat) }, \ 22 | { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_obs_position_t, alt) }, \ 23 | } \ 24 | } 25 | 26 | 27 | /** 28 | * @brief Pack a obs_position message 29 | * @param system_id ID of this system 30 | * @param component_id ID of this component (e.g. 200 for IMU) 31 | * @param msg The MAVLink message to compress the data into 32 | * 33 | * @param lon Longitude expressed in 1E7 34 | * @param lat Latitude expressed in 1E7 35 | * @param alt Altitude expressed in milimeters 36 | * @return length of the message in bytes (excluding serial stream start sign) 37 | */ 38 | static inline uint16_t mavlink_msg_obs_position_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 39 | int32_t lon, int32_t lat, int32_t alt) 40 | { 41 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 42 | char buf[12]; 43 | _mav_put_int32_t(buf, 0, lon); 44 | _mav_put_int32_t(buf, 4, lat); 45 | _mav_put_int32_t(buf, 8, alt); 46 | 47 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); 48 | #else 49 | mavlink_obs_position_t packet; 50 | packet.lon = lon; 51 | packet.lat = lat; 52 | packet.alt = alt; 53 | 54 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); 55 | #endif 56 | 57 | msg->msgid = MAVLINK_MSG_ID_OBS_POSITION; 58 | return mavlink_finalize_message(msg, system_id, component_id, 12, 15); 59 | } 60 | 61 | /** 62 | * @brief Pack a obs_position message on a channel 63 | * @param system_id ID of this system 64 | * @param component_id ID of this component (e.g. 200 for IMU) 65 | * @param chan The MAVLink channel this message was sent over 66 | * @param msg The MAVLink message to compress the data into 67 | * @param lon Longitude expressed in 1E7 68 | * @param lat Latitude expressed in 1E7 69 | * @param alt Altitude expressed in milimeters 70 | * @return length of the message in bytes (excluding serial stream start sign) 71 | */ 72 | static inline uint16_t mavlink_msg_obs_position_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 73 | mavlink_message_t* msg, 74 | int32_t lon,int32_t lat,int32_t alt) 75 | { 76 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 77 | char buf[12]; 78 | _mav_put_int32_t(buf, 0, lon); 79 | _mav_put_int32_t(buf, 4, lat); 80 | _mav_put_int32_t(buf, 8, alt); 81 | 82 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); 83 | #else 84 | mavlink_obs_position_t packet; 85 | packet.lon = lon; 86 | packet.lat = lat; 87 | packet.alt = alt; 88 | 89 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); 90 | #endif 91 | 92 | msg->msgid = MAVLINK_MSG_ID_OBS_POSITION; 93 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 15); 94 | } 95 | 96 | /** 97 | * @brief Encode a obs_position struct into a message 98 | * 99 | * @param system_id ID of this system 100 | * @param component_id ID of this component (e.g. 200 for IMU) 101 | * @param msg The MAVLink message to compress the data into 102 | * @param obs_position C-struct to read the message contents from 103 | */ 104 | static inline uint16_t mavlink_msg_obs_position_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_position_t* obs_position) 105 | { 106 | return mavlink_msg_obs_position_pack(system_id, component_id, msg, obs_position->lon, obs_position->lat, obs_position->alt); 107 | } 108 | 109 | /** 110 | * @brief Send a obs_position message 111 | * @param chan MAVLink channel to send the message 112 | * 113 | * @param lon Longitude expressed in 1E7 114 | * @param lat Latitude expressed in 1E7 115 | * @param alt Altitude expressed in milimeters 116 | */ 117 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 118 | 119 | static inline void mavlink_msg_obs_position_send(mavlink_channel_t chan, int32_t lon, int32_t lat, int32_t alt) 120 | { 121 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 122 | char buf[12]; 123 | _mav_put_int32_t(buf, 0, lon); 124 | _mav_put_int32_t(buf, 4, lat); 125 | _mav_put_int32_t(buf, 8, alt); 126 | 127 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, buf, 12, 15); 128 | #else 129 | mavlink_obs_position_t packet; 130 | packet.lon = lon; 131 | packet.lat = lat; 132 | packet.alt = alt; 133 | 134 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, (const char *)&packet, 12, 15); 135 | #endif 136 | } 137 | 138 | #endif 139 | 140 | // MESSAGE OBS_POSITION UNPACKING 141 | 142 | 143 | /** 144 | * @brief Get field lon from obs_position message 145 | * 146 | * @return Longitude expressed in 1E7 147 | */ 148 | static inline int32_t mavlink_msg_obs_position_get_lon(const mavlink_message_t* msg) 149 | { 150 | return _MAV_RETURN_int32_t(msg, 0); 151 | } 152 | 153 | /** 154 | * @brief Get field lat from obs_position message 155 | * 156 | * @return Latitude expressed in 1E7 157 | */ 158 | static inline int32_t mavlink_msg_obs_position_get_lat(const mavlink_message_t* msg) 159 | { 160 | return _MAV_RETURN_int32_t(msg, 4); 161 | } 162 | 163 | /** 164 | * @brief Get field alt from obs_position message 165 | * 166 | * @return Altitude expressed in milimeters 167 | */ 168 | static inline int32_t mavlink_msg_obs_position_get_alt(const mavlink_message_t* msg) 169 | { 170 | return _MAV_RETURN_int32_t(msg, 8); 171 | } 172 | 173 | /** 174 | * @brief Decode a obs_position message into a struct 175 | * 176 | * @param msg The message to decode 177 | * @param obs_position C-struct to decode the message contents into 178 | */ 179 | static inline void mavlink_msg_obs_position_decode(const mavlink_message_t* msg, mavlink_obs_position_t* obs_position) 180 | { 181 | #if MAVLINK_NEED_BYTE_SWAP 182 | obs_position->lon = mavlink_msg_obs_position_get_lon(msg); 183 | obs_position->lat = mavlink_msg_obs_position_get_lat(msg); 184 | obs_position->alt = mavlink_msg_obs_position_get_alt(msg); 185 | #else 186 | memcpy(obs_position, _MAV_PAYLOAD(msg), 12); 187 | #endif 188 | } 189 | -------------------------------------------------------------------------------- /libraries/mavlink/include/sensesoar/mavlink_msg_obs_qff.h: -------------------------------------------------------------------------------- 1 | // MESSAGE OBS_QFF PACKING 2 | 3 | #define MAVLINK_MSG_ID_OBS_QFF 182 4 | 5 | typedef struct __mavlink_obs_qff_t 6 | { 7 | float qff; ///< Wind 8 | } mavlink_obs_qff_t; 9 | 10 | #define MAVLINK_MSG_ID_OBS_QFF_LEN 4 11 | #define MAVLINK_MSG_ID_182_LEN 4 12 | 13 | 14 | 15 | #define MAVLINK_MESSAGE_INFO_OBS_QFF { \ 16 | "OBS_QFF", \ 17 | 1, \ 18 | { { "qff", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_obs_qff_t, qff) }, \ 19 | } \ 20 | } 21 | 22 | 23 | /** 24 | * @brief Pack a obs_qff message 25 | * @param system_id ID of this system 26 | * @param component_id ID of this component (e.g. 200 for IMU) 27 | * @param msg The MAVLink message to compress the data into 28 | * 29 | * @param qff Wind 30 | * @return length of the message in bytes (excluding serial stream start sign) 31 | */ 32 | static inline uint16_t mavlink_msg_obs_qff_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 33 | float qff) 34 | { 35 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 36 | char buf[4]; 37 | _mav_put_float(buf, 0, qff); 38 | 39 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 4); 40 | #else 41 | mavlink_obs_qff_t packet; 42 | packet.qff = qff; 43 | 44 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 4); 45 | #endif 46 | 47 | msg->msgid = MAVLINK_MSG_ID_OBS_QFF; 48 | return mavlink_finalize_message(msg, system_id, component_id, 4, 24); 49 | } 50 | 51 | /** 52 | * @brief Pack a obs_qff message on a channel 53 | * @param system_id ID of this system 54 | * @param component_id ID of this component (e.g. 200 for IMU) 55 | * @param chan The MAVLink channel this message was sent over 56 | * @param msg The MAVLink message to compress the data into 57 | * @param qff Wind 58 | * @return length of the message in bytes (excluding serial stream start sign) 59 | */ 60 | static inline uint16_t mavlink_msg_obs_qff_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 61 | mavlink_message_t* msg, 62 | float qff) 63 | { 64 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 65 | char buf[4]; 66 | _mav_put_float(buf, 0, qff); 67 | 68 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 4); 69 | #else 70 | mavlink_obs_qff_t packet; 71 | packet.qff = qff; 72 | 73 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 4); 74 | #endif 75 | 76 | msg->msgid = MAVLINK_MSG_ID_OBS_QFF; 77 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 24); 78 | } 79 | 80 | /** 81 | * @brief Encode a obs_qff struct into a message 82 | * 83 | * @param system_id ID of this system 84 | * @param component_id ID of this component (e.g. 200 for IMU) 85 | * @param msg The MAVLink message to compress the data into 86 | * @param obs_qff C-struct to read the message contents from 87 | */ 88 | static inline uint16_t mavlink_msg_obs_qff_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_qff_t* obs_qff) 89 | { 90 | return mavlink_msg_obs_qff_pack(system_id, component_id, msg, obs_qff->qff); 91 | } 92 | 93 | /** 94 | * @brief Send a obs_qff message 95 | * @param chan MAVLink channel to send the message 96 | * 97 | * @param qff Wind 98 | */ 99 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 100 | 101 | static inline void mavlink_msg_obs_qff_send(mavlink_channel_t chan, float qff) 102 | { 103 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 104 | char buf[4]; 105 | _mav_put_float(buf, 0, qff); 106 | 107 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, buf, 4, 24); 108 | #else 109 | mavlink_obs_qff_t packet; 110 | packet.qff = qff; 111 | 112 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, (const char *)&packet, 4, 24); 113 | #endif 114 | } 115 | 116 | #endif 117 | 118 | // MESSAGE OBS_QFF UNPACKING 119 | 120 | 121 | /** 122 | * @brief Get field qff from obs_qff message 123 | * 124 | * @return Wind 125 | */ 126 | static inline float mavlink_msg_obs_qff_get_qff(const mavlink_message_t* msg) 127 | { 128 | return _MAV_RETURN_float(msg, 0); 129 | } 130 | 131 | /** 132 | * @brief Decode a obs_qff message into a struct 133 | * 134 | * @param msg The message to decode 135 | * @param obs_qff C-struct to decode the message contents into 136 | */ 137 | static inline void mavlink_msg_obs_qff_decode(const mavlink_message_t* msg, mavlink_obs_qff_t* obs_qff) 138 | { 139 | #if MAVLINK_NEED_BYTE_SWAP 140 | obs_qff->qff = mavlink_msg_obs_qff_get_qff(msg); 141 | #else 142 | memcpy(obs_qff, _MAV_PAYLOAD(msg), 4); 143 | #endif 144 | } 145 | -------------------------------------------------------------------------------- /libraries/mavlink/include/sensesoar/mavlink_msg_obs_velocity.h: -------------------------------------------------------------------------------- 1 | // MESSAGE OBS_VELOCITY PACKING 2 | 3 | #define MAVLINK_MSG_ID_OBS_VELOCITY 172 4 | 5 | typedef struct __mavlink_obs_velocity_t 6 | { 7 | float vel[3]; ///< Velocity 8 | } mavlink_obs_velocity_t; 9 | 10 | #define MAVLINK_MSG_ID_OBS_VELOCITY_LEN 12 11 | #define MAVLINK_MSG_ID_172_LEN 12 12 | 13 | #define MAVLINK_MSG_OBS_VELOCITY_FIELD_VEL_LEN 3 14 | 15 | #define MAVLINK_MESSAGE_INFO_OBS_VELOCITY { \ 16 | "OBS_VELOCITY", \ 17 | 1, \ 18 | { { "vel", NULL, MAVLINK_TYPE_FLOAT, 3, 0, offsetof(mavlink_obs_velocity_t, vel) }, \ 19 | } \ 20 | } 21 | 22 | 23 | /** 24 | * @brief Pack a obs_velocity message 25 | * @param system_id ID of this system 26 | * @param component_id ID of this component (e.g. 200 for IMU) 27 | * @param msg The MAVLink message to compress the data into 28 | * 29 | * @param vel Velocity 30 | * @return length of the message in bytes (excluding serial stream start sign) 31 | */ 32 | static inline uint16_t mavlink_msg_obs_velocity_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 33 | const float *vel) 34 | { 35 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 36 | char buf[12]; 37 | 38 | _mav_put_float_array(buf, 0, vel, 3); 39 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); 40 | #else 41 | mavlink_obs_velocity_t packet; 42 | 43 | mav_array_memcpy(packet.vel, vel, sizeof(float)*3); 44 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); 45 | #endif 46 | 47 | msg->msgid = MAVLINK_MSG_ID_OBS_VELOCITY; 48 | return mavlink_finalize_message(msg, system_id, component_id, 12, 108); 49 | } 50 | 51 | /** 52 | * @brief Pack a obs_velocity message on a channel 53 | * @param system_id ID of this system 54 | * @param component_id ID of this component (e.g. 200 for IMU) 55 | * @param chan The MAVLink channel this message was sent over 56 | * @param msg The MAVLink message to compress the data into 57 | * @param vel Velocity 58 | * @return length of the message in bytes (excluding serial stream start sign) 59 | */ 60 | static inline uint16_t mavlink_msg_obs_velocity_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 61 | mavlink_message_t* msg, 62 | const float *vel) 63 | { 64 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 65 | char buf[12]; 66 | 67 | _mav_put_float_array(buf, 0, vel, 3); 68 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); 69 | #else 70 | mavlink_obs_velocity_t packet; 71 | 72 | mav_array_memcpy(packet.vel, vel, sizeof(float)*3); 73 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); 74 | #endif 75 | 76 | msg->msgid = MAVLINK_MSG_ID_OBS_VELOCITY; 77 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 108); 78 | } 79 | 80 | /** 81 | * @brief Encode a obs_velocity struct into a message 82 | * 83 | * @param system_id ID of this system 84 | * @param component_id ID of this component (e.g. 200 for IMU) 85 | * @param msg The MAVLink message to compress the data into 86 | * @param obs_velocity C-struct to read the message contents from 87 | */ 88 | static inline uint16_t mavlink_msg_obs_velocity_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_velocity_t* obs_velocity) 89 | { 90 | return mavlink_msg_obs_velocity_pack(system_id, component_id, msg, obs_velocity->vel); 91 | } 92 | 93 | /** 94 | * @brief Send a obs_velocity message 95 | * @param chan MAVLink channel to send the message 96 | * 97 | * @param vel Velocity 98 | */ 99 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 100 | 101 | static inline void mavlink_msg_obs_velocity_send(mavlink_channel_t chan, const float *vel) 102 | { 103 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 104 | char buf[12]; 105 | 106 | _mav_put_float_array(buf, 0, vel, 3); 107 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, buf, 12, 108); 108 | #else 109 | mavlink_obs_velocity_t packet; 110 | 111 | mav_array_memcpy(packet.vel, vel, sizeof(float)*3); 112 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, (const char *)&packet, 12, 108); 113 | #endif 114 | } 115 | 116 | #endif 117 | 118 | // MESSAGE OBS_VELOCITY UNPACKING 119 | 120 | 121 | /** 122 | * @brief Get field vel from obs_velocity message 123 | * 124 | * @return Velocity 125 | */ 126 | static inline uint16_t mavlink_msg_obs_velocity_get_vel(const mavlink_message_t* msg, float *vel) 127 | { 128 | return _MAV_RETURN_float_array(msg, vel, 3, 0); 129 | } 130 | 131 | /** 132 | * @brief Decode a obs_velocity message into a struct 133 | * 134 | * @param msg The message to decode 135 | * @param obs_velocity C-struct to decode the message contents into 136 | */ 137 | static inline void mavlink_msg_obs_velocity_decode(const mavlink_message_t* msg, mavlink_obs_velocity_t* obs_velocity) 138 | { 139 | #if MAVLINK_NEED_BYTE_SWAP 140 | mavlink_msg_obs_velocity_get_vel(msg, obs_velocity->vel); 141 | #else 142 | memcpy(obs_velocity, _MAV_PAYLOAD(msg), 12); 143 | #endif 144 | } 145 | -------------------------------------------------------------------------------- /libraries/mavlink/include/sensesoar/mavlink_msg_obs_wind.h: -------------------------------------------------------------------------------- 1 | // MESSAGE OBS_WIND PACKING 2 | 3 | #define MAVLINK_MSG_ID_OBS_WIND 176 4 | 5 | typedef struct __mavlink_obs_wind_t 6 | { 7 | float wind[3]; ///< Wind 8 | } mavlink_obs_wind_t; 9 | 10 | #define MAVLINK_MSG_ID_OBS_WIND_LEN 12 11 | #define MAVLINK_MSG_ID_176_LEN 12 12 | 13 | #define MAVLINK_MSG_OBS_WIND_FIELD_WIND_LEN 3 14 | 15 | #define MAVLINK_MESSAGE_INFO_OBS_WIND { \ 16 | "OBS_WIND", \ 17 | 1, \ 18 | { { "wind", NULL, MAVLINK_TYPE_FLOAT, 3, 0, offsetof(mavlink_obs_wind_t, wind) }, \ 19 | } \ 20 | } 21 | 22 | 23 | /** 24 | * @brief Pack a obs_wind message 25 | * @param system_id ID of this system 26 | * @param component_id ID of this component (e.g. 200 for IMU) 27 | * @param msg The MAVLink message to compress the data into 28 | * 29 | * @param wind Wind 30 | * @return length of the message in bytes (excluding serial stream start sign) 31 | */ 32 | static inline uint16_t mavlink_msg_obs_wind_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 33 | const float *wind) 34 | { 35 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 36 | char buf[12]; 37 | 38 | _mav_put_float_array(buf, 0, wind, 3); 39 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); 40 | #else 41 | mavlink_obs_wind_t packet; 42 | 43 | mav_array_memcpy(packet.wind, wind, sizeof(float)*3); 44 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); 45 | #endif 46 | 47 | msg->msgid = MAVLINK_MSG_ID_OBS_WIND; 48 | return mavlink_finalize_message(msg, system_id, component_id, 12, 16); 49 | } 50 | 51 | /** 52 | * @brief Pack a obs_wind message on a channel 53 | * @param system_id ID of this system 54 | * @param component_id ID of this component (e.g. 200 for IMU) 55 | * @param chan The MAVLink channel this message was sent over 56 | * @param msg The MAVLink message to compress the data into 57 | * @param wind Wind 58 | * @return length of the message in bytes (excluding serial stream start sign) 59 | */ 60 | static inline uint16_t mavlink_msg_obs_wind_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 61 | mavlink_message_t* msg, 62 | const float *wind) 63 | { 64 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 65 | char buf[12]; 66 | 67 | _mav_put_float_array(buf, 0, wind, 3); 68 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); 69 | #else 70 | mavlink_obs_wind_t packet; 71 | 72 | mav_array_memcpy(packet.wind, wind, sizeof(float)*3); 73 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); 74 | #endif 75 | 76 | msg->msgid = MAVLINK_MSG_ID_OBS_WIND; 77 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 16); 78 | } 79 | 80 | /** 81 | * @brief Encode a obs_wind struct into a message 82 | * 83 | * @param system_id ID of this system 84 | * @param component_id ID of this component (e.g. 200 for IMU) 85 | * @param msg The MAVLink message to compress the data into 86 | * @param obs_wind C-struct to read the message contents from 87 | */ 88 | static inline uint16_t mavlink_msg_obs_wind_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_wind_t* obs_wind) 89 | { 90 | return mavlink_msg_obs_wind_pack(system_id, component_id, msg, obs_wind->wind); 91 | } 92 | 93 | /** 94 | * @brief Send a obs_wind message 95 | * @param chan MAVLink channel to send the message 96 | * 97 | * @param wind Wind 98 | */ 99 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 100 | 101 | static inline void mavlink_msg_obs_wind_send(mavlink_channel_t chan, const float *wind) 102 | { 103 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 104 | char buf[12]; 105 | 106 | _mav_put_float_array(buf, 0, wind, 3); 107 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, buf, 12, 16); 108 | #else 109 | mavlink_obs_wind_t packet; 110 | 111 | mav_array_memcpy(packet.wind, wind, sizeof(float)*3); 112 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, (const char *)&packet, 12, 16); 113 | #endif 114 | } 115 | 116 | #endif 117 | 118 | // MESSAGE OBS_WIND UNPACKING 119 | 120 | 121 | /** 122 | * @brief Get field wind from obs_wind message 123 | * 124 | * @return Wind 125 | */ 126 | static inline uint16_t mavlink_msg_obs_wind_get_wind(const mavlink_message_t* msg, float *wind) 127 | { 128 | return _MAV_RETURN_float_array(msg, wind, 3, 0); 129 | } 130 | 131 | /** 132 | * @brief Decode a obs_wind message into a struct 133 | * 134 | * @param msg The message to decode 135 | * @param obs_wind C-struct to decode the message contents into 136 | */ 137 | static inline void mavlink_msg_obs_wind_decode(const mavlink_message_t* msg, mavlink_obs_wind_t* obs_wind) 138 | { 139 | #if MAVLINK_NEED_BYTE_SWAP 140 | mavlink_msg_obs_wind_get_wind(msg, obs_wind->wind); 141 | #else 142 | memcpy(obs_wind, _MAV_PAYLOAD(msg), 12); 143 | #endif 144 | } 145 | -------------------------------------------------------------------------------- /libraries/mavlink/include/sensesoar/mavlink_msg_pm_elec.h: -------------------------------------------------------------------------------- 1 | // MESSAGE PM_ELEC PACKING 2 | 3 | #define MAVLINK_MSG_ID_PM_ELEC 188 4 | 5 | typedef struct __mavlink_pm_elec_t 6 | { 7 | float PwCons; ///< current power consumption 8 | float BatStat; ///< battery status 9 | float PwGen[3]; ///< Power generation from each module 10 | } mavlink_pm_elec_t; 11 | 12 | #define MAVLINK_MSG_ID_PM_ELEC_LEN 20 13 | #define MAVLINK_MSG_ID_188_LEN 20 14 | 15 | #define MAVLINK_MSG_PM_ELEC_FIELD_PWGEN_LEN 3 16 | 17 | #define MAVLINK_MESSAGE_INFO_PM_ELEC { \ 18 | "PM_ELEC", \ 19 | 3, \ 20 | { { "PwCons", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_pm_elec_t, PwCons) }, \ 21 | { "BatStat", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_pm_elec_t, BatStat) }, \ 22 | { "PwGen", NULL, MAVLINK_TYPE_FLOAT, 3, 8, offsetof(mavlink_pm_elec_t, PwGen) }, \ 23 | } \ 24 | } 25 | 26 | 27 | /** 28 | * @brief Pack a pm_elec message 29 | * @param system_id ID of this system 30 | * @param component_id ID of this component (e.g. 200 for IMU) 31 | * @param msg The MAVLink message to compress the data into 32 | * 33 | * @param PwCons current power consumption 34 | * @param BatStat battery status 35 | * @param PwGen Power generation from each module 36 | * @return length of the message in bytes (excluding serial stream start sign) 37 | */ 38 | static inline uint16_t mavlink_msg_pm_elec_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 39 | float PwCons, float BatStat, const float *PwGen) 40 | { 41 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 42 | char buf[20]; 43 | _mav_put_float(buf, 0, PwCons); 44 | _mav_put_float(buf, 4, BatStat); 45 | _mav_put_float_array(buf, 8, PwGen, 3); 46 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 20); 47 | #else 48 | mavlink_pm_elec_t packet; 49 | packet.PwCons = PwCons; 50 | packet.BatStat = BatStat; 51 | mav_array_memcpy(packet.PwGen, PwGen, sizeof(float)*3); 52 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 20); 53 | #endif 54 | 55 | msg->msgid = MAVLINK_MSG_ID_PM_ELEC; 56 | return mavlink_finalize_message(msg, system_id, component_id, 20, 170); 57 | } 58 | 59 | /** 60 | * @brief Pack a pm_elec message on a channel 61 | * @param system_id ID of this system 62 | * @param component_id ID of this component (e.g. 200 for IMU) 63 | * @param chan The MAVLink channel this message was sent over 64 | * @param msg The MAVLink message to compress the data into 65 | * @param PwCons current power consumption 66 | * @param BatStat battery status 67 | * @param PwGen Power generation from each module 68 | * @return length of the message in bytes (excluding serial stream start sign) 69 | */ 70 | static inline uint16_t mavlink_msg_pm_elec_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 71 | mavlink_message_t* msg, 72 | float PwCons,float BatStat,const float *PwGen) 73 | { 74 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 75 | char buf[20]; 76 | _mav_put_float(buf, 0, PwCons); 77 | _mav_put_float(buf, 4, BatStat); 78 | _mav_put_float_array(buf, 8, PwGen, 3); 79 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 20); 80 | #else 81 | mavlink_pm_elec_t packet; 82 | packet.PwCons = PwCons; 83 | packet.BatStat = BatStat; 84 | mav_array_memcpy(packet.PwGen, PwGen, sizeof(float)*3); 85 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 20); 86 | #endif 87 | 88 | msg->msgid = MAVLINK_MSG_ID_PM_ELEC; 89 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 170); 90 | } 91 | 92 | /** 93 | * @brief Encode a pm_elec struct into a message 94 | * 95 | * @param system_id ID of this system 96 | * @param component_id ID of this component (e.g. 200 for IMU) 97 | * @param msg The MAVLink message to compress the data into 98 | * @param pm_elec C-struct to read the message contents from 99 | */ 100 | static inline uint16_t mavlink_msg_pm_elec_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_pm_elec_t* pm_elec) 101 | { 102 | return mavlink_msg_pm_elec_pack(system_id, component_id, msg, pm_elec->PwCons, pm_elec->BatStat, pm_elec->PwGen); 103 | } 104 | 105 | /** 106 | * @brief Send a pm_elec message 107 | * @param chan MAVLink channel to send the message 108 | * 109 | * @param PwCons current power consumption 110 | * @param BatStat battery status 111 | * @param PwGen Power generation from each module 112 | */ 113 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 114 | 115 | static inline void mavlink_msg_pm_elec_send(mavlink_channel_t chan, float PwCons, float BatStat, const float *PwGen) 116 | { 117 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 118 | char buf[20]; 119 | _mav_put_float(buf, 0, PwCons); 120 | _mav_put_float(buf, 4, BatStat); 121 | _mav_put_float_array(buf, 8, PwGen, 3); 122 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, buf, 20, 170); 123 | #else 124 | mavlink_pm_elec_t packet; 125 | packet.PwCons = PwCons; 126 | packet.BatStat = BatStat; 127 | mav_array_memcpy(packet.PwGen, PwGen, sizeof(float)*3); 128 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, (const char *)&packet, 20, 170); 129 | #endif 130 | } 131 | 132 | #endif 133 | 134 | // MESSAGE PM_ELEC UNPACKING 135 | 136 | 137 | /** 138 | * @brief Get field PwCons from pm_elec message 139 | * 140 | * @return current power consumption 141 | */ 142 | static inline float mavlink_msg_pm_elec_get_PwCons(const mavlink_message_t* msg) 143 | { 144 | return _MAV_RETURN_float(msg, 0); 145 | } 146 | 147 | /** 148 | * @brief Get field BatStat from pm_elec message 149 | * 150 | * @return battery status 151 | */ 152 | static inline float mavlink_msg_pm_elec_get_BatStat(const mavlink_message_t* msg) 153 | { 154 | return _MAV_RETURN_float(msg, 4); 155 | } 156 | 157 | /** 158 | * @brief Get field PwGen from pm_elec message 159 | * 160 | * @return Power generation from each module 161 | */ 162 | static inline uint16_t mavlink_msg_pm_elec_get_PwGen(const mavlink_message_t* msg, float *PwGen) 163 | { 164 | return _MAV_RETURN_float_array(msg, PwGen, 3, 8); 165 | } 166 | 167 | /** 168 | * @brief Decode a pm_elec message into a struct 169 | * 170 | * @param msg The message to decode 171 | * @param pm_elec C-struct to decode the message contents into 172 | */ 173 | static inline void mavlink_msg_pm_elec_decode(const mavlink_message_t* msg, mavlink_pm_elec_t* pm_elec) 174 | { 175 | #if MAVLINK_NEED_BYTE_SWAP 176 | pm_elec->PwCons = mavlink_msg_pm_elec_get_PwCons(msg); 177 | pm_elec->BatStat = mavlink_msg_pm_elec_get_BatStat(msg); 178 | mavlink_msg_pm_elec_get_PwGen(msg, pm_elec->PwGen); 179 | #else 180 | memcpy(pm_elec, _MAV_PAYLOAD(msg), 20); 181 | #endif 182 | } 183 | -------------------------------------------------------------------------------- /libraries/mavlink/include/sensesoar/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from sensesoar.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_VERSION_H 6 | #define MAVLINK_VERSION_H 7 | 8 | #define MAVLINK_BUILD_DATE "Fri Apr 20 12:22:51 2012" 9 | #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" 10 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101 11 | 12 | #endif // MAVLINK_VERSION_H 13 | -------------------------------------------------------------------------------- /libraries/mavlink/include/slugs/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from slugs.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_H 6 | #define MAVLINK_H 7 | 8 | #ifndef MAVLINK_STX 9 | #define MAVLINK_STX 254 10 | #endif 11 | 12 | #ifndef MAVLINK_ENDIAN 13 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 14 | #endif 15 | 16 | #ifndef MAVLINK_ALIGNED_FIELDS 17 | #define MAVLINK_ALIGNED_FIELDS 1 18 | #endif 19 | 20 | #ifndef MAVLINK_CRC_EXTRA 21 | #define MAVLINK_CRC_EXTRA 1 22 | #endif 23 | 24 | #include "version.h" 25 | #include "slugs.h" 26 | 27 | #endif // MAVLINK_H 28 | -------------------------------------------------------------------------------- /libraries/mavlink/include/slugs/mavlink_msg_cpu_load.h: -------------------------------------------------------------------------------- 1 | // MESSAGE CPU_LOAD PACKING 2 | 3 | #define MAVLINK_MSG_ID_CPU_LOAD 170 4 | 5 | typedef struct __mavlink_cpu_load_t 6 | { 7 | uint16_t batVolt; ///< Battery Voltage in millivolts 8 | uint8_t sensLoad; ///< Sensor DSC Load 9 | uint8_t ctrlLoad; ///< Control DSC Load 10 | } mavlink_cpu_load_t; 11 | 12 | #define MAVLINK_MSG_ID_CPU_LOAD_LEN 4 13 | #define MAVLINK_MSG_ID_170_LEN 4 14 | 15 | 16 | 17 | #define MAVLINK_MESSAGE_INFO_CPU_LOAD { \ 18 | "CPU_LOAD", \ 19 | 3, \ 20 | { { "batVolt", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_cpu_load_t, batVolt) }, \ 21 | { "sensLoad", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_cpu_load_t, sensLoad) }, \ 22 | { "ctrlLoad", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_cpu_load_t, ctrlLoad) }, \ 23 | } \ 24 | } 25 | 26 | 27 | /** 28 | * @brief Pack a cpu_load message 29 | * @param system_id ID of this system 30 | * @param component_id ID of this component (e.g. 200 for IMU) 31 | * @param msg The MAVLink message to compress the data into 32 | * 33 | * @param sensLoad Sensor DSC Load 34 | * @param ctrlLoad Control DSC Load 35 | * @param batVolt Battery Voltage in millivolts 36 | * @return length of the message in bytes (excluding serial stream start sign) 37 | */ 38 | static inline uint16_t mavlink_msg_cpu_load_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 39 | uint8_t sensLoad, uint8_t ctrlLoad, uint16_t batVolt) 40 | { 41 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 42 | char buf[4]; 43 | _mav_put_uint16_t(buf, 0, batVolt); 44 | _mav_put_uint8_t(buf, 2, sensLoad); 45 | _mav_put_uint8_t(buf, 3, ctrlLoad); 46 | 47 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 4); 48 | #else 49 | mavlink_cpu_load_t packet; 50 | packet.batVolt = batVolt; 51 | packet.sensLoad = sensLoad; 52 | packet.ctrlLoad = ctrlLoad; 53 | 54 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 4); 55 | #endif 56 | 57 | msg->msgid = MAVLINK_MSG_ID_CPU_LOAD; 58 | return mavlink_finalize_message(msg, system_id, component_id, 4, 75); 59 | } 60 | 61 | /** 62 | * @brief Pack a cpu_load message on a channel 63 | * @param system_id ID of this system 64 | * @param component_id ID of this component (e.g. 200 for IMU) 65 | * @param chan The MAVLink channel this message was sent over 66 | * @param msg The MAVLink message to compress the data into 67 | * @param sensLoad Sensor DSC Load 68 | * @param ctrlLoad Control DSC Load 69 | * @param batVolt Battery Voltage in millivolts 70 | * @return length of the message in bytes (excluding serial stream start sign) 71 | */ 72 | static inline uint16_t mavlink_msg_cpu_load_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 73 | mavlink_message_t* msg, 74 | uint8_t sensLoad,uint8_t ctrlLoad,uint16_t batVolt) 75 | { 76 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 77 | char buf[4]; 78 | _mav_put_uint16_t(buf, 0, batVolt); 79 | _mav_put_uint8_t(buf, 2, sensLoad); 80 | _mav_put_uint8_t(buf, 3, ctrlLoad); 81 | 82 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 4); 83 | #else 84 | mavlink_cpu_load_t packet; 85 | packet.batVolt = batVolt; 86 | packet.sensLoad = sensLoad; 87 | packet.ctrlLoad = ctrlLoad; 88 | 89 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 4); 90 | #endif 91 | 92 | msg->msgid = MAVLINK_MSG_ID_CPU_LOAD; 93 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 75); 94 | } 95 | 96 | /** 97 | * @brief Encode a cpu_load struct into a message 98 | * 99 | * @param system_id ID of this system 100 | * @param component_id ID of this component (e.g. 200 for IMU) 101 | * @param msg The MAVLink message to compress the data into 102 | * @param cpu_load C-struct to read the message contents from 103 | */ 104 | static inline uint16_t mavlink_msg_cpu_load_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_cpu_load_t* cpu_load) 105 | { 106 | return mavlink_msg_cpu_load_pack(system_id, component_id, msg, cpu_load->sensLoad, cpu_load->ctrlLoad, cpu_load->batVolt); 107 | } 108 | 109 | /** 110 | * @brief Send a cpu_load message 111 | * @param chan MAVLink channel to send the message 112 | * 113 | * @param sensLoad Sensor DSC Load 114 | * @param ctrlLoad Control DSC Load 115 | * @param batVolt Battery Voltage in millivolts 116 | */ 117 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 118 | 119 | static inline void mavlink_msg_cpu_load_send(mavlink_channel_t chan, uint8_t sensLoad, uint8_t ctrlLoad, uint16_t batVolt) 120 | { 121 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 122 | char buf[4]; 123 | _mav_put_uint16_t(buf, 0, batVolt); 124 | _mav_put_uint8_t(buf, 2, sensLoad); 125 | _mav_put_uint8_t(buf, 3, ctrlLoad); 126 | 127 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CPU_LOAD, buf, 4, 75); 128 | #else 129 | mavlink_cpu_load_t packet; 130 | packet.batVolt = batVolt; 131 | packet.sensLoad = sensLoad; 132 | packet.ctrlLoad = ctrlLoad; 133 | 134 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CPU_LOAD, (const char *)&packet, 4, 75); 135 | #endif 136 | } 137 | 138 | #endif 139 | 140 | // MESSAGE CPU_LOAD UNPACKING 141 | 142 | 143 | /** 144 | * @brief Get field sensLoad from cpu_load message 145 | * 146 | * @return Sensor DSC Load 147 | */ 148 | static inline uint8_t mavlink_msg_cpu_load_get_sensLoad(const mavlink_message_t* msg) 149 | { 150 | return _MAV_RETURN_uint8_t(msg, 2); 151 | } 152 | 153 | /** 154 | * @brief Get field ctrlLoad from cpu_load message 155 | * 156 | * @return Control DSC Load 157 | */ 158 | static inline uint8_t mavlink_msg_cpu_load_get_ctrlLoad(const mavlink_message_t* msg) 159 | { 160 | return _MAV_RETURN_uint8_t(msg, 3); 161 | } 162 | 163 | /** 164 | * @brief Get field batVolt from cpu_load message 165 | * 166 | * @return Battery Voltage in millivolts 167 | */ 168 | static inline uint16_t mavlink_msg_cpu_load_get_batVolt(const mavlink_message_t* msg) 169 | { 170 | return _MAV_RETURN_uint16_t(msg, 0); 171 | } 172 | 173 | /** 174 | * @brief Decode a cpu_load message into a struct 175 | * 176 | * @param msg The message to decode 177 | * @param cpu_load C-struct to decode the message contents into 178 | */ 179 | static inline void mavlink_msg_cpu_load_decode(const mavlink_message_t* msg, mavlink_cpu_load_t* cpu_load) 180 | { 181 | #if MAVLINK_NEED_BYTE_SWAP 182 | cpu_load->batVolt = mavlink_msg_cpu_load_get_batVolt(msg); 183 | cpu_load->sensLoad = mavlink_msg_cpu_load_get_sensLoad(msg); 184 | cpu_load->ctrlLoad = mavlink_msg_cpu_load_get_ctrlLoad(msg); 185 | #else 186 | memcpy(cpu_load, _MAV_PAYLOAD(msg), 4); 187 | #endif 188 | } 189 | -------------------------------------------------------------------------------- /libraries/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h: -------------------------------------------------------------------------------- 1 | // MESSAGE CTRL_SRFC_PT PACKING 2 | 3 | #define MAVLINK_MSG_ID_CTRL_SRFC_PT 181 4 | 5 | typedef struct __mavlink_ctrl_srfc_pt_t 6 | { 7 | uint16_t bitfieldPt; ///< Bitfield containing the PT configuration 8 | uint8_t target; ///< The system setting the commands 9 | } mavlink_ctrl_srfc_pt_t; 10 | 11 | #define MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN 3 12 | #define MAVLINK_MSG_ID_181_LEN 3 13 | 14 | 15 | 16 | #define MAVLINK_MESSAGE_INFO_CTRL_SRFC_PT { \ 17 | "CTRL_SRFC_PT", \ 18 | 2, \ 19 | { { "bitfieldPt", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_ctrl_srfc_pt_t, bitfieldPt) }, \ 20 | { "target", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_ctrl_srfc_pt_t, target) }, \ 21 | } \ 22 | } 23 | 24 | 25 | /** 26 | * @brief Pack a ctrl_srfc_pt message 27 | * @param system_id ID of this system 28 | * @param component_id ID of this component (e.g. 200 for IMU) 29 | * @param msg The MAVLink message to compress the data into 30 | * 31 | * @param target The system setting the commands 32 | * @param bitfieldPt Bitfield containing the PT configuration 33 | * @return length of the message in bytes (excluding serial stream start sign) 34 | */ 35 | static inline uint16_t mavlink_msg_ctrl_srfc_pt_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 36 | uint8_t target, uint16_t bitfieldPt) 37 | { 38 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 39 | char buf[3]; 40 | _mav_put_uint16_t(buf, 0, bitfieldPt); 41 | _mav_put_uint8_t(buf, 2, target); 42 | 43 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 3); 44 | #else 45 | mavlink_ctrl_srfc_pt_t packet; 46 | packet.bitfieldPt = bitfieldPt; 47 | packet.target = target; 48 | 49 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 3); 50 | #endif 51 | 52 | msg->msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; 53 | return mavlink_finalize_message(msg, system_id, component_id, 3, 104); 54 | } 55 | 56 | /** 57 | * @brief Pack a ctrl_srfc_pt message on a channel 58 | * @param system_id ID of this system 59 | * @param component_id ID of this component (e.g. 200 for IMU) 60 | * @param chan The MAVLink channel this message was sent over 61 | * @param msg The MAVLink message to compress the data into 62 | * @param target The system setting the commands 63 | * @param bitfieldPt Bitfield containing the PT configuration 64 | * @return length of the message in bytes (excluding serial stream start sign) 65 | */ 66 | static inline uint16_t mavlink_msg_ctrl_srfc_pt_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, 67 | mavlink_message_t* msg, 68 | uint8_t target,uint16_t bitfieldPt) 69 | { 70 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 71 | char buf[3]; 72 | _mav_put_uint16_t(buf, 0, bitfieldPt); 73 | _mav_put_uint8_t(buf, 2, target); 74 | 75 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 3); 76 | #else 77 | mavlink_ctrl_srfc_pt_t packet; 78 | packet.bitfieldPt = bitfieldPt; 79 | packet.target = target; 80 | 81 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 3); 82 | #endif 83 | 84 | msg->msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; 85 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 104); 86 | } 87 | 88 | /** 89 | * @brief Encode a ctrl_srfc_pt struct into a message 90 | * 91 | * @param system_id ID of this system 92 | * @param component_id ID of this component (e.g. 200 for IMU) 93 | * @param msg The MAVLink message to compress the data into 94 | * @param ctrl_srfc_pt C-struct to read the message contents from 95 | */ 96 | static inline uint16_t mavlink_msg_ctrl_srfc_pt_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ctrl_srfc_pt_t* ctrl_srfc_pt) 97 | { 98 | return mavlink_msg_ctrl_srfc_pt_pack(system_id, component_id, msg, ctrl_srfc_pt->target, ctrl_srfc_pt->bitfieldPt); 99 | } 100 | 101 | /** 102 | * @brief Send a ctrl_srfc_pt message 103 | * @param chan MAVLink channel to send the message 104 | * 105 | * @param target The system setting the commands 106 | * @param bitfieldPt Bitfield containing the PT configuration 107 | */ 108 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 109 | 110 | static inline void mavlink_msg_ctrl_srfc_pt_send(mavlink_channel_t chan, uint8_t target, uint16_t bitfieldPt) 111 | { 112 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS 113 | char buf[3]; 114 | _mav_put_uint16_t(buf, 0, bitfieldPt); 115 | _mav_put_uint8_t(buf, 2, target); 116 | 117 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CTRL_SRFC_PT, buf, 3, 104); 118 | #else 119 | mavlink_ctrl_srfc_pt_t packet; 120 | packet.bitfieldPt = bitfieldPt; 121 | packet.target = target; 122 | 123 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CTRL_SRFC_PT, (const char *)&packet, 3, 104); 124 | #endif 125 | } 126 | 127 | #endif 128 | 129 | // MESSAGE CTRL_SRFC_PT UNPACKING 130 | 131 | 132 | /** 133 | * @brief Get field target from ctrl_srfc_pt message 134 | * 135 | * @return The system setting the commands 136 | */ 137 | static inline uint8_t mavlink_msg_ctrl_srfc_pt_get_target(const mavlink_message_t* msg) 138 | { 139 | return _MAV_RETURN_uint8_t(msg, 2); 140 | } 141 | 142 | /** 143 | * @brief Get field bitfieldPt from ctrl_srfc_pt message 144 | * 145 | * @return Bitfield containing the PT configuration 146 | */ 147 | static inline uint16_t mavlink_msg_ctrl_srfc_pt_get_bitfieldPt(const mavlink_message_t* msg) 148 | { 149 | return _MAV_RETURN_uint16_t(msg, 0); 150 | } 151 | 152 | /** 153 | * @brief Decode a ctrl_srfc_pt message into a struct 154 | * 155 | * @param msg The message to decode 156 | * @param ctrl_srfc_pt C-struct to decode the message contents into 157 | */ 158 | static inline void mavlink_msg_ctrl_srfc_pt_decode(const mavlink_message_t* msg, mavlink_ctrl_srfc_pt_t* ctrl_srfc_pt) 159 | { 160 | #if MAVLINK_NEED_BYTE_SWAP 161 | ctrl_srfc_pt->bitfieldPt = mavlink_msg_ctrl_srfc_pt_get_bitfieldPt(msg); 162 | ctrl_srfc_pt->target = mavlink_msg_ctrl_srfc_pt_get_target(msg); 163 | #else 164 | memcpy(ctrl_srfc_pt, _MAV_PAYLOAD(msg), 3); 165 | #endif 166 | } 167 | -------------------------------------------------------------------------------- /libraries/mavlink/include/slugs/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from slugs.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_VERSION_H 6 | #define MAVLINK_VERSION_H 7 | 8 | #define MAVLINK_BUILD_DATE "Fri Apr 20 12:22:53 2012" 9 | #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" 10 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101 11 | 12 | #endif // MAVLINK_VERSION_H 13 | -------------------------------------------------------------------------------- /libraries/mavlink/include/test/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from test.xml 3 | * @see http://pixhawk.ethz.ch/software/mavlink 4 | */ 5 | #ifndef MAVLINK_H 6 | #define MAVLINK_H 7 | 8 | #ifndef MAVLINK_STX 9 | #define MAVLINK_STX 254 10 | #endif 11 | 12 | #ifndef MAVLINK_ENDIAN 13 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 14 | #endif 15 | 16 | #ifndef MAVLINK_ALIGNED_FIELDS 17 | #define MAVLINK_ALIGNED_FIELDS 1 18 | #endif 19 | 20 | #ifndef MAVLINK_CRC_EXTRA 21 | #define MAVLINK_CRC_EXTRA 1 22 | #endif 23 | 24 | #include "version.h" 25 | #include "test.h" 26 | 27 | #endif // MAVLINK_H 28 | -------------------------------------------------------------------------------- /libraries/mavlink/include/test/testsuite.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol testsuite generated from test.xml 3 | * @see http://qgroundcontrol.org/mavlink/ 4 | */ 5 | #ifndef TEST_TESTSUITE_H 6 | #define TEST_TESTSUITE_H 7 | 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | #ifndef MAVLINK_TEST_ALL 13 | #define MAVLINK_TEST_ALL 14 | 15 | static void mavlink_test_test(uint8_t, uint8_t, mavlink_message_t *last_msg); 16 | 17 | static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) 18 | { 19 | 20 | mavlink_test_test(system_id, component_id, last_msg); 21 | } 22 | #endif 23 | 24 | 25 | 26 | 27 | static void mavlink_test_test_types(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) 28 | { 29 | mavlink_message_t msg; 30 | uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; 31 | uint16_t i; 32 | mavlink_test_types_t packet_in = { 33 | 93372036854775807ULL, 34 | 93372036854776311LL, 35 | 235.0, 36 | { 93372036854777319, 93372036854777320, 93372036854777321 }, 37 | { 93372036854778831, 93372036854778832, 93372036854778833 }, 38 | { 627.0, 628.0, 629.0 }, 39 | 963502456, 40 | 963502664, 41 | 745.0, 42 | { 963503080, 963503081, 963503082 }, 43 | { 963503704, 963503705, 963503706 }, 44 | { 941.0, 942.0, 943.0 }, 45 | 24723, 46 | 24827, 47 | { 24931, 24932, 24933 }, 48 | { 25243, 25244, 25245 }, 49 | 'E', 50 | "FGHIJKLMN", 51 | 198, 52 | 9, 53 | { 76, 77, 78 }, 54 | { 21, 22, 23 }, 55 | }; 56 | mavlink_test_types_t packet1, packet2; 57 | memset(&packet1, 0, sizeof(packet1)); 58 | packet1.u64 = packet_in.u64; 59 | packet1.s64 = packet_in.s64; 60 | packet1.d = packet_in.d; 61 | packet1.u32 = packet_in.u32; 62 | packet1.s32 = packet_in.s32; 63 | packet1.f = packet_in.f; 64 | packet1.u16 = packet_in.u16; 65 | packet1.s16 = packet_in.s16; 66 | packet1.c = packet_in.c; 67 | packet1.u8 = packet_in.u8; 68 | packet1.s8 = packet_in.s8; 69 | 70 | mav_array_memcpy(packet1.u64_array, packet_in.u64_array, sizeof(uint64_t)*3); 71 | mav_array_memcpy(packet1.s64_array, packet_in.s64_array, sizeof(int64_t)*3); 72 | mav_array_memcpy(packet1.d_array, packet_in.d_array, sizeof(double)*3); 73 | mav_array_memcpy(packet1.u32_array, packet_in.u32_array, sizeof(uint32_t)*3); 74 | mav_array_memcpy(packet1.s32_array, packet_in.s32_array, sizeof(int32_t)*3); 75 | mav_array_memcpy(packet1.f_array, packet_in.f_array, sizeof(float)*3); 76 | mav_array_memcpy(packet1.u16_array, packet_in.u16_array, sizeof(uint16_t)*3); 77 | mav_array_memcpy(packet1.s16_array, packet_in.s16_array, sizeof(int16_t)*3); 78 | mav_array_memcpy(packet1.s, packet_in.s, sizeof(char)*10); 79 | mav_array_memcpy(packet1.u8_array, packet_in.u8_array, sizeof(uint8_t)*3); 80 | mav_array_memcpy(packet1.s8_array, packet_in.s8_array, sizeof(int8_t)*3); 81 | 82 | 83 | memset(&packet2, 0, sizeof(packet2)); 84 | mavlink_msg_test_types_encode(system_id, component_id, &msg, &packet1); 85 | mavlink_msg_test_types_decode(&msg, &packet2); 86 | MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); 87 | 88 | memset(&packet2, 0, sizeof(packet2)); 89 | mavlink_msg_test_types_pack(system_id, component_id, &msg , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array ); 90 | mavlink_msg_test_types_decode(&msg, &packet2); 91 | MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); 92 | 93 | memset(&packet2, 0, sizeof(packet2)); 94 | mavlink_msg_test_types_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array ); 95 | mavlink_msg_test_types_decode(&msg, &packet2); 96 | MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); 97 | 98 | memset(&packet2, 0, sizeof(packet2)); 99 | mavlink_msg_to_send_buffer(buffer, &msg); 100 | for (i=0; i 2 | 3 | common.xml 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /libraries/mavlink/message_definitions/slugs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | common.xml 4 | 5 | 6 | Sensor and DSC control loads. 7 | The system reporting the CPU load 8 | Sensor DSC Load 9 | Control DSC Load 10 | Battery Voltage in millivolts 11 | 12 | 13 | Air data for altitude and airspeed computation. 14 | The system reporting the air data 15 | Dynamic pressure (Pa) 16 | Static pressure (Pa) 17 | Board temperature 18 | 19 | 20 | 21 | Accelerometer and gyro biases. 22 | The system reporting the biases 23 | Accelerometer X bias (m/s) 24 | Accelerometer Y bias (m/s) 25 | Accelerometer Z bias (m/s) 26 | Gyro X bias (rad/s) 27 | Gyro Y bias (rad/s) 28 | Gyro Z bias (rad/s) 29 | 30 | 31 | 32 | Configurable diagnostic messages. 33 | The system reporting the diagnostic 34 | Diagnostic float 1 35 | Diagnostic float 2 36 | Diagnostic float 3 37 | Diagnostic short 1 38 | Diagnostic short 2 39 | Diagnostic short 3 40 | 41 | 42 | 43 | Pilot console PWM messges. 44 | The system reporting the diagnostic 45 | Pilot's console throttle command 46 | Pilot's console left aileron command 47 | Pilot's console right aileron command 48 | Pilot's console rudder command 49 | Pilot's console elevator command 50 | 51 | 52 | 53 | PWM Commands from the AP to the control surfaces. 54 | The system reporting the diagnostic 55 | AutoPilot's throttle command 56 | AutoPilot's left aileron command 57 | AutoPilot's right aileron command 58 | AutoPilot's rudder command 59 | AutoPilot's left elevator command 60 | AutoPilot's right elevator command 61 | AutoPilot's left flap command 62 | AutoPilot's right flap command 63 | AutoPilot's aux1 command 64 | AutoPilot's aux2 command 65 | 66 | 67 | -------------------------------------------------------------------------------- /libraries/mavlink/message_definitions/ualberta.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | common.xml 4 | 5 | 6 | Accelerometer and Gyro biases from the navigation filter 7 | Timestamp (microseconds) 8 | b_f[0] 9 | b_f[1] 10 | b_f[2] 11 | b_f[0] 12 | b_f[1] 13 | b_f[2] 14 | 15 | 16 | Request raw and normalized rc data from the UAV 17 | True: start sending data; False: stop sending data 18 | 19 | 20 | Complete set of calibration parameters for the radio 21 | Aileron setpoints: high, center, low 22 | Elevator setpoints: high, center, low 23 | Rudder setpoints: high, center, low 24 | Tail gyro mode/gain setpoints: heading hold, rate mode 25 | Pitch curve setpoints (every 25%) 26 | Throttle curve setpoints (every 25%) 27 | 28 | 29 | 30 | --------------------------------------------------------------------------------