├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── README.md ├── autopilot_interface.cpp ├── autopilot_interface.h ├── generic_port.h ├── makefile ├── mavlink_control.cpp ├── mavlink_control.h ├── serial_port.cpp ├── serial_port.h ├── udp_port.cpp └── udp_port.h /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | # unix files 3 | *~ 4 | 5 | # eclipse 6 | .settings/ 7 | build/ 8 | .cproject 9 | .project 10 | 11 | # build 12 | *.o 13 | *.d 14 | mavlink_control 15 | 16 | # mac 17 | .DS_Store 18 | 19 | 20 | /build/ 21 | /Debug/ 22 | 23 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "mavlink/include/mavlink/v2.0"] 2 | path = mavlink/include/mavlink/v2.0 3 | url = https://github.com/mavlink/c_library_v2.git 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | set(CMAKE_CXX_STANDARD 17) 3 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 4 | 5 | project(c_uart_interface_example CXX) 6 | 7 | set(SOURCES 8 | autopilot_interface.cpp 9 | mavlink_control.cpp 10 | serial_port.cpp 11 | udp_port.cpp 12 | ) 13 | 14 | add_executable(${PROJECT_NAME} ${SOURCES}) 15 | 16 | target_include_directories(${PROJECT_NAME} PRIVATE mavlink/include/mavlink/v2.0) 17 | 18 | target_link_libraries(${PROJECT_NAME} PRIVATE pthread) -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | C-UART Interface Example 2 | ======================== 3 | 4 | This is a simple MAVLink to UART interface example for *nix systems that can allow communication between Pixhawk and an offboard computer. 5 | 6 | This example will receive one MAVLink message and send one MAVLink message. 7 | 8 | 9 | Building 10 | ======== 11 | 12 | ``` 13 | $ cd c_uart_interface_example/ 14 | $ make 15 | ``` 16 | 17 | Hardware Setup 18 | ========= 19 | 20 | Connect the USB programming cable to your Pixhawk. 21 | 22 | If you want to be able to interact with this example in Pixhawk's NuttX shell, you'll need a Telemetry Radio or an FTDI developer's cable. See the Exploration section below for more detail. 23 | 24 | Also Note: Using a UART (serial) connection should be preferred over using the USB port for flying systems. The reason being that the driver for the USB port is much more complicated, so the UART is a much more trusted port for flight-critical functions. To learn how this works though the USB port will be fine and instructive. 25 | 26 | Execution 27 | ========= 28 | 29 | You have to pick a port name, try searching for it with 30 | ``` 31 | 32 | $ ls /dev/ttyACM* 33 | $ ls /dev/ttyUSB* 34 | ``` 35 | 36 | Alternatively, plug in Pixhawk USB cable again and issue the command: 37 | ``` 38 | $ dmesg 39 | ``` 40 | The device described at the bottom of dmesg's output will be the port on which the Pixhawk is mounted. 41 | 42 | The Pixhawk USB port will show up on a `ttyACM*`, an FTDI cable will show up on a `ttyUSB*`. 43 | 44 | 45 | Run the example executable on the host shell: 46 | 47 | ``` 48 | $ cd c_uart_interface_example/ 49 | $ ./mavlink_control -d /dev/ttyACM0 50 | ``` 51 | 52 | To stop the program, use the key sequence `Ctrl-C`. 53 | 54 | Here's an example output: 55 | 56 | ``` 57 | OPEN PORT 58 | Connected to /dev/ttyUSB0 with 57600 baud, 8 data bits, no parity, 1 stop bit (8N1) 59 | 60 | START READ THREAD 61 | 62 | CHECK FOR HEARTBEAT 63 | Found 64 | 65 | GOT VEHICLE SYSTEM ID: 1 66 | GOT AUTOPILOT COMPONENT ID: 50 67 | 68 | INITIAL POSITION XYZ = [ 8.2935 , -1.1447 , -0.7609 ] 69 | INITIAL POSITION YAW = 2.1539 70 | 71 | START WRITE THREAD 72 | 73 | ENABLE OFFBOARD MODE 74 | 75 | SEND OFFBOARD COMMANDS 76 | POSITION SETPOINT XYZ = [ 3.2935 , -6.1447 , -0.7609 ] 77 | POSITION SETPOINT YAW = 2.1539 78 | 0 CURRENT POSITION XYZ = [ 8.2935 , -1.1447 , -0.7609 ] 79 | 1 CURRENT POSITION XYZ = [ 8.2935 , -1.1447 , -0.7609 ] 80 | 2 CURRENT POSITION XYZ = [ 8.2524 , -1.1444 , -0.7667 ] 81 | 3 CURRENT POSITION XYZ = [ 8.2205 , -1.1431 , -0.7747 ] 82 | 4 CURRENT POSITION XYZ = [ 8.1920 , -1.1421 , -0.7737 ] 83 | 5 CURRENT POSITION XYZ = [ 8.1920 , -1.1421 , -0.7737 ] 84 | 6 CURRENT POSITION XYZ = [ 8.1539 , -1.1414 , -0.7847 ] 85 | 7 CURRENT POSITION XYZ = [ 8.1522 , -1.1417 , -0.7820 ] 86 | 87 | DISABLE OFFBOARD MODE 88 | 89 | READ SOME MESSAGES 90 | Got message LOCAL_POSITION_NED (spec: https://mavlink.io/en/messages/common.html#LOCAL_POSITION_NED) 91 | pos (NED): 8.152975 -1.141093 -0.784075 (m) 92 | Got message HIGHRES_IMU (spec: https://mavlink.io/en/messages/common.html#HIGHRES_IMU) 93 | ap time: 3611390110 94 | acc (NED): 0.005503 0.044659 -9.740363 (m/s^2) 95 | gyro (NED): -0.003064 0.003857 0.000005 (rad/s) 96 | mag (NED): -0.117767 -0.335362 -0.253204 (Ga) 97 | baro: 1020.519958 (mBar) 98 | altitude: -60.341393 (m) 99 | temperature: 46.779999 C 100 | 101 | CLOSE THREADS 102 | 103 | CLOSE PORT 104 | ``` 105 | 106 | Exploration 107 | =========== 108 | 109 | There are a few things to explore past this example. 110 | 111 | First you can connect via: 112 | * a [Telemetry Radio](https://docs.px4.io/en/telemetry/) on TELEM1 or 2 113 | * an [FTDI cable](https://www.sparkfun.com/products/9718) on TELEM2 or Serial 4 114 | 115 | > **Note** 116 | > * Serial 5 can't be used to receive data without reconfiguration (its receive pin is occupied by a second NuttX shell). 117 | > * TELEM1 is typically dedicated to Telemetry Radio, but if you're using another port you will need to make sure it is not configured for use by another peripheral. 118 | > * If using FTDI with a TELEM port, connect all the pins to corresponding pins on port. 119 | > * If using FTDI with SERIAL4 connect the TX, RX GND and 5V pins (CTS, RTS need not be connected). 120 | 121 | 122 | With this you'll be able to start a second port for communication, and leave the USB port available for viewing prints in the NuttX shell. 123 | 124 | For steps 2 and 3 from the above tutorial, you'll use a different port. On the off-board computer side, the port might now be `/dev/ttyUSB0`. On the Pixhawk side, here the port mappings are in the table below. 125 | 126 | | PX4 UART | NuttX UART | 127 | |----------|------------| 128 | | Telem 1 | /dev/ttyS1 | 129 | | Telem 2 | /dev/ttyS2 | 130 | | Serial 4 | /dev/ttyS6 | 131 | 132 | Now add a print statement in the Pixhawk Firmware to see received messages. Build and upload this to Pixhawk. 133 | 134 | ``` 135 | [Firmware/src/modules/mavlink/mavlink_receiver.cpp] 136 | /* if read failed, this loop won't execute */ 137 | for (ssize_t i = 0; i < nread; i++) { 138 | if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) { 139 | 140 | /* --- REPORT HANDLING OF MESSAGE --- */ 141 | printf("\n"); 142 | printf("HANDLE MESSAGE\n"); 143 | printf("MSGID:%i\n",msg.msgid); 144 | 145 | /* handle generic messages and commands */ 146 | handle_message(&msg); 147 | ``` 148 | 149 | Open the system terminal as described here: https://dev.px4.io/en/debug/system_console.html 150 | 151 | On the off-board side, in another terminal run the `c_uart_interface_example` executable. You should see output in the NuttX shell similar to this: 152 | 153 | ``` 154 | HANDLE MESSAGE 155 | MSGID:76 156 | 157 | HANDLE MESSAGE 158 | MSGID:84 159 | 160 | (...) 161 | 162 | HANDLE MESSAGE 163 | MSGID:84 164 | 165 | HANDLE MESSAGE 166 | MSGID:76 167 | ``` 168 | 169 | Past this, you can: 170 | - Modify the received message data type 171 | - Modify the sent message data type 172 | 173 | Simulation 174 | =========== 175 | 176 | There is also the possibility to connect this example to the simulator using: 177 | 178 | ``` 179 | $ ./mavlink_control -u 127.0.0.1 -a 180 | ``` 181 | The -a argument enables arming, takeoff and landing of the copter. Use this argument with care on a real copter! -------------------------------------------------------------------------------- /autopilot_interface.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2014 MAVlink Development Team. All rights reserved. 4 | * Author: Trent Lukaczyk, 5 | * Jaycee Lock, 6 | * Lorenz Meier, 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * 2. Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in 16 | * the documentation and/or other materials provided with the 17 | * distribution. 18 | * 3. Neither the name PX4 nor the names of its contributors may be 19 | * used to endorse or promote products derived from this software 20 | * without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 29 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 30 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | ****************************************************************************/ 36 | 37 | /** 38 | * @file autopilot_interface.cpp 39 | * 40 | * @brief Autopilot interface functions 41 | * 42 | * Functions for sending and recieving commands to an autopilot via MAVlink 43 | * 44 | * @author Trent Lukaczyk, 45 | * @author Jaycee Lock, 46 | * @author Lorenz Meier, 47 | * 48 | */ 49 | 50 | 51 | // ------------------------------------------------------------------------------ 52 | // Includes 53 | // ------------------------------------------------------------------------------ 54 | 55 | #include "autopilot_interface.h" 56 | 57 | 58 | // ---------------------------------------------------------------------------------- 59 | // Time 60 | // ------------------- --------------------------------------------------------------- 61 | uint64_t 62 | get_time_usec() 63 | { 64 | static struct timeval _time_stamp; 65 | gettimeofday(&_time_stamp, NULL); 66 | return _time_stamp.tv_sec*1000000 + _time_stamp.tv_usec; 67 | } 68 | 69 | 70 | // ---------------------------------------------------------------------------------- 71 | // Setpoint Helper Functions 72 | // ---------------------------------------------------------------------------------- 73 | 74 | // choose one of the next three 75 | 76 | /* 77 | * Set target local ned position 78 | * 79 | * Modifies a mavlink_set_position_target_local_ned_t struct with target XYZ locations 80 | * in the Local NED frame, in meters. 81 | */ 82 | void 83 | set_position(float x, float y, float z, mavlink_set_position_target_local_ned_t &sp) 84 | { 85 | sp.type_mask = 86 | MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_POSITION; 87 | 88 | sp.coordinate_frame = MAV_FRAME_LOCAL_NED; 89 | 90 | sp.x = x; 91 | sp.y = y; 92 | sp.z = z; 93 | 94 | printf("POSITION SETPOINT XYZ = [ %.4f , %.4f , %.4f ] \n", sp.x, sp.y, sp.z); 95 | 96 | } 97 | 98 | /* 99 | * Set target local ned velocity 100 | * 101 | * Modifies a mavlink_set_position_target_local_ned_t struct with target VX VY VZ 102 | * velocities in the Local NED frame, in meters per second. 103 | */ 104 | void 105 | set_velocity(float vx, float vy, float vz, mavlink_set_position_target_local_ned_t &sp) 106 | { 107 | sp.type_mask = 108 | MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_VELOCITY ; 109 | 110 | sp.coordinate_frame = MAV_FRAME_LOCAL_NED; 111 | 112 | sp.vx = vx; 113 | sp.vy = vy; 114 | sp.vz = vz; 115 | 116 | printf("VELOCITY SETPOINT UVW = [ %.4f , %.4f , %.4f ] \n", sp.vx, sp.vy, sp.vz); 117 | 118 | } 119 | 120 | /* 121 | * Set target local ned acceleration 122 | * 123 | * Modifies a mavlink_set_position_target_local_ned_t struct with target AX AY AZ 124 | * accelerations in the Local NED frame, in meters per second squared. 125 | */ 126 | void 127 | set_acceleration(float ax, float ay, float az, mavlink_set_position_target_local_ned_t &sp) 128 | { 129 | 130 | // NOT IMPLEMENTED 131 | fprintf(stderr,"set_acceleration doesn't work yet \n"); 132 | throw 1; 133 | 134 | 135 | sp.type_mask = 136 | MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_ACCELERATION & 137 | MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_VELOCITY ; 138 | 139 | sp.coordinate_frame = MAV_FRAME_LOCAL_NED; 140 | 141 | sp.afx = ax; 142 | sp.afy = ay; 143 | sp.afz = az; 144 | } 145 | 146 | // the next two need to be called after one of the above 147 | 148 | /* 149 | * Set target local ned yaw 150 | * 151 | * Modifies a mavlink_set_position_target_local_ned_t struct with a target yaw 152 | * in the Local NED frame, in radians. 153 | */ 154 | void 155 | set_yaw(float yaw, mavlink_set_position_target_local_ned_t &sp) 156 | { 157 | sp.type_mask &= 158 | MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_ANGLE ; 159 | 160 | sp.yaw = yaw; 161 | 162 | printf("POSITION SETPOINT YAW = %.4f \n", sp.yaw); 163 | 164 | } 165 | 166 | /* 167 | * Set target local ned yaw rate 168 | * 169 | * Modifies a mavlink_set_position_target_local_ned_t struct with a target yaw rate 170 | * in the Local NED frame, in radians per second. 171 | */ 172 | void 173 | set_yaw_rate(float yaw_rate, mavlink_set_position_target_local_ned_t &sp) 174 | { 175 | sp.type_mask &= 176 | MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_RATE ; 177 | 178 | sp.yaw_rate = yaw_rate; 179 | } 180 | 181 | 182 | // ---------------------------------------------------------------------------------- 183 | // Autopilot Interface Class 184 | // ---------------------------------------------------------------------------------- 185 | 186 | // ------------------------------------------------------------------------------ 187 | // Con/De structors 188 | // ------------------------------------------------------------------------------ 189 | Autopilot_Interface:: 190 | Autopilot_Interface(Generic_Port *port_) 191 | { 192 | // initialize attributes 193 | write_count = 0; 194 | 195 | reading_status = 0; // whether the read thread is running 196 | writing_status = 0; // whether the write thread is running 197 | control_status = 0; // whether the autopilot is in offboard control mode 198 | time_to_exit = false; // flag to signal thread exit 199 | 200 | read_tid = 0; // read thread id 201 | write_tid = 0; // write thread id 202 | 203 | system_id = 0; // system id 204 | autopilot_id = 0; // autopilot component id 205 | companion_id = 0; // companion computer component id 206 | 207 | current_messages.sysid = system_id; 208 | current_messages.compid = autopilot_id; 209 | 210 | port = port_; // port management object 211 | 212 | } 213 | 214 | Autopilot_Interface:: 215 | ~Autopilot_Interface() 216 | {} 217 | 218 | 219 | // ------------------------------------------------------------------------------ 220 | // Update Setpoint 221 | // ------------------------------------------------------------------------------ 222 | void 223 | Autopilot_Interface:: 224 | update_setpoint(mavlink_set_position_target_local_ned_t setpoint) 225 | { 226 | std::lock_guard lock(current_setpoint.mutex); 227 | current_setpoint.data = setpoint; 228 | } 229 | 230 | 231 | // ------------------------------------------------------------------------------ 232 | // Read Messages 233 | // ------------------------------------------------------------------------------ 234 | void 235 | Autopilot_Interface:: 236 | read_messages() 237 | { 238 | bool success; // receive success flag 239 | bool received_all = false; // receive only one message 240 | Time_Stamps this_timestamps; 241 | 242 | // Blocking wait for new data 243 | while ( !received_all and !time_to_exit ) 244 | { 245 | // ---------------------------------------------------------------------- 246 | // READ MESSAGE 247 | // ---------------------------------------------------------------------- 248 | mavlink_message_t message; 249 | success = port->read_message(message); 250 | 251 | // ---------------------------------------------------------------------- 252 | // HANDLE MESSAGE 253 | // ---------------------------------------------------------------------- 254 | if( success ) 255 | { 256 | 257 | // Store message sysid and compid. 258 | // Note this doesn't handle multiple message sources. 259 | current_messages.sysid = message.sysid; 260 | current_messages.compid = message.compid; 261 | 262 | // Handle Message ID 263 | switch (message.msgid) 264 | { 265 | 266 | case MAVLINK_MSG_ID_HEARTBEAT: 267 | { 268 | //printf("MAVLINK_MSG_ID_HEARTBEAT\n"); 269 | mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat)); 270 | current_messages.time_stamps.heartbeat = get_time_usec(); 271 | this_timestamps.heartbeat = current_messages.time_stamps.heartbeat; 272 | break; 273 | } 274 | 275 | case MAVLINK_MSG_ID_SYS_STATUS: 276 | { 277 | //printf("MAVLINK_MSG_ID_SYS_STATUS\n"); 278 | mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status)); 279 | current_messages.time_stamps.sys_status = get_time_usec(); 280 | this_timestamps.sys_status = current_messages.time_stamps.sys_status; 281 | break; 282 | } 283 | 284 | case MAVLINK_MSG_ID_BATTERY_STATUS: 285 | { 286 | //printf("MAVLINK_MSG_ID_BATTERY_STATUS\n"); 287 | mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status)); 288 | current_messages.time_stamps.battery_status = get_time_usec(); 289 | this_timestamps.battery_status = current_messages.time_stamps.battery_status; 290 | break; 291 | } 292 | 293 | case MAVLINK_MSG_ID_RADIO_STATUS: 294 | { 295 | //printf("MAVLINK_MSG_ID_RADIO_STATUS\n"); 296 | mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status)); 297 | current_messages.time_stamps.radio_status = get_time_usec(); 298 | this_timestamps.radio_status = current_messages.time_stamps.radio_status; 299 | break; 300 | } 301 | 302 | case MAVLINK_MSG_ID_LOCAL_POSITION_NED: 303 | { 304 | //printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n"); 305 | mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned)); 306 | current_messages.time_stamps.local_position_ned = get_time_usec(); 307 | this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned; 308 | break; 309 | } 310 | 311 | case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: 312 | { 313 | //printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n"); 314 | mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int)); 315 | current_messages.time_stamps.global_position_int = get_time_usec(); 316 | this_timestamps.global_position_int = current_messages.time_stamps.global_position_int; 317 | break; 318 | } 319 | 320 | case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED: 321 | { 322 | //printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n"); 323 | mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned)); 324 | current_messages.time_stamps.position_target_local_ned = get_time_usec(); 325 | this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned; 326 | break; 327 | } 328 | 329 | case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT: 330 | { 331 | //printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n"); 332 | mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int)); 333 | current_messages.time_stamps.position_target_global_int = get_time_usec(); 334 | this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int; 335 | break; 336 | } 337 | 338 | case MAVLINK_MSG_ID_HIGHRES_IMU: 339 | { 340 | //printf("MAVLINK_MSG_ID_HIGHRES_IMU\n"); 341 | mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu)); 342 | current_messages.time_stamps.highres_imu = get_time_usec(); 343 | this_timestamps.highres_imu = current_messages.time_stamps.highres_imu; 344 | break; 345 | } 346 | 347 | case MAVLINK_MSG_ID_ATTITUDE: 348 | { 349 | //printf("MAVLINK_MSG_ID_ATTITUDE\n"); 350 | mavlink_msg_attitude_decode(&message, &(current_messages.attitude)); 351 | current_messages.time_stamps.attitude = get_time_usec(); 352 | this_timestamps.attitude = current_messages.time_stamps.attitude; 353 | break; 354 | } 355 | 356 | default: 357 | { 358 | // printf("Warning, did not handle message id %i\n",message.msgid); 359 | break; 360 | } 361 | 362 | 363 | } // end: switch msgid 364 | 365 | } // end: if read message 366 | 367 | // Check for receipt of all items 368 | received_all = 369 | this_timestamps.heartbeat && 370 | // this_timestamps.battery_status && 371 | // this_timestamps.radio_status && 372 | // this_timestamps.local_position_ned && 373 | // this_timestamps.global_position_int && 374 | // this_timestamps.position_target_local_ned && 375 | // this_timestamps.position_target_global_int && 376 | // this_timestamps.highres_imu && 377 | // this_timestamps.attitude && 378 | this_timestamps.sys_status 379 | ; 380 | 381 | // give the write thread time to use the port 382 | if ( writing_status > false ) { 383 | usleep(100); // look for components of batches at 10kHz 384 | } 385 | 386 | } // end: while not received all 387 | 388 | return; 389 | } 390 | 391 | // ------------------------------------------------------------------------------ 392 | // Write Message 393 | // ------------------------------------------------------------------------------ 394 | int 395 | Autopilot_Interface:: 396 | write_message(mavlink_message_t message) 397 | { 398 | // do the write 399 | int len = port->write_message(message); 400 | 401 | // book keep 402 | write_count++; 403 | 404 | // Done! 405 | return len; 406 | } 407 | 408 | // ------------------------------------------------------------------------------ 409 | // Write Setpoint Message 410 | // ------------------------------------------------------------------------------ 411 | void 412 | Autopilot_Interface:: 413 | write_setpoint() 414 | { 415 | // -------------------------------------------------------------------------- 416 | // PACK PAYLOAD 417 | // -------------------------------------------------------------------------- 418 | 419 | // pull from position target 420 | mavlink_set_position_target_local_ned_t sp; 421 | { 422 | std::lock_guard lock(current_setpoint.mutex); 423 | sp = current_setpoint.data; 424 | } 425 | 426 | // double check some system parameters 427 | if ( not sp.time_boot_ms ) 428 | sp.time_boot_ms = (uint32_t) (get_time_usec()/1000); 429 | sp.target_system = system_id; 430 | sp.target_component = autopilot_id; 431 | 432 | 433 | // -------------------------------------------------------------------------- 434 | // ENCODE 435 | // -------------------------------------------------------------------------- 436 | 437 | mavlink_message_t message; 438 | mavlink_msg_set_position_target_local_ned_encode(system_id, companion_id, &message, &sp); 439 | 440 | 441 | // -------------------------------------------------------------------------- 442 | // WRITE 443 | // -------------------------------------------------------------------------- 444 | 445 | // do the write 446 | int len = write_message(message); 447 | 448 | // check the write 449 | if ( len <= 0 ) 450 | fprintf(stderr,"WARNING: could not send POSITION_TARGET_LOCAL_NED \n"); 451 | // else 452 | // printf("%lu POSITION_TARGET = [ %f , %f , %f ] \n", write_count, position_target.x, position_target.y, position_target.z); 453 | 454 | return; 455 | } 456 | 457 | 458 | // ------------------------------------------------------------------------------ 459 | // Start Off-Board Mode 460 | // ------------------------------------------------------------------------------ 461 | void 462 | Autopilot_Interface:: 463 | enable_offboard_control() 464 | { 465 | // Should only send this command once 466 | if ( control_status == false ) 467 | { 468 | printf("ENABLE OFFBOARD MODE\n"); 469 | 470 | // ---------------------------------------------------------------------- 471 | // TOGGLE OFF-BOARD MODE 472 | // ---------------------------------------------------------------------- 473 | 474 | // Sends the command to go off-board 475 | int success = toggle_offboard_control( true ); 476 | 477 | // Check the command was written 478 | if ( success ) 479 | control_status = true; 480 | else 481 | { 482 | fprintf(stderr,"Error: off-board mode not set, could not write message\n"); 483 | //throw EXIT_FAILURE; 484 | } 485 | 486 | printf("\n"); 487 | 488 | } // end: if not offboard_status 489 | 490 | } 491 | 492 | 493 | // ------------------------------------------------------------------------------ 494 | // Stop Off-Board Mode 495 | // ------------------------------------------------------------------------------ 496 | void 497 | Autopilot_Interface:: 498 | disable_offboard_control() 499 | { 500 | 501 | // Should only send this command once 502 | if ( control_status == true ) 503 | { 504 | printf("DISABLE OFFBOARD MODE\n"); 505 | 506 | // ---------------------------------------------------------------------- 507 | // TOGGLE OFF-BOARD MODE 508 | // ---------------------------------------------------------------------- 509 | 510 | // Sends the command to stop off-board 511 | int success = toggle_offboard_control( false ); 512 | 513 | // Check the command was written 514 | if ( success ) 515 | control_status = false; 516 | else 517 | { 518 | fprintf(stderr,"Error: off-board mode not set, could not write message\n"); 519 | //throw EXIT_FAILURE; 520 | } 521 | 522 | printf("\n"); 523 | 524 | } // end: if offboard_status 525 | 526 | } 527 | 528 | // ------------------------------------------------------------------------------ 529 | // Arm 530 | // ------------------------------------------------------------------------------ 531 | int 532 | Autopilot_Interface:: 533 | arm_disarm( bool flag ) 534 | { 535 | if(flag) 536 | { 537 | printf("ARM ROTORS\n"); 538 | } 539 | else 540 | { 541 | printf("DISARM ROTORS\n"); 542 | } 543 | 544 | // Prepare command for off-board mode 545 | mavlink_command_long_t com = { 0 }; 546 | com.target_system = system_id; 547 | com.target_component = autopilot_id; 548 | com.command = MAV_CMD_COMPONENT_ARM_DISARM; 549 | com.confirmation = true; 550 | com.param1 = (float) flag; 551 | com.param2 = 21196; 552 | 553 | // Encode 554 | mavlink_message_t message; 555 | mavlink_msg_command_long_encode(system_id, companion_id, &message, &com); 556 | 557 | // Send the message 558 | int len = port->write_message(message); 559 | 560 | // Done! 561 | return len; 562 | } 563 | 564 | // ------------------------------------------------------------------------------ 565 | // Toggle Off-Board Mode 566 | // ------------------------------------------------------------------------------ 567 | int 568 | Autopilot_Interface:: 569 | toggle_offboard_control( bool flag ) 570 | { 571 | // Prepare command for off-board mode 572 | mavlink_command_long_t com = { 0 }; 573 | com.target_system = system_id; 574 | com.target_component = autopilot_id; 575 | com.command = MAV_CMD_NAV_GUIDED_ENABLE; 576 | com.confirmation = true; 577 | com.param1 = (float) flag; // flag >0.5 => start, <0.5 => stop 578 | 579 | // Encode 580 | mavlink_message_t message; 581 | mavlink_msg_command_long_encode(system_id, companion_id, &message, &com); 582 | 583 | // Send the message 584 | int len = port->write_message(message); 585 | 586 | // Done! 587 | return len; 588 | } 589 | 590 | 591 | // ------------------------------------------------------------------------------ 592 | // STARTUP 593 | // ------------------------------------------------------------------------------ 594 | void 595 | Autopilot_Interface:: 596 | start() 597 | { 598 | int result; 599 | 600 | // -------------------------------------------------------------------------- 601 | // CHECK PORT 602 | // -------------------------------------------------------------------------- 603 | 604 | if ( !port->is_running() ) // PORT_OPEN 605 | { 606 | fprintf(stderr,"ERROR: port not open\n"); 607 | throw 1; 608 | } 609 | 610 | 611 | // -------------------------------------------------------------------------- 612 | // READ THREAD 613 | // -------------------------------------------------------------------------- 614 | 615 | printf("START READ THREAD \n"); 616 | 617 | result = pthread_create( &read_tid, NULL, &start_autopilot_interface_read_thread, this ); 618 | if ( result ) throw result; 619 | 620 | // now we're reading messages 621 | printf("\n"); 622 | 623 | 624 | // -------------------------------------------------------------------------- 625 | // CHECK FOR MESSAGES 626 | // -------------------------------------------------------------------------- 627 | 628 | printf("CHECK FOR MESSAGES\n"); 629 | 630 | while ( not current_messages.sysid ) 631 | { 632 | if ( time_to_exit ) 633 | return; 634 | usleep(500000); // check at 2Hz 635 | } 636 | 637 | printf("Found\n"); 638 | 639 | // now we know autopilot is sending messages 640 | printf("\n"); 641 | 642 | 643 | // -------------------------------------------------------------------------- 644 | // GET SYSTEM and COMPONENT IDs 645 | // -------------------------------------------------------------------------- 646 | 647 | // This comes from the heartbeat, which in theory should only come from 648 | // the autopilot we're directly connected to it. If there is more than one 649 | // vehicle then we can't expect to discover id's like this. 650 | // In which case set the id's manually. 651 | 652 | // System ID 653 | if ( not system_id ) 654 | { 655 | system_id = current_messages.sysid; 656 | printf("GOT VEHICLE SYSTEM ID: %i\n", system_id ); 657 | } 658 | 659 | // Component ID 660 | if ( not autopilot_id ) 661 | { 662 | autopilot_id = current_messages.compid; 663 | printf("GOT AUTOPILOT COMPONENT ID: %i\n", autopilot_id); 664 | printf("\n"); 665 | } 666 | 667 | 668 | // -------------------------------------------------------------------------- 669 | // GET INITIAL POSITION 670 | // -------------------------------------------------------------------------- 671 | 672 | // Wait for initial position ned 673 | while ( not ( current_messages.time_stamps.local_position_ned && 674 | current_messages.time_stamps.attitude ) ) 675 | { 676 | if ( time_to_exit ) 677 | return; 678 | usleep(500000); 679 | } 680 | 681 | // copy initial position ned 682 | Mavlink_Messages local_data = current_messages; 683 | initial_position.x = local_data.local_position_ned.x; 684 | initial_position.y = local_data.local_position_ned.y; 685 | initial_position.z = local_data.local_position_ned.z; 686 | initial_position.vx = local_data.local_position_ned.vx; 687 | initial_position.vy = local_data.local_position_ned.vy; 688 | initial_position.vz = local_data.local_position_ned.vz; 689 | initial_position.yaw = local_data.attitude.yaw; 690 | initial_position.yaw_rate = local_data.attitude.yawspeed; 691 | 692 | printf("INITIAL POSITION XYZ = [ %.4f , %.4f , %.4f ] \n", initial_position.x, initial_position.y, initial_position.z); 693 | printf("INITIAL POSITION YAW = %.4f \n", initial_position.yaw); 694 | printf("\n"); 695 | 696 | // we need this before starting the write thread 697 | 698 | 699 | // -------------------------------------------------------------------------- 700 | // WRITE THREAD 701 | // -------------------------------------------------------------------------- 702 | printf("START WRITE THREAD \n"); 703 | 704 | result = pthread_create( &write_tid, NULL, &start_autopilot_interface_write_thread, this ); 705 | if ( result ) throw result; 706 | 707 | // wait for it to be started 708 | while ( not writing_status ) 709 | usleep(100000); // 10Hz 710 | 711 | // now we're streaming setpoint commands 712 | printf("\n"); 713 | 714 | 715 | // Done! 716 | return; 717 | 718 | } 719 | 720 | 721 | // ------------------------------------------------------------------------------ 722 | // SHUTDOWN 723 | // ------------------------------------------------------------------------------ 724 | void 725 | Autopilot_Interface:: 726 | stop() 727 | { 728 | // -------------------------------------------------------------------------- 729 | // CLOSE THREADS 730 | // -------------------------------------------------------------------------- 731 | printf("CLOSE THREADS\n"); 732 | 733 | // signal exit 734 | time_to_exit = true; 735 | 736 | // wait for exit 737 | pthread_join(read_tid ,NULL); 738 | pthread_join(write_tid,NULL); 739 | 740 | // now the read and write threads are closed 741 | printf("\n"); 742 | 743 | // still need to close the port separately 744 | } 745 | 746 | // ------------------------------------------------------------------------------ 747 | // Read Thread 748 | // ------------------------------------------------------------------------------ 749 | void 750 | Autopilot_Interface:: 751 | start_read_thread() 752 | { 753 | 754 | if ( reading_status != 0 ) 755 | { 756 | fprintf(stderr,"read thread already running\n"); 757 | return; 758 | } 759 | else 760 | { 761 | read_thread(); 762 | return; 763 | } 764 | 765 | } 766 | 767 | 768 | // ------------------------------------------------------------------------------ 769 | // Write Thread 770 | // ------------------------------------------------------------------------------ 771 | void 772 | Autopilot_Interface:: 773 | start_write_thread(void) 774 | { 775 | if ( not writing_status == false ) 776 | { 777 | fprintf(stderr,"write thread already running\n"); 778 | return; 779 | } 780 | 781 | else 782 | { 783 | write_thread(); 784 | return; 785 | } 786 | 787 | } 788 | 789 | 790 | // ------------------------------------------------------------------------------ 791 | // Quit Handler 792 | // ------------------------------------------------------------------------------ 793 | void 794 | Autopilot_Interface:: 795 | handle_quit( int sig ) 796 | { 797 | 798 | disable_offboard_control(); 799 | 800 | try { 801 | stop(); 802 | 803 | } 804 | catch (int error) { 805 | fprintf(stderr,"Warning, could not stop autopilot interface\n"); 806 | } 807 | 808 | } 809 | 810 | 811 | 812 | // ------------------------------------------------------------------------------ 813 | // Read Thread 814 | // ------------------------------------------------------------------------------ 815 | void 816 | Autopilot_Interface:: 817 | read_thread() 818 | { 819 | reading_status = true; 820 | 821 | while ( ! time_to_exit ) 822 | { 823 | read_messages(); 824 | usleep(100000); // Read batches at 10Hz 825 | } 826 | 827 | reading_status = false; 828 | 829 | return; 830 | } 831 | 832 | 833 | // ------------------------------------------------------------------------------ 834 | // Write Thread 835 | // ------------------------------------------------------------------------------ 836 | void 837 | Autopilot_Interface:: 838 | write_thread(void) 839 | { 840 | // signal startup 841 | writing_status = 2; 842 | 843 | // prepare an initial setpoint, just stay put 844 | mavlink_set_position_target_local_ned_t sp; 845 | sp.type_mask = MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_VELOCITY & 846 | MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_RATE; 847 | sp.coordinate_frame = MAV_FRAME_LOCAL_NED; 848 | sp.vx = 0.0; 849 | sp.vy = 0.0; 850 | sp.vz = 0.0; 851 | sp.yaw_rate = 0.0; 852 | 853 | // set position target 854 | { 855 | std::lock_guard lock(current_setpoint.mutex); 856 | current_setpoint.data = sp; 857 | } 858 | 859 | // write a message and signal writing 860 | write_setpoint(); 861 | writing_status = true; 862 | 863 | // Pixhawk needs to see off-board commands at minimum 2Hz, 864 | // otherwise it will go into fail safe 865 | while ( !time_to_exit ) 866 | { 867 | usleep(250000); // Stream at 4Hz 868 | write_setpoint(); 869 | } 870 | 871 | // signal end 872 | writing_status = false; 873 | 874 | return; 875 | 876 | } 877 | 878 | // End Autopilot_Interface 879 | 880 | 881 | // ------------------------------------------------------------------------------ 882 | // Pthread Starter Helper Functions 883 | // ------------------------------------------------------------------------------ 884 | 885 | void* 886 | start_autopilot_interface_read_thread(void *args) 887 | { 888 | // takes an autopilot object argument 889 | Autopilot_Interface *autopilot_interface = (Autopilot_Interface *)args; 890 | 891 | // run the object's read thread 892 | autopilot_interface->start_read_thread(); 893 | 894 | // done! 895 | return NULL; 896 | } 897 | 898 | void* 899 | start_autopilot_interface_write_thread(void *args) 900 | { 901 | // takes an autopilot object argument 902 | Autopilot_Interface *autopilot_interface = (Autopilot_Interface *)args; 903 | 904 | // run the object's read thread 905 | autopilot_interface->start_write_thread(); 906 | 907 | // done! 908 | return NULL; 909 | } 910 | 911 | 912 | 913 | -------------------------------------------------------------------------------- /autopilot_interface.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2014 MAVlink Development Team. All rights reserved. 4 | * Author: Trent Lukaczyk, 5 | * Jaycee Lock, 6 | * Lorenz Meier, 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * 2. Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in 16 | * the documentation and/or other materials provided with the 17 | * distribution. 18 | * 3. Neither the name PX4 nor the names of its contributors may be 19 | * used to endorse or promote products derived from this software 20 | * without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 29 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 30 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | ****************************************************************************/ 36 | 37 | /** 38 | * @file autopilot_interface.h 39 | * 40 | * @brief Autopilot interface definition 41 | * 42 | * Functions for sending and recieving commands to an autopilot via MAVlink 43 | * 44 | * @author Trent Lukaczyk, 45 | * @author Jaycee Lock, 46 | * @author Lorenz Meier, 47 | * 48 | */ 49 | 50 | 51 | #ifndef AUTOPILOT_INTERFACE_H_ 52 | #define AUTOPILOT_INTERFACE_H_ 53 | 54 | // ------------------------------------------------------------------------------ 55 | // Includes 56 | // ------------------------------------------------------------------------------ 57 | 58 | #include "generic_port.h" 59 | 60 | #include 61 | #include 62 | #include 63 | #include // This uses POSIX Threads 64 | #include // UNIX standard function definitions 65 | #include 66 | 67 | #include 68 | 69 | // ------------------------------------------------------------------------------ 70 | // Defines 71 | // ------------------------------------------------------------------------------ 72 | 73 | /** 74 | * Defines for mavlink_set_position_target_local_ned_t.type_mask 75 | * 76 | * Bitmask to indicate which dimensions should be ignored by the vehicle 77 | * 78 | * a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of 79 | * the setpoint dimensions should be ignored. 80 | * 81 | * If bit 10 is set the floats afx afy afz should be interpreted as force 82 | * instead of acceleration. 83 | * 84 | * Mapping: 85 | * bit 1: x, 86 | * bit 2: y, 87 | * bit 3: z, 88 | * bit 4: vx, 89 | * bit 5: vy, 90 | * bit 6: vz, 91 | * bit 7: ax, 92 | * bit 8: ay, 93 | * bit 9: az, 94 | * bit 10: is force setpoint, 95 | * bit 11: yaw, 96 | * bit 12: yaw rate 97 | * remaining bits unused 98 | * 99 | * Combine bitmasks with bitwise & 100 | * 101 | * Example for position and yaw angle: 102 | * uint16_t type_mask = 103 | * MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_POSITION & 104 | * MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_ANGLE; 105 | */ 106 | 107 | // bit number 876543210987654321 108 | #define MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_POSITION 0b0000110111111000 109 | #define MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_VELOCITY 0b0000110111000111 110 | #define MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_ACCELERATION 0b0000110000111111 111 | #define MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_FORCE 0b0000111000111111 112 | #define MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_ANGLE 0b0000100111111111 113 | #define MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_RATE 0b0000010111111111 114 | 115 | #define MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_TAKEOFF 0x1000 116 | #define MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_LAND 0x2000 117 | #define MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_LOITER 0x3000 118 | #define MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_IDLE 0x4000 119 | 120 | // ------------------------------------------------------------------------------ 121 | // Prototypes 122 | // ------------------------------------------------------------------------------ 123 | 124 | 125 | // helper functions 126 | uint64_t get_time_usec(); 127 | void set_position(float x, float y, float z, mavlink_set_position_target_local_ned_t &sp); 128 | void set_velocity(float vx, float vy, float vz, mavlink_set_position_target_local_ned_t &sp); 129 | void set_acceleration(float ax, float ay, float az, mavlink_set_position_target_local_ned_t &sp); 130 | void set_yaw(float yaw, mavlink_set_position_target_local_ned_t &sp); 131 | void set_yaw_rate(float yaw_rate, mavlink_set_position_target_local_ned_t &sp); 132 | 133 | void* start_autopilot_interface_read_thread(void *args); 134 | void* start_autopilot_interface_write_thread(void *args); 135 | 136 | 137 | // ------------------------------------------------------------------------------ 138 | // Data Structures 139 | // ------------------------------------------------------------------------------ 140 | 141 | struct Time_Stamps 142 | { 143 | Time_Stamps() 144 | { 145 | reset_timestamps(); 146 | } 147 | 148 | uint64_t heartbeat; 149 | uint64_t sys_status; 150 | uint64_t battery_status; 151 | uint64_t radio_status; 152 | uint64_t local_position_ned; 153 | uint64_t global_position_int; 154 | uint64_t position_target_local_ned; 155 | uint64_t position_target_global_int; 156 | uint64_t highres_imu; 157 | uint64_t attitude; 158 | 159 | void 160 | reset_timestamps() 161 | { 162 | heartbeat = 0; 163 | sys_status = 0; 164 | battery_status = 0; 165 | radio_status = 0; 166 | local_position_ned = 0; 167 | global_position_int = 0; 168 | position_target_local_ned = 0; 169 | position_target_global_int = 0; 170 | highres_imu = 0; 171 | attitude = 0; 172 | } 173 | 174 | }; 175 | 176 | 177 | // Struct containing information on the MAV we are currently connected to 178 | 179 | struct Mavlink_Messages { 180 | 181 | int sysid; 182 | int compid; 183 | 184 | // Heartbeat 185 | mavlink_heartbeat_t heartbeat; 186 | 187 | // System Status 188 | mavlink_sys_status_t sys_status; 189 | 190 | // Battery Status 191 | mavlink_battery_status_t battery_status; 192 | 193 | // Radio Status 194 | mavlink_radio_status_t radio_status; 195 | 196 | // Local Position 197 | mavlink_local_position_ned_t local_position_ned; 198 | 199 | // Global Position 200 | mavlink_global_position_int_t global_position_int; 201 | 202 | // Local Position Target 203 | mavlink_position_target_local_ned_t position_target_local_ned; 204 | 205 | // Global Position Target 206 | mavlink_position_target_global_int_t position_target_global_int; 207 | 208 | // HiRes IMU 209 | mavlink_highres_imu_t highres_imu; 210 | 211 | // Attitude 212 | mavlink_attitude_t attitude; 213 | 214 | // System Parameters? 215 | 216 | 217 | // Time Stamps 218 | Time_Stamps time_stamps; 219 | 220 | void 221 | reset_timestamps() 222 | { 223 | time_stamps.reset_timestamps(); 224 | } 225 | 226 | }; 227 | 228 | 229 | // ---------------------------------------------------------------------------------- 230 | // Autopilot Interface Class 231 | // ---------------------------------------------------------------------------------- 232 | /* 233 | * Autopilot Interface Class 234 | * 235 | * This starts two threads for read and write over MAVlink. The read thread 236 | * listens for any MAVlink message and pushes it to the current_messages 237 | * attribute. The write thread at the moment only streams a position target 238 | * in the local NED frame (mavlink_set_position_target_local_ned_t), which 239 | * is changed by using the method update_setpoint(). Sending these messages 240 | * are only half the requirement to get response from the autopilot, a signal 241 | * to enter "offboard_control" mode is sent by using the enable_offboard_control() 242 | * method. Signal the exit of this mode with disable_offboard_control(). It's 243 | * important that one way or another this program signals offboard mode exit, 244 | * otherwise the vehicle will go into failsafe. 245 | */ 246 | class Autopilot_Interface 247 | { 248 | 249 | public: 250 | 251 | Autopilot_Interface(); 252 | Autopilot_Interface(Generic_Port *port_); 253 | ~Autopilot_Interface(); 254 | 255 | char reading_status; 256 | char writing_status; 257 | char control_status; 258 | uint64_t write_count; 259 | 260 | int system_id; 261 | int autopilot_id; 262 | int companion_id; 263 | 264 | Mavlink_Messages current_messages; 265 | mavlink_set_position_target_local_ned_t initial_position; 266 | 267 | void update_setpoint(mavlink_set_position_target_local_ned_t setpoint); 268 | void read_messages(); 269 | int write_message(mavlink_message_t message); 270 | 271 | int arm_disarm( bool flag ); 272 | void enable_offboard_control(); 273 | void disable_offboard_control(); 274 | 275 | void start(); 276 | void stop(); 277 | 278 | void start_read_thread(); 279 | void start_write_thread(void); 280 | 281 | void handle_quit( int sig ); 282 | 283 | 284 | private: 285 | 286 | Generic_Port *port; 287 | 288 | bool time_to_exit; 289 | 290 | pthread_t read_tid; 291 | pthread_t write_tid; 292 | 293 | struct { 294 | std::mutex mutex; 295 | mavlink_set_position_target_local_ned_t data; 296 | } current_setpoint; 297 | 298 | void read_thread(); 299 | void write_thread(void); 300 | 301 | int toggle_offboard_control( bool flag ); 302 | void write_setpoint(); 303 | 304 | }; 305 | 306 | 307 | 308 | #endif // AUTOPILOT_INTERFACE_H_ 309 | 310 | 311 | -------------------------------------------------------------------------------- /generic_port.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018 MAVlink Development Team. All rights reserved. 4 | * Author: Hannes Diethelm, 5 | * Trent Lukaczyk, 6 | * Jaycee Lock, 7 | * Lorenz Meier, 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * 2. Redistributions in binary form must reproduce the above copyright 16 | * notice, this list of conditions and the following disclaimer in 17 | * the documentation and/or other materials provided with the 18 | * distribution. 19 | * 3. Neither the name PX4 nor the names of its contributors may be 20 | * used to endorse or promote products derived from this software 21 | * without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 30 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 31 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | ****************************************************************************/ 37 | 38 | /** 39 | * @file generic_port.h 40 | * 41 | * @brief Generic interface definition 42 | * 43 | * Abstract port definition 44 | * 45 | * @author Hannes Diethelm, 46 | * @author Trent Lukaczyk, 47 | * @author Jaycee Lock, 48 | * @author Lorenz Meier, 49 | * 50 | */ 51 | 52 | #ifndef GENERIC_PORT_H_ 53 | #define GENERIC_PORT_H_ 54 | 55 | // ------------------------------------------------------------------------------ 56 | // Includes 57 | // ------------------------------------------------------------------------------ 58 | 59 | #include 60 | 61 | // ------------------------------------------------------------------------------ 62 | // Defines 63 | // ------------------------------------------------------------------------------ 64 | 65 | // ------------------------------------------------------------------------------ 66 | // Prototypes 67 | // ------------------------------------------------------------------------------ 68 | 69 | // ---------------------------------------------------------------------------------- 70 | // Generic Port Manager Class 71 | // ---------------------------------------------------------------------------------- 72 | /* 73 | * Generic Port Class 74 | * 75 | * This is an abstract port definition to handle both serial and UDP ports. 76 | */ 77 | class Generic_Port 78 | { 79 | public: 80 | Generic_Port(){}; 81 | virtual ~Generic_Port(){}; 82 | virtual int read_message(mavlink_message_t &message)=0; 83 | virtual int write_message(const mavlink_message_t &message)=0; 84 | virtual bool is_running()=0; 85 | virtual void start()=0; 86 | virtual void stop()=0; 87 | }; 88 | 89 | 90 | 91 | #endif // GENERIC_PORT_H_ 92 | 93 | 94 | -------------------------------------------------------------------------------- /makefile: -------------------------------------------------------------------------------- 1 | all: git_submodule mavlink_control 2 | 3 | mavlink_control: mavlink_control.cpp serial_port.cpp udp_port.cpp autopilot_interface.cpp 4 | g++ -g -Wall -I mavlink/include/mavlink/v2.0 mavlink_control.cpp serial_port.cpp udp_port.cpp autopilot_interface.cpp -o mavlink_control -lpthread 5 | 6 | git_submodule: 7 | git submodule update --init --recursive 8 | 9 | clean: 10 | rm -rf *o mavlink_control 11 | -------------------------------------------------------------------------------- /mavlink_control.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2014 MAVlink Development Team. All rights reserved. 4 | * Author: Trent Lukaczyk, 5 | * Jaycee Lock, 6 | * Lorenz Meier, 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * 2. Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in 16 | * the documentation and/or other materials provided with the 17 | * distribution. 18 | * 3. Neither the name PX4 nor the names of its contributors may be 19 | * used to endorse or promote products derived from this software 20 | * without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 29 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 30 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | ****************************************************************************/ 36 | 37 | /** 38 | * @file mavlink_control.cpp 39 | * 40 | * @brief An example offboard control process via mavlink 41 | * 42 | * This process connects an external MAVLink UART device to send an receive data 43 | * 44 | * @author Trent Lukaczyk, 45 | * @author Jaycee Lock, 46 | * @author Lorenz Meier, 47 | * 48 | */ 49 | 50 | 51 | 52 | // ------------------------------------------------------------------------------ 53 | // Includes 54 | // ------------------------------------------------------------------------------ 55 | 56 | #include "mavlink_control.h" 57 | 58 | 59 | // ------------------------------------------------------------------------------ 60 | // TOP 61 | // ------------------------------------------------------------------------------ 62 | int 63 | top (int argc, char **argv) 64 | { 65 | 66 | // -------------------------------------------------------------------------- 67 | // PARSE THE COMMANDS 68 | // -------------------------------------------------------------------------- 69 | 70 | // Default input arguments 71 | #ifdef __APPLE__ 72 | char *uart_name = (char*)"/dev/tty.usbmodem1"; 73 | #else 74 | char *uart_name = (char*)"/dev/ttyUSB0"; 75 | #endif 76 | int baudrate = 57600; 77 | 78 | bool use_udp = false; 79 | char *udp_ip = (char*)"127.0.0.1"; 80 | int udp_port = 14540; 81 | bool autotakeoff = false; 82 | 83 | // do the parse, will throw an int if it fails 84 | parse_commandline(argc, argv, uart_name, baudrate, use_udp, udp_ip, udp_port, autotakeoff); 85 | 86 | 87 | // -------------------------------------------------------------------------- 88 | // PORT and THREAD STARTUP 89 | // -------------------------------------------------------------------------- 90 | 91 | /* 92 | * Instantiate a generic port object 93 | * 94 | * This object handles the opening and closing of the offboard computer's 95 | * port over which it will communicate to an autopilot. It has 96 | * methods to read and write a mavlink_message_t object. To help with read 97 | * and write in the context of pthreading, it gaurds port operations with a 98 | * pthread mutex lock. It can be a serial or an UDP port. 99 | * 100 | */ 101 | Generic_Port *port; 102 | if(use_udp) 103 | { 104 | port = new UDP_Port(udp_ip, udp_port); 105 | } 106 | else 107 | { 108 | port = new Serial_Port(uart_name, baudrate); 109 | } 110 | 111 | 112 | /* 113 | * Instantiate an autopilot interface object 114 | * 115 | * This starts two threads for read and write over MAVlink. The read thread 116 | * listens for any MAVlink message and pushes it to the current_messages 117 | * attribute. The write thread at the moment only streams a position target 118 | * in the local NED frame (mavlink_set_position_target_local_ned_t), which 119 | * is changed by using the method update_setpoint(). Sending these messages 120 | * are only half the requirement to get response from the autopilot, a signal 121 | * to enter "offboard_control" mode is sent by using the enable_offboard_control() 122 | * method. Signal the exit of this mode with disable_offboard_control(). It's 123 | * important that one way or another this program signals offboard mode exit, 124 | * otherwise the vehicle will go into failsafe. 125 | * 126 | */ 127 | Autopilot_Interface autopilot_interface(port); 128 | 129 | /* 130 | * Setup interrupt signal handler 131 | * 132 | * Responds to early exits signaled with Ctrl-C. The handler will command 133 | * to exit offboard mode if required, and close threads and the port. 134 | * The handler in this example needs references to the above objects. 135 | * 136 | */ 137 | port_quit = port; 138 | autopilot_interface_quit = &autopilot_interface; 139 | signal(SIGINT,quit_handler); 140 | 141 | /* 142 | * Start the port and autopilot_interface 143 | * This is where the port is opened, and read and write threads are started. 144 | */ 145 | port->start(); 146 | autopilot_interface.start(); 147 | 148 | 149 | // -------------------------------------------------------------------------- 150 | // RUN COMMANDS 151 | // -------------------------------------------------------------------------- 152 | 153 | /* 154 | * Now we can implement the algorithm we want on top of the autopilot interface 155 | */ 156 | commands(autopilot_interface, autotakeoff); 157 | 158 | 159 | // -------------------------------------------------------------------------- 160 | // THREAD and PORT SHUTDOWN 161 | // -------------------------------------------------------------------------- 162 | 163 | /* 164 | * Now that we are done we can stop the threads and close the port 165 | */ 166 | autopilot_interface.stop(); 167 | port->stop(); 168 | 169 | delete port; 170 | 171 | // -------------------------------------------------------------------------- 172 | // DONE 173 | // -------------------------------------------------------------------------- 174 | 175 | // woot! 176 | return 0; 177 | 178 | } 179 | 180 | 181 | // ------------------------------------------------------------------------------ 182 | // COMMANDS 183 | // ------------------------------------------------------------------------------ 184 | 185 | void 186 | commands(Autopilot_Interface &api, bool autotakeoff) 187 | { 188 | 189 | // -------------------------------------------------------------------------- 190 | // START OFFBOARD MODE 191 | // -------------------------------------------------------------------------- 192 | 193 | api.enable_offboard_control(); 194 | usleep(100); // give some time to let it sink in 195 | 196 | // now the autopilot is accepting setpoint commands 197 | 198 | if(autotakeoff) 199 | { 200 | // arm autopilot 201 | api.arm_disarm(true); 202 | usleep(100); // give some time to let it sink in 203 | } 204 | 205 | // -------------------------------------------------------------------------- 206 | // SEND OFFBOARD COMMANDS 207 | // -------------------------------------------------------------------------- 208 | printf("SEND OFFBOARD COMMANDS\n"); 209 | 210 | // initialize command data strtuctures 211 | mavlink_set_position_target_local_ned_t sp; 212 | mavlink_set_position_target_local_ned_t ip = api.initial_position; 213 | 214 | // autopilot_interface.h provides some helper functions to build the command 215 | 216 | 217 | 218 | 219 | // Example 1 - Fly up by to 2m 220 | set_position( ip.x , // [m] 221 | ip.y , // [m] 222 | ip.z - 2.0 , // [m] 223 | sp ); 224 | 225 | if(autotakeoff) 226 | { 227 | sp.type_mask |= MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_TAKEOFF; 228 | } 229 | 230 | // SEND THE COMMAND 231 | api.update_setpoint(sp); 232 | // NOW pixhawk will try to move 233 | 234 | // Wait for 8 seconds, check position 235 | for (int i=0; i < 8; i++) 236 | { 237 | mavlink_local_position_ned_t pos = api.current_messages.local_position_ned; 238 | printf("%i CURRENT POSITION XYZ = [ % .4f , % .4f , % .4f ] \n", i, pos.x, pos.y, pos.z); 239 | sleep(1); 240 | } 241 | 242 | 243 | // Example 2 - Set Velocity 244 | set_velocity( -1.0 , // [m/s] 245 | -1.0 , // [m/s] 246 | 0.0 , // [m/s] 247 | sp ); 248 | 249 | // Example 2.1 - Append Yaw Command 250 | set_yaw( ip.yaw + 90.0/180.0*M_PI, // [rad] 251 | sp ); 252 | 253 | // SEND THE COMMAND 254 | api.update_setpoint(sp); 255 | // NOW pixhawk will try to move 256 | 257 | // Wait for 4 seconds, check position 258 | for (int i=0; i < 4; i++) 259 | { 260 | mavlink_local_position_ned_t pos = api.current_messages.local_position_ned; 261 | printf("%i CURRENT POSITION XYZ = [ % .4f , % .4f , % .4f ] \n", i, pos.x, pos.y, pos.z); 262 | sleep(1); 263 | } 264 | 265 | if(autotakeoff) 266 | { 267 | // Example 3 - Land using fixed velocity 268 | set_velocity( 0.0 , // [m/s] 269 | 0.0 , // [m/s] 270 | 1.0 , // [m/s] 271 | sp ); 272 | 273 | sp.type_mask |= MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_LAND; 274 | 275 | // SEND THE COMMAND 276 | api.update_setpoint(sp); 277 | // NOW pixhawk will try to move 278 | 279 | // Wait for 8 seconds, check position 280 | for (int i=0; i < 8; i++) 281 | { 282 | mavlink_local_position_ned_t pos = api.current_messages.local_position_ned; 283 | printf("%i CURRENT POSITION XYZ = [ % .4f , % .4f , % .4f ] \n", i, pos.x, pos.y, pos.z); 284 | sleep(1); 285 | } 286 | 287 | printf("\n"); 288 | 289 | // disarm autopilot 290 | api.arm_disarm(false); 291 | usleep(100); // give some time to let it sink in 292 | } 293 | 294 | // -------------------------------------------------------------------------- 295 | // STOP OFFBOARD MODE 296 | // -------------------------------------------------------------------------- 297 | 298 | api.disable_offboard_control(); 299 | 300 | // now pixhawk isn't listening to setpoint commands 301 | 302 | 303 | // -------------------------------------------------------------------------- 304 | // GET A MESSAGE 305 | // -------------------------------------------------------------------------- 306 | printf("READ SOME MESSAGES \n"); 307 | 308 | // copy current messages 309 | Mavlink_Messages messages = api.current_messages; 310 | 311 | // local position in ned frame 312 | mavlink_local_position_ned_t pos = messages.local_position_ned; 313 | printf("Got message LOCAL_POSITION_NED (spec: https://mavlink.io/en/messages/common.html#LOCAL_POSITION_NED)\n"); 314 | printf(" pos (NED): %f %f %f (m)\n", pos.x, pos.y, pos.z ); 315 | 316 | // hires imu 317 | mavlink_highres_imu_t imu = messages.highres_imu; 318 | printf("Got message HIGHRES_IMU (spec: https://mavlink.io/en/messages/common.html#HIGHRES_IMU)\n"); 319 | printf(" ap time: %lu \n", imu.time_usec); 320 | printf(" acc (NED): % f % f % f (m/s^2)\n", imu.xacc , imu.yacc , imu.zacc ); 321 | printf(" gyro (NED): % f % f % f (rad/s)\n", imu.xgyro, imu.ygyro, imu.zgyro); 322 | printf(" mag (NED): % f % f % f (Ga)\n" , imu.xmag , imu.ymag , imu.zmag ); 323 | printf(" baro: %f (mBar) \n" , imu.abs_pressure); 324 | printf(" altitude: %f (m) \n" , imu.pressure_alt); 325 | printf(" temperature: %f C \n" , imu.temperature ); 326 | 327 | printf("\n"); 328 | 329 | 330 | // -------------------------------------------------------------------------- 331 | // END OF COMMANDS 332 | // -------------------------------------------------------------------------- 333 | 334 | return; 335 | 336 | } 337 | 338 | 339 | // ------------------------------------------------------------------------------ 340 | // Parse Command Line 341 | // ------------------------------------------------------------------------------ 342 | // throws EXIT_FAILURE if could not open the port 343 | void 344 | parse_commandline(int argc, char **argv, char *&uart_name, int &baudrate, 345 | bool &use_udp, char *&udp_ip, int &udp_port, bool &autotakeoff) 346 | { 347 | 348 | // string for command line usage 349 | const char *commandline_usage = "usage: mavlink_control [-d -b ] [-u -p ] [-a ]"; 350 | 351 | // Read input arguments 352 | for (int i = 1; i < argc; i++) { // argv[0] is "mavlink" 353 | 354 | // Help 355 | if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { 356 | printf("%s\n",commandline_usage); 357 | throw EXIT_FAILURE; 358 | } 359 | 360 | // UART device ID 361 | if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { 362 | if (argc > i + 1) { 363 | i++; 364 | uart_name = argv[i]; 365 | } else { 366 | printf("%s\n",commandline_usage); 367 | throw EXIT_FAILURE; 368 | } 369 | } 370 | 371 | // Baud rate 372 | if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--baud") == 0) { 373 | if (argc > i + 1) { 374 | i++; 375 | baudrate = atoi(argv[i]); 376 | } else { 377 | printf("%s\n",commandline_usage); 378 | throw EXIT_FAILURE; 379 | } 380 | } 381 | 382 | // UDP ip 383 | if (strcmp(argv[i], "-u") == 0 || strcmp(argv[i], "--udp_ip") == 0) { 384 | if (argc > i + 1) { 385 | i++; 386 | udp_ip = argv[i]; 387 | use_udp = true; 388 | } else { 389 | printf("%s\n",commandline_usage); 390 | throw EXIT_FAILURE; 391 | } 392 | } 393 | 394 | // UDP port 395 | if (strcmp(argv[i], "-p") == 0 || strcmp(argv[i], "--port") == 0) { 396 | if (argc > i + 1) { 397 | i++; 398 | udp_port = atoi(argv[i]); 399 | } else { 400 | printf("%s\n",commandline_usage); 401 | throw EXIT_FAILURE; 402 | } 403 | } 404 | 405 | // Autotakeoff 406 | if (strcmp(argv[i], "-a") == 0 || strcmp(argv[i], "--autotakeoff") == 0) { 407 | autotakeoff = true; 408 | } 409 | 410 | } 411 | // end: for each input argument 412 | 413 | // Done! 414 | return; 415 | } 416 | 417 | 418 | // ------------------------------------------------------------------------------ 419 | // Quit Signal Handler 420 | // ------------------------------------------------------------------------------ 421 | // this function is called when you press Ctrl-C 422 | void 423 | quit_handler( int sig ) 424 | { 425 | printf("\n"); 426 | printf("TERMINATING AT USER REQUEST\n"); 427 | printf("\n"); 428 | 429 | // autopilot interface 430 | try { 431 | autopilot_interface_quit->handle_quit(sig); 432 | } 433 | catch (int error){} 434 | 435 | // port 436 | try { 437 | port_quit->stop(); 438 | } 439 | catch (int error){} 440 | 441 | // end program here 442 | exit(0); 443 | 444 | } 445 | 446 | 447 | // ------------------------------------------------------------------------------ 448 | // Main 449 | // ------------------------------------------------------------------------------ 450 | int 451 | main(int argc, char **argv) 452 | { 453 | // This program uses throw, wrap one big try/catch here 454 | try 455 | { 456 | int result = top(argc,argv); 457 | return result; 458 | } 459 | 460 | catch ( int error ) 461 | { 462 | fprintf(stderr,"mavlink_control threw exception %i \n" , error); 463 | return error; 464 | } 465 | 466 | } 467 | 468 | 469 | -------------------------------------------------------------------------------- /mavlink_control.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2014 MAVlink Development Team. All rights reserved. 4 | * Author: Trent Lukaczyk, 5 | * Jaycee Lock, 6 | * Lorenz Meier, 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * 2. Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in 16 | * the documentation and/or other materials provided with the 17 | * distribution. 18 | * 3. Neither the name PX4 nor the names of its contributors may be 19 | * used to endorse or promote products derived from this software 20 | * without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 29 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 30 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | ****************************************************************************/ 36 | 37 | /** 38 | * @file mavlink_control.h 39 | * 40 | * @brief An example offboard control process via mavlink, definition 41 | * 42 | * This process connects an external MAVLink UART device to send an receive data 43 | * 44 | * @author Trent Lukaczyk, 45 | * @author Jaycee Lock, 46 | * @author Lorenz Meier, 47 | * 48 | */ 49 | 50 | 51 | // ------------------------------------------------------------------------------ 52 | // Includes 53 | // ------------------------------------------------------------------------------ 54 | 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | #include 66 | 67 | using std::string; 68 | using namespace std; 69 | 70 | #include 71 | 72 | #include "autopilot_interface.h" 73 | #include "serial_port.h" 74 | #include "udp_port.h" 75 | 76 | // ------------------------------------------------------------------------------ 77 | // Prototypes 78 | // ------------------------------------------------------------------------------ 79 | 80 | int main(int argc, char **argv); 81 | int top(int argc, char **argv); 82 | 83 | void commands(Autopilot_Interface &autopilot_interface, bool autotakeoff); 84 | void parse_commandline(int argc, char **argv, char *&uart_name, int &baudrate, 85 | bool &use_udp, char *&udp_ip, int &udp_port, bool &autotakeoff); 86 | 87 | // quit handler 88 | Autopilot_Interface *autopilot_interface_quit; 89 | Generic_Port *port_quit; 90 | void quit_handler( int sig ); 91 | 92 | -------------------------------------------------------------------------------- /serial_port.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2014 MAVlink Development Team. All rights reserved. 4 | * Author: Trent Lukaczyk, 5 | * Jaycee Lock, 6 | * Lorenz Meier, 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * 2. Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in 16 | * the documentation and/or other materials provided with the 17 | * distribution. 18 | * 3. Neither the name PX4 nor the names of its contributors may be 19 | * used to endorse or promote products derived from this software 20 | * without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 29 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 30 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | ****************************************************************************/ 36 | 37 | /** 38 | * @file serial_port.cpp 39 | * 40 | * @brief Serial interface functions 41 | * 42 | * Functions for opening, closing, reading and writing via serial ports 43 | * 44 | * @author Trent Lukaczyk, 45 | * @author Jaycee Lock, 46 | * @author Lorenz Meier, 47 | * 48 | */ 49 | 50 | 51 | // ------------------------------------------------------------------------------ 52 | // Includes 53 | // ------------------------------------------------------------------------------ 54 | 55 | #include "serial_port.h" 56 | 57 | 58 | // ---------------------------------------------------------------------------------- 59 | // Serial Port Manager Class 60 | // ---------------------------------------------------------------------------------- 61 | 62 | // ------------------------------------------------------------------------------ 63 | // Con/De structors 64 | // ------------------------------------------------------------------------------ 65 | Serial_Port:: 66 | Serial_Port(const char *uart_name_ , int baudrate_) 67 | { 68 | initialize_defaults(); 69 | uart_name = uart_name_; 70 | baudrate = baudrate_; 71 | } 72 | 73 | Serial_Port:: 74 | Serial_Port() 75 | { 76 | initialize_defaults(); 77 | } 78 | 79 | Serial_Port:: 80 | ~Serial_Port() 81 | { 82 | // destroy mutex 83 | pthread_mutex_destroy(&lock); 84 | } 85 | 86 | void 87 | Serial_Port:: 88 | initialize_defaults() 89 | { 90 | // Initialize attributes 91 | debug = false; 92 | fd = -1; 93 | is_open = false; 94 | 95 | uart_name = (char*)"/dev/ttyUSB0"; 96 | baudrate = 57600; 97 | 98 | // Start mutex 99 | int result = pthread_mutex_init(&lock, NULL); 100 | if ( result != 0 ) 101 | { 102 | printf("\n mutex init failed\n"); 103 | throw 1; 104 | } 105 | } 106 | 107 | 108 | // ------------------------------------------------------------------------------ 109 | // Read from Serial 110 | // ------------------------------------------------------------------------------ 111 | int 112 | Serial_Port:: 113 | read_message(mavlink_message_t &message) 114 | { 115 | uint8_t cp; 116 | mavlink_status_t status; 117 | uint8_t msgReceived = false; 118 | 119 | // -------------------------------------------------------------------------- 120 | // READ FROM PORT 121 | // -------------------------------------------------------------------------- 122 | 123 | // this function locks the port during read 124 | int result = _read_port(cp); 125 | 126 | 127 | // -------------------------------------------------------------------------- 128 | // PARSE MESSAGE 129 | // -------------------------------------------------------------------------- 130 | if (result > 0) 131 | { 132 | // the parsing 133 | msgReceived = mavlink_parse_char(MAVLINK_COMM_1, cp, &message, &status); 134 | 135 | // check for dropped packets 136 | if ( (lastStatus.packet_rx_drop_count != status.packet_rx_drop_count) && debug ) 137 | { 138 | printf("ERROR: DROPPED %d PACKETS\n", status.packet_rx_drop_count); 139 | unsigned char v=cp; 140 | fprintf(stderr,"%02x ", v); 141 | } 142 | lastStatus = status; 143 | } 144 | 145 | // Couldn't read from port 146 | else 147 | { 148 | fprintf(stderr, "ERROR: Could not read from fd %d\n", fd); 149 | } 150 | 151 | // -------------------------------------------------------------------------- 152 | // DEBUGGING REPORTS 153 | // -------------------------------------------------------------------------- 154 | if(msgReceived && debug) 155 | { 156 | // Report info 157 | printf("Received message from serial with ID #%d (sys:%d|comp:%d):\n", message.msgid, message.sysid, message.compid); 158 | 159 | fprintf(stderr,"Received serial data: "); 160 | unsigned int i; 161 | uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; 162 | 163 | // check message is write length 164 | unsigned int messageLength = mavlink_msg_to_send_buffer(buffer, &message); 165 | 166 | // message length error 167 | if (messageLength > MAVLINK_MAX_PACKET_LEN) 168 | { 169 | fprintf(stderr, "\nFATAL ERROR: MESSAGE LENGTH IS LARGER THAN BUFFER SIZE\n"); 170 | } 171 | 172 | // print out the buffer 173 | else 174 | { 175 | for (i=0; i(write(fd, buf, len)); 498 | 499 | // Wait until all data has been written 500 | tcdrain(fd); 501 | 502 | // Unlock 503 | pthread_mutex_unlock(&lock); 504 | 505 | 506 | return bytesWritten; 507 | } 508 | 509 | 510 | -------------------------------------------------------------------------------- /serial_port.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2014 MAVlink Development Team. All rights reserved. 4 | * Author: Trent Lukaczyk, 5 | * Jaycee Lock, 6 | * Lorenz Meier, 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * 2. Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in 16 | * the documentation and/or other materials provided with the 17 | * distribution. 18 | * 3. Neither the name PX4 nor the names of its contributors may be 19 | * used to endorse or promote products derived from this software 20 | * without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 29 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 30 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | ****************************************************************************/ 36 | 37 | /** 38 | * @file serial_port.h 39 | * 40 | * @brief Serial interface definition 41 | * 42 | * Functions for opening, closing, reading and writing via serial ports 43 | * 44 | * @author Trent Lukaczyk, 45 | * @author Jaycee Lock, 46 | * @author Lorenz Meier, 47 | * 48 | */ 49 | 50 | #ifndef SERIAL_PORT_H_ 51 | #define SERIAL_PORT_H_ 52 | 53 | // ------------------------------------------------------------------------------ 54 | // Includes 55 | // ------------------------------------------------------------------------------ 56 | 57 | #include 58 | #include // Standard input/output definitions 59 | #include // UNIX standard function definitions 60 | #include // File control definitions 61 | #include // POSIX terminal control definitions 62 | #include // This uses POSIX Threads 63 | #include 64 | 65 | #include 66 | 67 | #include "generic_port.h" 68 | 69 | // ------------------------------------------------------------------------------ 70 | // Defines 71 | // ------------------------------------------------------------------------------ 72 | 73 | // The following two non-standard baudrates should have been defined by the system 74 | // If not, just fallback to number 75 | #ifndef B460800 76 | #define B460800 460800 77 | #endif 78 | 79 | #ifndef B921600 80 | #define B921600 921600 81 | #endif 82 | 83 | // ------------------------------------------------------------------------------ 84 | // Prototypes 85 | // ------------------------------------------------------------------------------ 86 | 87 | //class Serial_Port; 88 | 89 | 90 | 91 | // ---------------------------------------------------------------------------------- 92 | // Serial Port Manager Class 93 | // ---------------------------------------------------------------------------------- 94 | /* 95 | * Serial Port Class 96 | * 97 | * This object handles the opening and closing of the offboard computer's 98 | * serial port over which we'll communicate. It also has methods to write 99 | * a byte stream buffer. MAVlink is not used in this object yet, it's just 100 | * a serialization interface. To help with read and write pthreading, it 101 | * gaurds any port operation with a pthread mutex. 102 | */ 103 | class Serial_Port: public Generic_Port 104 | { 105 | 106 | public: 107 | 108 | Serial_Port(); 109 | Serial_Port(const char *uart_name_, int baudrate_); 110 | virtual ~Serial_Port(); 111 | 112 | int read_message(mavlink_message_t &message); 113 | int write_message(const mavlink_message_t &message); 114 | 115 | bool is_running(){ 116 | return is_open; 117 | } 118 | void start(); 119 | void stop(); 120 | 121 | private: 122 | 123 | int fd; 124 | mavlink_status_t lastStatus; 125 | pthread_mutex_t lock; 126 | 127 | void initialize_defaults(); 128 | 129 | bool debug; 130 | const char *uart_name; 131 | int baudrate; 132 | bool is_open; 133 | 134 | int _open_port(const char* port); 135 | bool _setup_port(int baud, int data_bits, int stop_bits, bool parity, bool hardware_control); 136 | int _read_port(uint8_t &cp); 137 | int _write_port(char *buf, unsigned len); 138 | 139 | }; 140 | 141 | 142 | 143 | #endif // SERIAL_PORT_H_ 144 | 145 | 146 | -------------------------------------------------------------------------------- /udp_port.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018 MAVlink Development Team. All rights reserved. 4 | * Author: Hannes Diethelm, 5 | * Trent Lukaczyk, 6 | * Jaycee Lock, 7 | * Lorenz Meier, 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * 2. Redistributions in binary form must reproduce the above copyright 16 | * notice, this list of conditions and the following disclaimer in 17 | * the documentation and/or other materials provided with the 18 | * distribution. 19 | * 3. Neither the name PX4 nor the names of its contributors may be 20 | * used to endorse or promote products derived from this software 21 | * without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 30 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 31 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | ****************************************************************************/ 37 | 38 | /** 39 | * @file udp_port.cpp 40 | * 41 | * @brief UDP interface functions 42 | * 43 | * Functions for opening, closing, reading and writing via UDP ports 44 | * 45 | * @author Hannes Diethelm, 46 | * @author Trent Lukaczyk, 47 | * @author Jaycee Lock, 48 | * @author Lorenz Meier, 49 | * 50 | */ 51 | 52 | 53 | // ------------------------------------------------------------------------------ 54 | // Includes 55 | // ------------------------------------------------------------------------------ 56 | 57 | #include "udp_port.h" 58 | 59 | 60 | // ---------------------------------------------------------------------------------- 61 | // UDP Port Manager Class 62 | // ---------------------------------------------------------------------------------- 63 | 64 | // ------------------------------------------------------------------------------ 65 | // Con/De structors 66 | // ------------------------------------------------------------------------------ 67 | UDP_Port:: 68 | UDP_Port(const char *target_ip_, int udp_port_) 69 | { 70 | initialize_defaults(); 71 | target_ip = target_ip_; 72 | rx_port = udp_port_; 73 | is_open = false; 74 | } 75 | 76 | UDP_Port:: 77 | UDP_Port() 78 | { 79 | initialize_defaults(); 80 | } 81 | 82 | UDP_Port:: 83 | ~UDP_Port() 84 | { 85 | // destroy mutex 86 | pthread_mutex_destroy(&lock); 87 | } 88 | 89 | void 90 | UDP_Port:: 91 | initialize_defaults() 92 | { 93 | // Initialize attributes 94 | target_ip = "127.0.0.1"; 95 | rx_port = 14550; 96 | tx_port = -1; 97 | is_open = false; 98 | debug = false; 99 | sock = -1; 100 | 101 | // Start mutex 102 | int result = pthread_mutex_init(&lock, NULL); 103 | if ( result != 0 ) 104 | { 105 | printf("\n mutex init failed\n"); 106 | throw 1; 107 | } 108 | } 109 | 110 | 111 | // ------------------------------------------------------------------------------ 112 | // Read from UDP 113 | // ------------------------------------------------------------------------------ 114 | int 115 | UDP_Port:: 116 | read_message(mavlink_message_t &message) 117 | { 118 | uint8_t cp; 119 | mavlink_status_t status; 120 | uint8_t msgReceived = false; 121 | 122 | // -------------------------------------------------------------------------- 123 | // READ FROM PORT 124 | // -------------------------------------------------------------------------- 125 | 126 | // this function locks the port during read 127 | int result = _read_port(cp); 128 | 129 | 130 | // -------------------------------------------------------------------------- 131 | // PARSE MESSAGE 132 | // -------------------------------------------------------------------------- 133 | if (result > 0) 134 | { 135 | // the parsing 136 | msgReceived = mavlink_parse_char(MAVLINK_COMM_1, cp, &message, &status); 137 | 138 | // check for dropped packets 139 | if ( (lastStatus.packet_rx_drop_count != status.packet_rx_drop_count) && debug ) 140 | { 141 | printf("ERROR: DROPPED %d PACKETS\n", status.packet_rx_drop_count); 142 | unsigned char v=cp; 143 | fprintf(stderr,"%02x ", v); 144 | } 145 | lastStatus = status; 146 | } 147 | 148 | // Couldn't read from port 149 | else 150 | { 151 | fprintf(stderr, "ERROR: Could not read, res = %d, errno = %d : %m\n", result, errno); 152 | } 153 | 154 | // -------------------------------------------------------------------------- 155 | // DEBUGGING REPORTS 156 | // -------------------------------------------------------------------------- 157 | if(msgReceived && debug) 158 | { 159 | // Report info 160 | printf("Received message from UDP with ID #%d (sys:%d|comp:%d):\n", message.msgid, message.sysid, message.compid); 161 | 162 | fprintf(stderr,"Received UDP data: "); 163 | unsigned int i; 164 | uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; 165 | 166 | // check message is write length 167 | unsigned int messageLength = mavlink_msg_to_send_buffer(buffer, &message); 168 | 169 | // message length error 170 | if (messageLength > MAVLINK_MAX_PACKET_LEN) 171 | { 172 | fprintf(stderr, "\nFATAL ERROR: MESSAGE LENGTH IS LARGER THAN BUFFER SIZE\n"); 173 | } 174 | 175 | // print out the buffer 176 | else 177 | { 178 | for (i=0; i 0){ 326 | buff_len=result; 327 | buff_ptr=0; 328 | cp=buff[buff_ptr]; 329 | buff_ptr++; 330 | //printf("recvfrom: %i %i\n", result, cp); 331 | } 332 | } 333 | 334 | // Unlock 335 | pthread_mutex_unlock(&lock); 336 | 337 | return result; 338 | } 339 | 340 | 341 | // ------------------------------------------------------------------------------ 342 | // Write Port with Lock 343 | // ------------------------------------------------------------------------------ 344 | int 345 | UDP_Port:: 346 | _write_port(char *buf, unsigned len) 347 | { 348 | 349 | // Lock 350 | pthread_mutex_lock(&lock); 351 | 352 | // Write packet via UDP link 353 | int bytesWritten = 0; 354 | if(tx_port > 0){ 355 | struct sockaddr_in addr; 356 | memset(&addr, 0, sizeof(addr)); 357 | addr.sin_family = AF_INET; 358 | addr.sin_addr.s_addr = inet_addr(target_ip); 359 | addr.sin_port = htons(tx_port); 360 | bytesWritten = sendto(sock, buf, len, 0, (struct sockaddr*)&addr, sizeof(struct sockaddr_in)); 361 | //printf("sendto: %i\n", bytesWritten); 362 | }else{ 363 | printf("ERROR: Sending before first packet received!\n"); 364 | bytesWritten = -1; 365 | } 366 | 367 | // Unlock 368 | pthread_mutex_unlock(&lock); 369 | 370 | 371 | return bytesWritten; 372 | } 373 | 374 | 375 | -------------------------------------------------------------------------------- /udp_port.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018 MAVlink Development Team. All rights reserved. 4 | * Author: Hannes Diethelm, 5 | * Trent Lukaczyk, 6 | * Jaycee Lock, 7 | * Lorenz Meier, 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * 2. Redistributions in binary form must reproduce the above copyright 16 | * notice, this list of conditions and the following disclaimer in 17 | * the documentation and/or other materials provided with the 18 | * distribution. 19 | * 3. Neither the name PX4 nor the names of its contributors may be 20 | * used to endorse or promote products derived from this software 21 | * without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 30 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 31 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | ****************************************************************************/ 37 | 38 | /** 39 | * @file udp_port.h 40 | * 41 | * @brief UDP interface definition 42 | * 43 | * Functions for opening, closing, reading and writing via UDP ports 44 | * 45 | * @author Hannes Diethelm, 46 | * @author Trent Lukaczyk, 47 | * @author Jaycee Lock, 48 | * @author Lorenz Meier, 49 | * 50 | */ 51 | 52 | #ifndef UDP_PORT_H_ 53 | #define UDP_PORT_H_ 54 | 55 | // ------------------------------------------------------------------------------ 56 | // Includes 57 | // ------------------------------------------------------------------------------ 58 | 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include // This uses POSIX Threads 64 | #include 65 | #include 66 | #include 67 | #include 68 | #include 69 | #include 70 | #include 71 | #include 72 | #include 73 | #include 74 | #include 75 | 76 | #include 77 | 78 | #include "generic_port.h" 79 | 80 | // ------------------------------------------------------------------------------ 81 | // Defines 82 | // ------------------------------------------------------------------------------ 83 | 84 | // ------------------------------------------------------------------------------ 85 | // Prototypes 86 | // ------------------------------------------------------------------------------ 87 | 88 | // ---------------------------------------------------------------------------------- 89 | // UDP Port Manager Class 90 | // ---------------------------------------------------------------------------------- 91 | /* 92 | * UDP Port Class 93 | * 94 | * This object handles the opening and closing of the offboard computer's 95 | * UDP port over which we'll communicate. It also has methods to write 96 | * a byte stream buffer. MAVlink is not used in this object yet, it's just 97 | * a serialization interface. To help with read and write pthreading, it 98 | * gaurds any port operation with a pthread mutex. 99 | */ 100 | class UDP_Port: public Generic_Port 101 | { 102 | 103 | public: 104 | 105 | UDP_Port(); 106 | UDP_Port(const char *target_ip_, int udp_port_); 107 | virtual ~UDP_Port(); 108 | 109 | int read_message(mavlink_message_t &message); 110 | int write_message(const mavlink_message_t &message); 111 | 112 | bool is_running(){ 113 | return is_open; 114 | } 115 | void start(); 116 | void stop(); 117 | 118 | private: 119 | 120 | mavlink_status_t lastStatus; 121 | pthread_mutex_t lock; 122 | 123 | void initialize_defaults(); 124 | 125 | const static int BUFF_LEN=2041; 126 | char buff[BUFF_LEN]; 127 | int buff_ptr; 128 | int buff_len; 129 | bool debug; 130 | const char *target_ip; 131 | int rx_port; 132 | int tx_port; 133 | int sock; 134 | bool is_open; 135 | 136 | int _read_port(uint8_t &cp); 137 | int _write_port(char *buf, unsigned len); 138 | 139 | }; 140 | 141 | 142 | 143 | #endif // UDP_PORT_H_ 144 | 145 | 146 | --------------------------------------------------------------------------------