├── .gitmodules ├── LICENSE.md ├── README.md ├── README_images ├── mav_axis_monitor.png ├── mav_button_monitor.png ├── mavinspect.png └── virt_joystick.png └── barebones_MAVLink.ino /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "mavlink_c_v2"] 2 | path = mavlink_c_v2 3 | url = https://github.com/mavlink/c_library_v2.git 4 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Daniel S. Zimmerman 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Establish a MAVLink connection with QGroundControl 2 | 3 | [QGroundControl](http://qgroundcontrol.com/) is an open-source 4 | "ground control station" for uncrewed aerial vehicles. It's an all-in-one center for display of live-streaming inbound UDP video, attractive display of vehicle telemetry, and manual joystick remote control. It communicates with the vehicle controller using the [open MAVLink protocol](https://mavlink.io/). Though it's specialized for autonomous aerial vehicle control, I thought it would be a good option for remote human-piloted teleoperation of a project I was working on. 5 | 6 | I got it working, but had a hard time finding clear documentation or an example of the specific messages required to establish a MAVLink "connection" between a vehicle and [QGroundControl](http://qgroundcontrol.com/) outside of a complicated autopilot framework. 7 | 8 | The Arduino-compatible sketch here uses the [common MAVLink message set](https://mavlink.io/en/messages/common.html) via auto-generated C headers (added to this project as a submodule) to negotiate a serial-port connection with QGroundControl. Once connected, the controller will send some fake telemetry to QGroundControl and respond to manual control packets from QGC. Manual control assumes that a [supported joystick](https://docs.qgroundcontrol.com/en/SetupView/Joystick.html#supported-joysticks) is connected to the ground control computer and calibrated, or that you turn on virtual joysticks. I tested this with the Logitech F710. 9 | 10 | This sketch is a demonstration and is not necessarily intended to be useful by itself. If you are working with a [quadcopter](http://px4.io/), [boat](https://discuss.ardupilot.org/t/rover-3-0-0-release/8267), [submarine](https://github.com/bluerobotics/ardusub/), or [wheeled rover](https://discuss.ardupilot.org/t/rover-3-0-0-release/8267), using one of the appropriate autopilot software offerings would probably be more appropriate than working with MAVLink messages directly. 11 | 12 | If you're trying implement some new functionality using a microcontroller running in parallel with an existing autopilot flight stack, you don't need to negotiate a connection with QGC, and you're probably looking for something more like [MAVLink and Arduino: step by step](https://discuss.ardupilot.org/t/mavlink-and-arduino-step-by-step/25566/22). 13 | 14 | My example is targeted toward using QGroundControl to control and monitor a well-developed robot or vehicle that is a poor fit for the existing autopilots (like a snake robot or a grappling-hook-launching cable robot): something that already has significant work invested in its custom control hardware and code, and needs MAVLink as an added feature. 15 | 16 | ## Using This Sketch 17 | 18 | I was hoping to implement this on an Arduino Uno, but in its current form, it doesn't fit in memory. It uses about 36k flash and 5.7k for globals. In tests so far, it's running on a [Teensy 3.6](https://www.pjrc.com/store/teensy36.html), where it only consumes a few percent of the available memory. I might try to optimize it to fit on the Uno but it will require careful sharing of resources, and/or tinkering with the internal MAVLink buffer allocations and I don't want that complication in a basic demo. 19 | 20 | If you clone this repository, make sure you run `git submodule init` and `git submodule update` in the project directory to actually 21 | pull in the MAVLink headers. 22 | 23 | Once you have the MAVLink headers, compile and upload this to anything Arduino-compatible (implementing the `Serial` methods, etc). To open the communications link, you need to manually configure (`Q Menu->Comm Links->Add`) and connect to the appropriate serial port in QGroundControl. This will result in a connected "vehicle" and some incoming telemetry. 24 | 25 | Connecting, calibrating, and enabling a compatible joystick, like my Logitech F710, will allow QGroundControl to send [`#69 MANUAL_CONTROL`](https://mavlink.io/en/messages/common.html#MANUAL_CONTROL) messages to the target controller. To avoid the need for external hardware for this demonstration, the sketch simply transmits the recieved manual control message back to QGroundControl. The re-transmitted messages can be viewed in the MAVLink Inspector widget: 26 | 27 | ![](README_images/mavinspect.png) 28 | 29 | The fields in the manual control message show the continous joystick values `x`, `y`, `z`, and `r`: 30 | 31 | ![](README_images/mav_axis_monitor.png) 32 | 33 | and `buttons` gives one integer value for the entire button bitfield. Here, buttons 2 and 14 are pressed, giving 22+214=16388: 34 | 35 | ![](README_images/mav_button_monitor.png) 36 | 37 | To use the manual control data, you can access the values like `mvl_joy.z` or `mvl_joy.buttons` in the function `MVL_Handle_Manual_Control()`. Note that the joystick axis that's mapped to `throttle` has a different behavior from the others. 38 | 39 | If you don't have a physical joystick or gamepad yet, you can turn on the virtual joysticks in the QGC settings to start the streaming and loopback of the `MANUAL_CONTROL` messages: 40 | 41 | ![](README_images/virt_joystick.png) 42 | 43 | Without a real or virtual joystick, QGC won't send any `MANUAL_CONTROL` messages to the microcontroller and so you won't see them in MAVLink Inspector. 44 | 45 | This sketch demonstrates: 46 | * Handling QGC's series of start-up queries to negotiate a successful "connection" between the controller and QGroundControl, i.e. 47 | * Responding to `PARAM_REQUEST_LIST` with a single `PARAM_VALUE` message. 48 | * Responding to `MISSION_REQUEST_LIST` with a `MISSION_COUNT` message saying there are zero mission items. 49 | * Responding to a `MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES` command (in a `COMMAND_LONG` message) with an `AUTOPILOT_VERSION` message. 50 | * Handling the `MANUAL_CONTROL` messages that you'll want to use for vehicle/robot teleoperation. 51 | * Handling and acknowledging arm/disarm commands (which come in as a `COMMAND_LONG` message) 52 | * Sending some fake telemetry via `SYS_STATUS` and `ATTITUDE_QUATERNION` messages including: 53 | * A gentle rocking in pitch and roll in the attitude display. 54 | * A continuous rotation in the heading display. 55 | * A battery voltage ramp from 10-16V. 56 | 57 | ## Caveats 58 | 59 | I started with the [connection control flow](https://dev.qgroundcontrol.com/en/communication_flow.html) in the [QGroundControl Developers' Guide](https://dev.qgroundcontrol.com/en/). I dug through several autopilots' source code trying to find the specific MAVLink messages that were sent at each step, but with many layers of object-oriented code across hundreds of files, this was not particularly productive. 60 | 61 | Instead, I printed information about every valid MAVLink message that came in and worked out the specific messages I needed through trial and error. I believe that all the message responses in this sketch *are* necessary to establish a "connection" and to get the joystick interface to appear in QGC, but I haven't tested that exhaustively since I got the connection to work. 62 | 63 | I also assume that the connection between QGC and the vehicle controller is perfect. With unreliable communications, there may be some re-request messages that would need to be handled that I wouldn't have seen in my testing over a wired link. 64 | 65 | I've tested this on Windows 10 with the [Blue Robotics Fork of QGroundControl (v3.2.4)](https://discuss.bluerobotics.com/t/software-updates-8-may-2018/2617) and the [official version (v 3.3.2)](http://qgroundcontrol.com/downloads/). 66 | -------------------------------------------------------------------------------- /README_images/mav_axis_monitor.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/danzimmerman/barebones_MAVLink/557b8b9b5d2aa2f887332a05837a37ede6bb201a/README_images/mav_axis_monitor.png -------------------------------------------------------------------------------- /README_images/mav_button_monitor.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/danzimmerman/barebones_MAVLink/557b8b9b5d2aa2f887332a05837a37ede6bb201a/README_images/mav_button_monitor.png -------------------------------------------------------------------------------- /README_images/mavinspect.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/danzimmerman/barebones_MAVLink/557b8b9b5d2aa2f887332a05837a37ede6bb201a/README_images/mavinspect.png -------------------------------------------------------------------------------- /README_images/virt_joystick.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/danzimmerman/barebones_MAVLink/557b8b9b5d2aa2f887332a05837a37ede6bb201a/README_images/virt_joystick.png -------------------------------------------------------------------------------- /barebones_MAVLink.ino: -------------------------------------------------------------------------------- 1 | /* barebones_MAVLink.ino 2 | 3 | A Simple QGroundControl MAVLink Example 4 | 5 | MIT License 6 | 7 | Copyright (c) 2018 Daniel S. Zimmerman 8 | 9 | Permission is hereby granted, free of charge, to any person obtaining a copy 10 | of this software and associated documentation files (the "Software"), to deal 11 | in the Software without restriction, including without limitation the rights 12 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 13 | copies of the Software, and to permit persons to whom the Software is 14 | furnished to do so, subject to the following conditions: 15 | 16 | The above copyright notice and this permission notice shall be included in all 17 | copies or substantial portions of the Software. 18 | 19 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 20 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 21 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 22 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 23 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 24 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 25 | SOFTWARE. 26 | 27 | 28 | */ 29 | 30 | #include "./mavlink_c_v2/common/mavlink.h" 31 | #include "math.h" 32 | 33 | mavlink_message_t mvl_tx_message; //A special MAVLink message data structure. 34 | mavlink_message_t mvl_rx_message; 35 | mavlink_status_t mvl_rx_status; 36 | const uint8_t mvl_compid = 1; //Component ID and System ID identify us to QGroundControl 37 | const uint8_t mvl_sysid = 1; 38 | const uint8_t mvl_chan = MAVLINK_COMM_1; //MAVLink channel 1 appears to be required at least for Blue Robotics QGC 39 | 40 | //In my actual code for the Cypress PSoC, I use timer interrupts to schedule all the robot tasks. 41 | const uint32_t hb_interval = 1000; //heartbeat interval in milliseconds - 1 second 42 | uint32_t t_last_hb = 0; 43 | const uint32_t sys_stat_interval = 100; //10 system status messages per second 44 | uint32_t t_last_sys_stat =0; 45 | int16_t sys_stat_count = 0; 46 | uint8_t mvl_armed = 0; 47 | uint8_t mvl_packet_received = 0; 48 | 49 | /*============================================================== 50 | * Message-handling and transmitting functions 51 | * used in the main loop are defined below. 52 | *==============================================================*/ 53 | 54 | void MVL_Transmit_Message(mavlink_message_t* mvl_msg_ptr) 55 | { 56 | uint8_t tx_byte_buffer[512]={0}; //A byte buffer that will be sent from the serial port. 57 | uint16_t tx_buflen = mavlink_msg_to_send_buffer(tx_byte_buffer,mvl_msg_ptr); 58 | Serial.write(tx_byte_buffer,tx_buflen); 59 | } 60 | 61 | void MVL_Handle_Manual_Control(mavlink_message_t* mvl_msg_ptr) 62 | { 63 | mavlink_manual_control_t mvl_joy; //manual control data structure into which we decode the message 64 | mavlink_msg_manual_control_decode(mvl_msg_ptr,&mvl_joy); 65 | //For now, let's just retransmit the manual control message to see it in MAVLink Inspector 66 | mavlink_msg_manual_control_encode_chan(mvl_sysid,mvl_compid,mvl_chan, 67 | &mvl_tx_message,&mvl_joy); 68 | MVL_Transmit_Message(&mvl_tx_message); 69 | } 70 | 71 | void MVL_Handle_Param_Request_List(mavlink_message_t* mvl_msg_ptr) 72 | { 73 | mavlink_param_value_t mvl_param; 74 | 75 | mvl_param.param_id[0] = 'a'; //a parameter ID string, less than 16 characters. 76 | mvl_param.param_id[1] = '_'; 77 | mvl_param.param_id[2] = 'p'; 78 | mvl_param.param_id[3] = 'a'; 79 | mvl_param.param_id[4] = 'r'; 80 | mvl_param.param_id[5] = 'm'; 81 | mvl_param.param_id[6] = 0; //null terminated 82 | mvl_param.param_value = 123.456; //the parameter value as a float 83 | mvl_param.param_type = MAV_PARAM_TYPE_REAL32; //https://mavlink.io/en/messages/common.html#MAV_PARAM_TYPE 84 | mvl_param.param_count = 1; //We have just one parameter to send. 85 | mvl_param.param_index = 0; 86 | mavlink_msg_param_value_encode_chan(mvl_sysid,mvl_compid,mvl_chan, 87 | &mvl_tx_message,&mvl_param); 88 | MVL_Transmit_Message(&mvl_tx_message); 89 | 90 | } 91 | 92 | void MVL_Handle_Command_Long(mavlink_message_t* mvl_msg_ptr) 93 | { 94 | mavlink_command_long_t mvl_cmd; 95 | mavlink_msg_command_long_decode(mvl_msg_ptr,&mvl_cmd); 96 | switch (mvl_cmd.command) 97 | { 98 | case (MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES): 99 | { 100 | if (1==mvl_cmd.param1) 101 | { 102 | mavlink_autopilot_version_t mvl_apv; https://mavlink.io/en/messages/common.html#AUTOPILOT_VERSION 103 | mvl_apv.flight_sw_version = 2; 104 | mvl_apv.middleware_sw_version = 1; 105 | mvl_apv.board_version = 1; 106 | mvl_apv.vendor_id = 10101; 107 | mvl_apv.product_id = 20202; 108 | mvl_apv.uid = 0; 109 | mvl_apv.capabilities = 0; //See: https://mavlink.io/en/messages/common.html#MAV_PROTOCOL_CAPABILITY 110 | mvl_apv.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET; //Just as an example, code does not support! https://mavlink.io/en/messages/common.html#MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET 111 | mvl_apv.capabilities |= MAV_PROTOCOL_CAPABILITY_MAVLINK2; 112 | mavlink_msg_autopilot_version_encode_chan(mvl_sysid,mvl_compid,mvl_chan, 113 | &mvl_tx_message,&mvl_apv); 114 | MVL_Transmit_Message(&mvl_tx_message); 115 | } 116 | break; 117 | }//end handling of autopilot capabilities request 118 | case (MAV_CMD_COMPONENT_ARM_DISARM): 119 | { 120 | if (1==mvl_cmd.param1) 121 | { 122 | mvl_armed = 1; 123 | } 124 | else 125 | { 126 | mvl_armed = 0; 127 | } 128 | //Acknowledge the arm/disarm command. 129 | mavlink_command_ack_t mvl_ack; //https://mavlink.io/en/messages/common.html#COMMAND_ACK 130 | mvl_ack.command = MAV_CMD_COMPONENT_ARM_DISARM; //https://mavlink.io/en/messages/common.html#MAV_CMD_COMPONENT_ARM_DISARM 131 | mvl_ack.result = MAV_RESULT_ACCEPTED; //https://mavlink.io/en/messages/common.html#MAV_RESULT 132 | mavlink_msg_command_ack_encode_chan(mvl_sysid,mvl_compid,mvl_chan, 133 | &mvl_tx_message,&mvl_ack); 134 | //skipped setting several fields here, with unknown consequences. 135 | MVL_Transmit_Message(&mvl_tx_message); 136 | break; 137 | }//end handling of arm/disarm command 138 | }//end switch/case 139 | }//end MVL_Handle_Command_Long() 140 | 141 | void MVL_Handle_Mission_Request_List(mavlink_message_t* mvl_msg_ptr) 142 | { 143 | mavlink_mission_count_t mvl_mc; 144 | mvl_mc.target_system = mvl_sysid; 145 | mvl_mc.target_component = mvl_compid; 146 | mvl_mc.count = 0; 147 | mavlink_msg_mission_count_encode_chan(mvl_sysid,mvl_compid,mvl_chan, 148 | &mvl_tx_message,&mvl_mc); 149 | MVL_Transmit_Message(&mvl_tx_message); 150 | } 151 | 152 | //=========================== Main program is below ====================================== 153 | 154 | void setup() 155 | { 156 | Serial.begin(115200); //start the serial port for the MAVLink packets. 157 | } 158 | 159 | void loop() 160 | { 161 | /* ======================== Serial MAVLink Message Reception ==================== 162 | * If bytes come in on the serial port, we feed them to mavlink_parse_char(). 163 | * This helper function keeps track of incoming bytes and alerts us when it's 164 | * received a complete, valid MAVLink message. 165 | * See https://github.com/mavlink/c_library_v2/blob/master/mavlink_helpers.h#L966 166 | *==============================================================================*/ 167 | 168 | while (Serial.available()) 169 | { 170 | uint8_t rxbyte = Serial.read(); 171 | mvl_packet_received = mavlink_parse_char(mvl_chan,rxbyte, 172 | &mvl_rx_message, 173 | &mvl_rx_status); 174 | } 175 | 176 | /* ====================== received MAVLink Message Handling ================== 177 | * If a full incoming MAVLink message is received, AND the message 178 | * came from the GCS (System ID 255), we handle it here. 179 | * 180 | * In this code we: 181 | * -Respond to initial messages sent from QGC to the vehicle when it first connects, including: 182 | * -A parameter list request. We send an arbitary float parameter. 183 | * -A request for "mission items." We say we have none. 184 | * -A request for autopilot capabilities and version. We make some things up. 185 | * -Take action on manual control joystick messages -- retransmitted back to QGC for viewing in MAVLink inspector 186 | * -Arm or disarm the vehicle and acknowledge arm/disarm status based on incoming command messages. 187 | * There may be more messages you want to handle. 188 | * The Message IDs below are defined in individual message's .h files. 189 | * For example: https://github.com/mavlink/c_library_v2/blob/master/common/mavlink_msg_manual_control.h#L4 190 | * You can find message numbers and field descriptions at https://mavlink.io/en/messages/common.html 191 | * ==================================================================== */ 192 | if ((mvl_packet_received) && (255==mvl_rx_message.sysid)) 193 | { 194 | mvl_packet_received = 0; //reset the "packet received" flag 195 | switch (mvl_rx_message.msgid) 196 | { 197 | case MAVLINK_MSG_ID_MANUAL_CONTROL: //#69 https://mavlink.io/en/messages/common.html#MANUAL_CONTROL 198 | { 199 | MVL_Handle_Manual_Control(&mvl_rx_message); 200 | break; 201 | } 202 | case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: //#21 https://mavlink.io/en/messages/common.html#PARAM_REQUEST_LIST 203 | { 204 | MVL_Handle_Param_Request_List(&mvl_rx_message); 205 | break; 206 | } 207 | case MAVLINK_MSG_ID_COMMAND_LONG: //#76 https://mavlink.io/en/messages/common.html#COMMAND_LONG 208 | { 209 | MVL_Handle_Command_Long(&mvl_rx_message); 210 | break; 211 | } 212 | case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: //#43 https://mavlink.io/en/messages/common.html#MISSION_REQUEST_LIST 213 | { 214 | MVL_Handle_Mission_Request_List(&mvl_rx_message); 215 | break; 216 | } 217 | 218 | } 219 | 220 | 221 | } 222 | 223 | /*=========================== Periodic Telemetry Transmissions ====================== 224 | * To let QGroundControl know a vehicle is present, we send a heartbeat every 1s. 225 | * Here I'm using the millis() function to schedule outbound messages. 226 | * You don't want to use blocking delay() calls in code like this, because you want 227 | * to read the incoming serial port as often as possible. 228 | * 229 | * In my usual target system, I use timer interrupts for scheduling. 230 | * ==============================================================================*/ 231 | 232 | if ((millis()-t_last_hb)>hb_interval) 233 | { //#0 HEARTBEAT https://mavlink.io/en/messages/common.html#HEARTBEAT 234 | mavlink_heartbeat_t mvl_hb; //struct with user fields: uint32_t custom_mode, uint8_t type, uint8_t autopilot, uint8_t base_mode, uint8_t system_status; 235 | mvl_hb.type = MAV_TYPE_SUBMARINE; //My vehicle is an underwater ROV. Change as appropriate. See: https://github.com/mavlink/c_library_v2/blob/748192f661d0df3763501cfc432861d981952921/common/common.h#L69 236 | mvl_hb.autopilot = MAV_AUTOPILOT_GENERIC; //See https://github.com/mavlink/c_library_v2/blob/748192f661d0df3763501cfc432861d981952921/common/common.h#L40 237 | mvl_hb.system_status = MAV_STATE_ACTIVE; 238 | if (mvl_armed) 239 | { 240 | mvl_hb.base_mode = MAV_MODE_MANUAL_ARMED; 241 | } 242 | else 243 | { 244 | mvl_hb.base_mode = MAV_MODE_MANUAL_DISARMED; 245 | } 246 | mvl_hb.base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; //I always use CUSTOM_MODE_ENABLED 247 | mvl_hb.custom_mode=0xABBA; //custom mode, can be anything, I guess 248 | mavlink_msg_heartbeat_encode_chan(mvl_sysid,mvl_compid,mvl_chan, 249 | &mvl_tx_message,&mvl_hb); 250 | MVL_Transmit_Message(&mvl_tx_message); 251 | t_last_hb = millis(); 252 | } 253 | 254 | if ((millis()-t_last_sys_stat)>sys_stat_interval) 255 | { 256 | 257 | //We'll make up some fake periodic data to feed to the QGroundControl widget 258 | float pfreq = 0.2; //slowly varying 259 | float phaserad = 2*PI*pfreq*millis()/1000.0; 260 | 261 | //We'll send a varying voltage signal that ramps from 10000 to 16,000 mV over 60 sys_stat_intervals... 10-16V on the QGC widget 262 | int16_t angle_as_mV = sys_stat_count*100+10000; 263 | sys_stat_count+=1; 264 | sys_stat_count%=60; 265 | //I often use the mavlink__pack_chan() functions that 266 | //accept each field as an argument instead of the mavlink__encode() that 267 | //accepts a struct. They should save some memory to skip the extra 268 | //message struct, but I think setting each field by name in a demo code is easier to follow. 269 | 270 | mavlink_sys_status_t mvl_sys_stat; //#1 SYS_STATUS https://mavlink.io/en/messages/common.html#SYS_STATUS 271 | mvl_sys_stat.onboard_control_sensors_present = 0; //To set these, consult https://mavlink.io/en/messages/common.html#MAV_SYS_STATUS_SENSOR 272 | mvl_sys_stat.onboard_control_sensors_enabled = 0; 273 | mvl_sys_stat.onboard_control_sensors_health = 0; 274 | mvl_sys_stat.load = 0; 275 | mvl_sys_stat.voltage_battery = angle_as_mV; //the only non-trivial telemetry we're sending, shows up several places in QGC 276 | mvl_sys_stat.current_battery = -1; 277 | mvl_sys_stat.battery_remaining = -1; 278 | mvl_sys_stat.drop_rate_comm = 0; 279 | mvl_sys_stat.errors_comm = 0; 280 | mvl_sys_stat.errors_count1 = 0; 281 | mvl_sys_stat.errors_count2 = 0; 282 | mvl_sys_stat.errors_count3 = 0; 283 | mvl_sys_stat.errors_count4 = 0; 284 | 285 | //We'll also send an attitude quaternion to display something in the QGC 286 | //roll/pitch widget and in the compass. 287 | //The code below results in a gentle spherical rocking in the QGC roll/pitch widget 288 | //and a continuous rotation of the displayed heading. 289 | 290 | float maxang = 0.0873; // about five degrees 291 | float roll = maxang*sin(phaserad); 292 | float pitch = maxang*cos(phaserad); 293 | float yaw = phaserad; 294 | 295 | //Quaternion conversion taken from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Source_Code 296 | float cy = cos(yaw * 0.5); 297 | float sy = sin(yaw * 0.5); 298 | float cr = cos(roll * 0.5); 299 | float sr = sin(roll * 0.5); 300 | float cp = cos(pitch * 0.5); 301 | float sp = sin(pitch * 0.5); 302 | 303 | mavlink_attitude_quaternion_t mvl_att_quat; //#31 ATTITUDE_QUATERNION https://mavlink.io/en/messages/common.html#ATTITUDE_QUATERNION 304 | mvl_att_quat.time_boot_ms = millis(); 305 | mvl_att_quat.q1 = cy * cr * cp + sy * sr * sp; //https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Source_Code 306 | mvl_att_quat.q2 = cy * sr * cp - sy * cr * sp; 307 | mvl_att_quat.q3 = cy * cr * sp + sy * sr * cp; 308 | mvl_att_quat.q4 = sy * cr * cp - cy * sr * sp; 309 | 310 | mvl_att_quat.rollspeed = 2*PI*pfreq*maxang*cos(phaserad); //d/dt A*sin(2*pi*f*t) = 2*pi*f*A*cos(2*pi*f*t) 311 | mvl_att_quat.pitchspeed = -2*PI*pfreq*maxang*sin(phaserad); 312 | mvl_att_quat.yawspeed = 2*PI*pfreq; 313 | 314 | mavlink_msg_sys_status_encode_chan(mvl_sysid,mvl_compid,mvl_chan, 315 | &mvl_tx_message,&mvl_sys_stat); 316 | MVL_Transmit_Message(&mvl_tx_message); 317 | mavlink_msg_attitude_quaternion_encode_chan(mvl_sysid,mvl_compid,mvl_chan, 318 | &mvl_tx_message,&mvl_att_quat); 319 | MVL_Transmit_Message(&mvl_tx_message); 320 | t_last_sys_stat = millis(); 321 | } 322 | 323 | 324 | } //end main loop 325 | 326 | 327 | --------------------------------------------------------------------------------