├── .gitignore ├── Combined ├── drone.py ├── drone.pyc ├── example.py ├── mission.txt └── mission_read.txt ├── README.md ├── arm_disarm.py ├── change_flight_mode.py ├── filter_data_msg_type.py ├── mavgen.py ├── mavlink_message.py ├── mavtest.py ├── mavtest.pyc ├── mission.txt ├── mission_read.py ├── mission_upload.py ├── read_important.py ├── read_parameters.py ├── specific_msg.py └── takeoff.py /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__ 2 | drone.pyc 3 | -------------------------------------------------------------------------------- /Combined/drone.py: -------------------------------------------------------------------------------- 1 | # Import mavutil 2 | from pymavlink import mavutil 3 | from pymavlink import mavwp 4 | from pymavlink.dialects.v10 import ardupilotmega 5 | 6 | import time 7 | 8 | #Import threading for reading mavlink messages 9 | import threading 10 | 11 | class MavlinkMessage: 12 | def __init__(self,master): 13 | 14 | #master connection 15 | self.master = master 16 | 17 | ## The data coming from mavlink (not all the data are present) 18 | #'GLOBAL_POSITION_INT' 19 | self._lat = None 20 | self._lon = None 21 | self._alt = None 22 | self._relative_alt = None 23 | self._vx = None 24 | self._vy = None 25 | self._vz = None 26 | self._heading = None 27 | 28 | #'SYS_STATUS' 29 | self._voltage = None 30 | self._current = None 31 | self._level = None 32 | 33 | #'VFR_HUD' 34 | self._airspeed = None 35 | self._groundspeed = None 36 | self._throttle = None 37 | self._alt = None 38 | self._climb = None 39 | 40 | #'SERVO_OUTPUT_RAW' 41 | self._servo1_raw = None 42 | self._servo2_raw = None 43 | self._servo3_raw = None 44 | self._servo4_raw = None 45 | self._servo5_raw = None 46 | self._servo6_raw = None 47 | self._servo7_raw = None 48 | self._servo8_raw = None 49 | 50 | #'GPS_RAW_INIT' 51 | self._eph = None 52 | self._epv = None 53 | self._satellites_visible = None 54 | self._fix_type = None 55 | 56 | #'EKF_STATUS_REPORT' 57 | self._ekf_poshorizabs = False 58 | self._ekf_constposmode = False 59 | self._ekf_predposhorizabs = False 60 | self._ekf_flag = None 61 | 62 | #'LOCAL_POSITION_NED' 63 | self._north = None 64 | self._east = None 65 | self._down = None 66 | 67 | #'HEARTBEAT' 68 | self._flightmode = None 69 | self._armed = False 70 | self._system_status = None 71 | self._autopilot_type = None # PX4, ArduPilot, etc. 72 | self._vehicle_type = None # quadcopter, plane, etc. 73 | 74 | #'ATTITUDE' 75 | self._roll = None 76 | self._pitch = None 77 | self._yaw = None 78 | self._rollspeed = None 79 | self._pitchspeed = None 80 | self._yawspeed = None 81 | 82 | #'MISSION_COUNT' 83 | self._msg_mission_count = None 84 | #'MISSION_ITEM' 85 | self._msg_mission_item = None 86 | #'COMMAND_ACK' 87 | self._msg_command_ack = None 88 | #'MISSION_REQUEST' 89 | self._wp = mavwp.MAVWPLoader() 90 | self._wp_uploaded = None 91 | self._msg_mission_request = None 92 | 93 | self.messages = { 94 | 'GLOBAL_POSITION_INT' :self.__read_global_pos_int, 95 | 'SYS_STATUS' :self.__read_system_status, 96 | 'VFR_HUD' :self.__read_vfr_hud, 97 | 'SERVO_OUTPUT_RAW' :self.__read_servo_output_raw, 98 | 'GPS_RAW_INT' :self.__read_gps_raw_int, 99 | 'EKF_STATUS_REPORT' :self.__read_ekf_status_report, 100 | 'LOCAL_POSITION_NED' :self.__read_local_position_ned, 101 | 'HEARTBEAT' :self.__read_heartbeat, 102 | 'ATTITUDE' :self.__read_attitude, 103 | #The variables for mavlink message listed below should be cleared once it is read. 104 | 'MISSION_COUNT' :self.__read_mission_count, 105 | 'MISSION_ITEM' :self.__read_mission_item, 106 | 'MISSION_REQUEST' :self.__read_mission_request, 107 | 'COMMAND_ACK' :self.__read_command_ack 108 | } 109 | 110 | #start new thread for getting data whenever object is called 111 | self.data_read = threading.Thread(target = self.__update) 112 | self.data_read.daemon = True # In daemon mode so that ctrl + c will close the program 113 | self.data_read.start() 114 | 115 | def __update(self): 116 | while True: 117 | #print("Here") 118 | msg = self.master.recv_match() 119 | 120 | if not msg: 121 | continue 122 | 123 | function = self.messages.get(msg.get_type(),lambda x:"Invalid") 124 | function(msg) 125 | 126 | 127 | def __read_global_pos_int(self,msg): 128 | self._lat = msg.lat * 1e-7 129 | self._lon = msg.lon * 1e-7 130 | self._alt = msg.alt * 1e-3 131 | self._relative_alt = msg.relative_alt * 1e-3 132 | self._vx = msg.vx 133 | self._vy = msg.vy 134 | self._vz = msg.vz 135 | self._heading = int(msg.hdg * 1e-2) 136 | 137 | def __read_system_status(self,msg): 138 | self._voltage = msg.voltage_battery 139 | self._current = msg.current_battery 140 | self._level = msg.battery_remaining 141 | 142 | def __read_vfr_hud(self,msg): 143 | self._airspeed = msg.airspeed 144 | self._groundspeed = msg.groundspeed 145 | self._throttle = msg.throttle 146 | self._alt = msg.alt 147 | self._climb = msg.climb 148 | 149 | def __read_servo_output_raw(self,msg): 150 | self._servo1_raw = msg.servo1_raw 151 | self._servo2_raw = msg.servo2_raw 152 | self._servo3_raw = msg.servo3_raw 153 | self._servo4_raw = msg.servo4_raw 154 | self._servo5_raw = msg.servo5_raw 155 | self._servo6_raw = msg.servo6_raw 156 | self._servo7_raw = msg.servo7_raw 157 | self._servo8_raw = msg.servo8_raw 158 | 159 | def __read_gps_raw_int(self,msg): 160 | self._eph = msg.eph 161 | self._epv = msg.epv 162 | self._satellites_visible = msg.satellites_visible 163 | self._fix_type = msg.fix_type 164 | 165 | def __read_ekf_status_report(self,msg): 166 | ekf_flags = msg.flags 167 | # boolean: EKF's horizontal position (absolute) estimate is good 168 | self._ekf_poshorizabs = (ekf_flags & ardupilotmega.EKF_POS_HORIZ_ABS) > 0 169 | # boolean: EKF is in constant position mode and does not know it's absolute or relative position 170 | self._ekf_constposmode = (ekf_flags & ardupilotmega.EKF_CONST_POS_MODE) > 0 171 | # boolean: EKF's predicted horizontal position (absolute) estimate is good 172 | self._ekf_predposhorizabs = (ekf_flags & ardupilotmega.EKF_PRED_POS_HORIZ_ABS) > 0 173 | 174 | 175 | 176 | def __read_local_position_ned(self,msg): 177 | self._north = msg.y 178 | self._east = msg.x 179 | self._down = msg.z 180 | 181 | def __read_heartbeat(self,msg): 182 | if self.master.probably_vehicle_heartbeat(msg): 183 | self._flightmode = mavutil.mode_mapping_acm[msg.custom_mode] 184 | self._armed = (msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0 185 | self._system_status = msg.system_status 186 | self._autopilot_type = msg.autopilot 187 | self._vehicle_type = msg.type # quadcopter, plane, etc. 188 | 189 | def __read_attitude(self,msg): 190 | self._roll = msg.roll 191 | self._pitch = msg.pitch 192 | self._yaw = msg.yaw 193 | self._rollspeed = msg.rollspeed 194 | self._pitchspeed = msg.pitchspeed 195 | self._yawspeed = msg.yawspeed 196 | 197 | def __read_mission_count(self,msg): 198 | self._msg_mission_count = msg 199 | 200 | def __read_mission_item(self,msg): 201 | self._msg_mission_item = msg 202 | 203 | def __read_mission_request(self,msg): 204 | 205 | if self._wp_uploaded is not None: 206 | wp = self._wp.wp(msg.seq) 207 | self.master.mav.send(wp) 208 | self._wp_uploaded[msg.seq] = True 209 | 210 | def __read_command_ack(self,msg): 211 | self._msg_command_ack = msg 212 | 213 | 214 | 215 | class Drone(MavlinkMessage): 216 | def __init__(self,port): 217 | #start connection on the given port 218 | self.master = mavutil.mavlink_connection(port) 219 | 220 | # store waypoints from the command 221 | self._waypoints = {} 222 | self._home = None 223 | 224 | #wait_heartbeat can be called before MavlinkMessage class initialization 225 | #after MavlinkMessage class initialization, due to thread for reading mavlink message, 226 | #it is not advisable to even look for mavlink message inside methods of this class. 227 | #instead, check for the mavlink message inside MavlinkMessage class by adding the respective message. 228 | self.master.wait_heartbeat() 229 | 230 | #set home location when initializing 231 | msg = self.master.recv_match(type = 'GLOBAL_POSITION_INT',blocking = True) 232 | self._home = Location(msg.lat*1e-7,msg.lon*1e-7,msg.alt*1e-3,msg.relative_alt*1e-3) 233 | print("Home location set to lat = ", self._home.lat," lon = ",self._home.lon, "alt = ",self._home.alt) 234 | 235 | #read current mission 236 | self.mission_read() 237 | 238 | MavlinkMessage.__init__(self,self.master) 239 | 240 | 241 | def set_flight_mode(self,mode): 242 | """Set drone flight mode 243 | 244 | Args: 245 | mode (string): flight mode name such as 'GUIDED','LOITER','RTL' 246 | """ 247 | mavutil.mavfile.set_mode(self.master,mode,0,0) 248 | 249 | 250 | def arm(self): 251 | """Drone arm 252 | """ 253 | self.master.mav.command_long_send(self.master.target_system, 254 | self.master.target_component, 255 | mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 256 | 0, 257 | 1, 0, 0, 0, 0, 0, 0) 258 | 259 | def disarm(self): 260 | """Drone disarm 261 | """ 262 | self.master.mav.command_long_send(self.master.target_system, 263 | self.master.target_component, 264 | mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 265 | 0, 266 | 0, 0, 0, 0, 0, 0, 0) 267 | 268 | def arm_and_takeoff(self,altitude,auto_mode = True): 269 | """Drone arm and takeoff 270 | 271 | Args: 272 | altitude (integer): altitude in meters(m) 273 | auto_mode (bool, optional): continue auto mission after takeoff. Defaults to True. 274 | """ 275 | armable = False 276 | while not armable: 277 | armable = self.is_armable 278 | self.set_flight_mode('GUIDED') 279 | self.arm() 280 | self.master.mav.command_long_send(0, 0, 281 | mavutil.mavlink.MAV_CMD_NAV_TAKEOFF 282 | ,0, 0, 0, 0, 0, 0, 0, altitude) 283 | 284 | if(auto_mode): 285 | self.set_flight_mode('AUTO') 286 | 287 | def simple_goto(self,location): 288 | """Drone goto a waypoint 289 | 290 | Args: 291 | location (Location): Location class with lat,lon and alt 292 | """ 293 | self.master.mav.mission_item_send(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, 294 | mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2, 0, 0, 295 | 0, 0, 0, location.lat, location.lon, 296 | location.alt) 297 | 298 | def set_home(self,home_location=None): 299 | """Drone set home 300 | Args: 301 | home_location (Location, optional): If location given, sets that location as home. If not given sets boot position as home. Defaults to None. 302 | """ 303 | if home_location == None: 304 | home_location = self._home 305 | 306 | self.master.mav.command_long_send( 307 | self.master.target_system, self.master.target_component, 308 | mavutil.mavlink.MAV_CMD_DO_SET_HOME, 309 | 1, # set position 310 | 0, # param1 311 | 0, # param2 312 | 0, # param3 313 | 0, # param4 314 | home_location.lat, # lat 315 | home_location.lon, # lon 316 | home_location.alt) # alt 317 | 318 | def mission_read(self, file_name = 'mission_read.txt'): 319 | """Drone current mission read 320 | 321 | Args: 322 | file_name (str, optional): File name to store mission into. Defaults to 'mission_read.txt'. 323 | 324 | """ 325 | #ask for mission count 326 | self.master.waypoint_request_list_send() 327 | 328 | #wait for receive mavlink msg type MISSION_COUNT 329 | msg = self.master.recv_match(type = ['MISSION_COUNT'],blocking = True) 330 | 331 | waypoint_count = msg.count 332 | print("msg.count:",waypoint_count) 333 | 334 | output = 'QGC WPL 110\n' 335 | mission_count = 0 336 | for i in range(waypoint_count): 337 | #ask for individual waypoint 338 | self.master.waypoint_request_send(i) 339 | #wait for receive mavlink msg type MISSION_ITEM 340 | 341 | msg = self.master.recv_match(type = ['MISSION_ITEM'],blocking = True) 342 | 343 | #commandline is used to store msg in a given format 344 | commandline="%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\n" % (msg.seq,msg.current,msg.frame,msg.command,msg.param1,msg.param2,msg.param3,msg.param4,msg.x,msg.y,msg.z,msg.autocontinue) 345 | output += commandline 346 | 347 | #store the waypoints in waypoint dictionary 348 | if (msg.command != 22): 349 | if(msg.seq != 0): #i.e not home location 350 | self._waypoints[mission_count] = { 351 | 'lat':msg.x, 352 | 'lng':msg.y, 353 | 'alt':msg.z, 354 | 'command':msg.command 355 | } 356 | else: #i.e home location 357 | self._home.lat = msg.x 358 | self._home.lon = msg.y 359 | self._waypoints[mission_count] = { 360 | 'lat':self._home.lat, 361 | 'lng':self._home.lon 362 | } 363 | mission_count += 1 364 | 365 | 366 | #write to file 367 | with open(file_name,'w') as file_: 368 | print("Write mission to file") 369 | file_.write(output) 370 | 371 | 372 | def mission_upload(self, file_name = 'mission.txt'): 373 | """Drone mission upload from available mission text file 374 | 375 | Args: 376 | file_name (str, optional): File name to upload mission from. Defaults to 'mission.txt'. 377 | 378 | Raises: 379 | Exception: Mission file type not supported 380 | """ 381 | #clear waypoints before uploading, so that new waypoints can be added 382 | self._waypoints.clear() 383 | mission_count = 0 384 | with open(file_name) as f: 385 | for i, line in enumerate(f): 386 | if i == 0: 387 | if not line.startswith('QGC WPL 110'): 388 | raise Exception('File is not supported WP version') 389 | else: 390 | linearray=line.split('\t') 391 | ln_seq = int(linearray[0]) 392 | ln_current = int(linearray[1]) 393 | ln_frame = int(linearray[2]) 394 | ln_command = int(linearray[3]) 395 | ln_param1=float(linearray[4]) 396 | ln_param2=float(linearray[5]) 397 | ln_param3=float(linearray[6]) 398 | ln_param4=float(linearray[7]) 399 | ln_x=float(linearray[8]) 400 | ln_y=float(linearray[9]) 401 | ln_z=float(linearray[10]) 402 | ln_autocontinue = int(float(linearray[11].strip())) 403 | 404 | #store in waypoints 405 | if(ln_command != 22): 406 | if(ln_seq != 0): #i.e not home location 407 | self._waypoints[mission_count] = { 408 | 'lat':ln_x, 409 | 'lng':ln_y, 410 | 'alt':ln_z, 411 | 'command':ln_command 412 | } 413 | else: 414 | self._waypoints[mission_count] = { 415 | 'lat':ln_x, 416 | 'lng':ln_y 417 | } 418 | mission_count += 1 419 | p = mavutil.mavlink.MAVLink_mission_item_message(0, 0, ln_seq, ln_frame, 420 | ln_command, 421 | ln_current, ln_autocontinue, ln_param1, ln_param2, ln_param3, ln_param4, ln_x, ln_y, ln_z) 422 | self._wp.add(p) 423 | 424 | #while uploading mission, first home should be given 425 | self.set_home() 426 | #msg = self.master.recv_match(type = ['COMMAND_ACK'],blocking = True) 427 | #print(msg) 428 | print('Set home location: {0} {1}'.format(self._home.lat,self._home.lon)) 429 | time.sleep(1) 430 | 431 | #send waypoint to airframe 432 | self.master.waypoint_clear_all_send() 433 | if self._wp.count() > 0: 434 | self._wp_uploaded = [False] * self._wp.count() 435 | self.master.waypoint_count_send(self._wp.count()) 436 | while False in self._wp_uploaded: 437 | time.sleep(0.1) 438 | self._wp_uploaded = None 439 | 440 | #From this on, waypoint sending is handled inside MavlinkMessage Class whenever mission request is called 441 | 442 | 443 | @property 444 | def flight_plan(self): 445 | """Drone waypoints in which it will fly 446 | 447 | Returns: 448 | [dict]: a dictionary with all the waypoint that drone will fly 449 | """ 450 | return self._waypoints 451 | 452 | @property 453 | def is_armable(self): 454 | """Drone condition that whether it is safe to arm or not 455 | 456 | Returns: 457 | bool: safe to arm if True , not safe to arm if False 458 | """ 459 | # check that we have a GPS fix 460 | # check that EKF pre-arm is complete 461 | return (self._fix_type > 1) and self._ekf_predposhorizabs 462 | 463 | @property 464 | def ekf_ok(self): 465 | """Drone EKF Status 466 | 467 | Returns: 468 | bool: EKF ok if True, EKF not ok if False 469 | """ 470 | # use same check that ArduCopter::system.pde::position_ok() is using 471 | if self._armed: 472 | return self._ekf_poshorizabs and not self._ekf_constposmode 473 | else: 474 | return self._ekf_poshorizabs or self._ekf_predposhorizabs 475 | 476 | @property 477 | def system_status(self): 478 | """Drone system status 479 | 480 | Returns: 481 | string: The current status of drone. 'BOOT' means drone is booting, 'STANDBY' means drone is in standby mode. 482 | """ 483 | return { 484 | 0: 'UNINIT', 485 | 1: 'BOOT', 486 | 2: 'CALIBRATING', 487 | 3: 'STANDBY', 488 | 4: 'ACTIVE', 489 | 5: 'CRITICAL', 490 | 6: 'EMERGENCY', 491 | 7: 'POWEROFF', 492 | 8 : 'FLIGHT_TERMINATION' 493 | }.get(self._system_status, None) 494 | 495 | @property 496 | def is_armed(self): 497 | """Arming status of the drone 498 | 499 | Returns: 500 | bool: True if armed, False if disarmed 501 | """ 502 | return self._armed 503 | 504 | @property 505 | def flight_mode(self): 506 | """Flight mode status of the drone 507 | 508 | Returns: 509 | string: 'GUIDED' if in guided mode, 'RTL' if in rtl mode and so on 510 | """ 511 | return self._flightmode 512 | 513 | @property 514 | def heading(self): 515 | """Heading of the drone 516 | 517 | Returns: 518 | integer: True heading of the drone 519 | """ 520 | return self._heading 521 | 522 | @property 523 | def groundspeed(self): 524 | """Ground speed of the drone 525 | 526 | Returns: 527 | float: Ground speed of the drone (m/s) 528 | """ 529 | return self._groundspeed 530 | 531 | @property 532 | def airspeed(self): 533 | """Airspeed of the drone 534 | 535 | Returns: 536 | float: Air speed of the drone (m/s) 537 | """ 538 | return self._airspeed 539 | 540 | @property 541 | def velocity(self): 542 | """Velocity of the drone in x,y,z frame 543 | 544 | Returns: 545 | Velocity: Velocity.vx = velocity in N direction, Velocity.vy = velocity in E direction, Velocity.vz = velocity in U direction 546 | """ 547 | return Velocity(self._vx, self._vy, self._vz) 548 | 549 | @property 550 | def battery(self): 551 | """Battery status of the drone 552 | 553 | Returns: 554 | Battery: Battery.voltage = voltage, Battery.current = current , Battery.level = charge percentage 555 | """ 556 | return Battery(self._voltage,self._current,self._level) 557 | 558 | @property 559 | def attitude(self): 560 | """Attitude status of the drone 561 | 562 | Returns: 563 | Attitude: Attitude.roll = roll, Attitude.pitch = pitch, Attitude.yaw = yaw of the drone 564 | """ 565 | return Attitude(self._roll,self._pitch,self._yaw,self._rollspeed,self._pitchspeed,self._yawspeed) 566 | 567 | @property 568 | def location(self): 569 | """Current Location of the drone 570 | 571 | Returns: 572 | Location: Location.lat = latitude 573 | Location.lon = longitude 574 | Location.alt = altitude 575 | Location.altR = relative altitude 576 | Location.north = north in NEU frame 577 | Location.east = east in NEU frame 578 | Location.down = down in NEU frame 579 | """ 580 | return Location(self._lat,self._lon,self._alt,self._relative_alt,self._north,self._east,self._down) 581 | 582 | @property 583 | def gps_0(self): 584 | """GPS status of the drone 585 | 586 | Returns: 587 | GPSInfo: GPSInfo.eph = eph of drone, GPSInfo.epv = epv of drone, GPSInfo.fix_type = fix type, GPSInfo.satellites_visible = number of satellites visible 588 | """ 589 | return(GPSInfo(self._eph,self._epv,self._fix_type,self._satellites_visible)) 590 | 591 | 592 | ## Classes for drone conditions defined below 593 | class Battery(): 594 | 595 | def __init__(self, voltage, current, level): 596 | self.voltage = voltage / 1000.0 597 | if current == -1: 598 | self.current = None 599 | else: 600 | self.current = current #/ 100.0 601 | if level == -1: 602 | self.level = None 603 | else: 604 | self.level = level 605 | def __str__(self): 606 | return "Battery:voltage={},current={},level={}".format(self.voltage, self.current, 607 | self.level) 608 | 609 | class Location(): 610 | 611 | def __init__(self,lat=None,lon=None,alt=None,altR=None,north=None,east=None,down=None): 612 | self.lat = lat 613 | self.lon = lon 614 | self.alt = alt 615 | self.altR = altR 616 | self.north = north 617 | self.east = east 618 | self.down = down 619 | 620 | def __str__(self): 621 | return "LocationGlobal:lat=%s,lon=%s,altR=%s,alt=%s || LocationLocal:north=%s,east=%s,down=%s" % (self.lat, self.lon, self.altR,self.alt,self.north, self.east, self.down) 622 | 623 | class GPSInfo(): 624 | def __init__(self, eph, epv, fix_type, satellites_visible): 625 | self.eph = eph 626 | self.epv = epv 627 | self.fix_type = fix_type 628 | self.satellites_visible = satellites_visible 629 | 630 | def __str__(self): 631 | return "GPSInfo:fix=%s,num_sat=%s" % (self.fix_type, self.satellites_visible) 632 | 633 | class Attitude(): 634 | def __init__(self, roll, pitch, yaw,rollspeed,pitchspeed,yawspeed): 635 | self.pitch = pitch 636 | self.yaw = yaw 637 | self.roll = roll 638 | self.rollspeed = rollspeed 639 | self.pitchspeed = pitchspeed 640 | self.yawspeed = yawspeed 641 | 642 | def __str__(self): 643 | return "Attitude:roll=%s,pitch=%s,yaw=%s" % (self.roll, self.pitch,self.yaw) 644 | 645 | class Velocity(): 646 | def __init__(self,vx,vy,vz): 647 | self.vx = vx 648 | self.vy = vy 649 | self.vz = vz 650 | 651 | def __str__(self): 652 | return "Velocity:vx=%s,vy=%s,vz=%s" % (self.vx, self.vy,self.vz) -------------------------------------------------------------------------------- /Combined/drone.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aabs7/pymavlink_tutorial/fc78fca35fcd2c638001fe347c48bbf12042a850/Combined/drone.pyc -------------------------------------------------------------------------------- /Combined/example.py: -------------------------------------------------------------------------------- 1 | from drone import Drone,Location 2 | import time 3 | 4 | drone = Drone('tcp:127.0.0.1:5762') 5 | 6 | time.sleep(1) 7 | 8 | drone.mission_upload() 9 | 10 | flight_plan = drone.flight_plan 11 | print(flight_plan) 12 | input() 13 | #drone.arm() 14 | #time.sleep(1) 15 | #targetAltitude = 10 16 | #drone.arm_and_takeoff(targetAltitude,auto_mode=True) 17 | 18 | 19 | # while True: 20 | # #print("Altitude:",drone.location.altR) 21 | # if drone.location.altR > 0.95 * targetAltitude: 22 | # print("break") 23 | # break 24 | 25 | # point1 = Location(-35.3625939,149.1650759,10) 26 | # drone.simple_goto(point1) 27 | 28 | #time.sleep(10) 29 | #print("RTL set") 30 | #drone.set_flight_mode('RTL') 31 | 32 | #input() 33 | # print("connecting !!!") 34 | # time.sleep(1) 35 | # print("Connected!!!") 36 | # while True: 37 | # print("Battery:",drone.battery.voltage) 38 | # print("gs:",drone.groundspeed) 39 | # #print(drone.location) 40 | # print("Arm:",drone.is_armed) 41 | # print("is_armable:",drone.is_armable) 42 | # print("Ekf ok?",drone.ekf_ok) 43 | # print("status:",drone.system_status) 44 | # print("Mode:",drone.flight_mode) 45 | # print("\r\n") 46 | # time.sleep(1) 47 | 48 | -------------------------------------------------------------------------------- /Combined/mission.txt: -------------------------------------------------------------------------------- 1 | QGC WPL 110 2 | 0 0 0 16 0.0 0.0 0.0 0.0 -35.3632621765 149.165237427 584.08001709 1 3 | 1 0 3 22 15.0 0.0 0.0 0.0 0.0 0.0 10.0 1 4 | 2 0 3 16 0.0 0.0 0.0 0.0 -35.3628540039 149.163864136 10.0 1 5 | 3 0 3 16 0.0 0.0 0.0 0.0 -35.3620910645 149.163192749 10.0 1 6 | 4 0 3 18 1.0 0.0 80.0 1.0 -35.3616333008 149.163070679 10.0 1 7 | 5 0 3 16 0.0 0.0 0.0 0.0 -35.3612213135 149.163101196 10.0 1 8 | 6 0 3 16 0.0 0.0 0.0 0.0 -35.360584259 149.163391113 10.0 1 9 | 7 0 3 16 0.0 0.0 0.0 0.0 -35.3603134155 149.164123535 10.0 1 10 | 8 0 3 16 0.0 0.0 0.0 0.0 -35.3602294922 149.165115356 10.0 1 11 | 9 0 0 20 0.0 0.0 0.0 0.0 0.0 0.0 0.0 1 12 | 10 0 3 16 0.0 0.0 0.0 0.0 -35.3607711792 149.166412354 10.0 1 13 | 11 0 3 16 0.0 0.0 0.0 0.0 -35.3611717224 149.167007446 10.0 1 14 | 12 0 3 16 0.0 0.0 0.0 0.0 -35.3617210388 149.166732788 10.0 1 15 | 13 0 3 16 0.0 0.0 0.0 0.0 -35.3624534607 149.166183472 10.0 1 16 | -------------------------------------------------------------------------------- /Combined/mission_read.txt: -------------------------------------------------------------------------------- 1 | QGC WPL 110 2 | 0 0 0 16 0.0 0.0 0.0 0.0 -35.36326217651367 149.1652374267578 584.02001953125 1 3 | 1 0 3 22 15.0 0.0 0.0 0.0 0.0 0.0 10.0 1 4 | 2 0 3 16 0.0 0.0 0.0 0.0 -35.36285400390625 149.1638641357422 10.0 1 5 | 3 0 3 16 0.0 0.0 0.0 0.0 -35.362091064453125 149.16319274902344 10.0 1 6 | 4 0 3 18 1.0 0.0 80.0 1.0 -35.36163330078125 149.16307067871094 10.0 1 7 | 5 0 3 16 0.0 0.0 0.0 0.0 -35.36122131347656 149.16310119628906 10.0 1 8 | 6 0 3 16 0.0 0.0 0.0 0.0 -35.3605842590332 149.16339111328125 10.0 1 9 | 7 0 3 16 0.0 0.0 0.0 0.0 -35.360313415527344 149.16412353515625 10.0 1 10 | 8 0 3 16 0.0 0.0 0.0 0.0 -35.3602294921875 149.1651153564453 10.0 1 11 | 9 0 0 20 0.0 0.0 0.0 0.0 0.0 0.0 0.0 1 12 | 10 0 3 16 0.0 0.0 0.0 0.0 -35.36077117919922 149.16641235351562 10.0 1 13 | 11 0 3 16 0.0 0.0 0.0 0.0 -35.36117172241211 149.16700744628906 10.0 1 14 | 12 0 3 16 0.0 0.0 0.0 0.0 -35.36172103881836 149.16673278808594 10.0 1 15 | 13 0 3 16 0.0 0.0 0.0 0.0 -35.36245346069336 149.1661834716797 10.0 1 16 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # pymavlink_tutorial 2 | This repo contains code for connecting drone with pymavlink. All the drones that uses mavlink message can be controlled by codes in this repository. 3 | ```drone.py``` under ```Combined``` folder is the implementation of ```prokura_drone``` api which is high level API to control drones. 4 | 5 | ## Note: 6 | THIS API IS TESTED IN ARDUPILOT AND WORKS WELL WITH ARDUPILOT FLIGHT STACK. HOWEVER, BECAUSE PX4 DOESN'T UTILIZE ALL THE MAVLINK MESSAGE USED IN ARDUPILOT, OR USES SOME MAVLINK MESSAGES DIFFERENT THAN ARDUPILOT, ERROR MAY OCCUR WHILE HANDLING MAVLINK MESSAGE IN PX4. 7 | -------------------------------------------------------------------------------- /arm_disarm.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example of how to Arm and Disarm an Autopilot with pymavlink 3 | """ 4 | # Import mavutil 5 | from pymavlink import mavutil 6 | import time 7 | 8 | # Create the connection 9 | master = mavutil.mavlink_connection('tcp:127.0.0.1:5762') 10 | # Wait a heartbeat before sending commands 11 | master.wait_heartbeat() 12 | 13 | # https://mavlink.io/en/messages/common.html#MAV_CMD_COMPONENT_ARM_DISARM 14 | 15 | # Arm 16 | # master.arducopter_arm() or: 17 | master.mav.command_long_send( 18 | master.target_system, 19 | master.target_component, 20 | mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 21 | 0, 22 | 1, 0, 0, 0, 0, 0, 0) 23 | 24 | time.sleep(2) 25 | # Disarm 26 | # master.arducopter_disarm() or: 27 | master.mav.command_long_send( 28 | master.target_system, 29 | master.target_component, 30 | mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 31 | 0, 32 | 0, 0, 0, 0, 0, 0, 0) -------------------------------------------------------------------------------- /change_flight_mode.py: -------------------------------------------------------------------------------- 1 | # Import mavutil 2 | from pymavlink import mavutil 3 | import time 4 | 5 | # Create the connection 6 | master = mavutil.mavlink_connection('tcp:127.0.0.1:5762') 7 | # Wait a heartbeat before sending commands 8 | master.wait_heartbeat() 9 | 10 | #mavutil.mavfile.set_mode(master,'GUIDED',0,0) 11 | 12 | mode = 'LOITER' 13 | mode_id = master.mode_mapping()[mode] 14 | 15 | master.mav.set_mode_send( 16 | master.target_system, 17 | mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, 18 | mode_id) 19 | 20 | while True: 21 | # Wait for ACK command 22 | ack_msg = master.recv_match(type='COMMAND_ACK', blocking=True) 23 | ack_msg = ack_msg.to_dict() 24 | 25 | # Check if command in the same in `set_mode` 26 | if ack_msg['command'] != mavutil.mavlink.MAVLINK_MSG_ID_SET_MODE: 27 | continue 28 | 29 | # Print the ACK result ! 30 | #print(mavutil.mavlink.enums['MAV_RESULT'][ack_msg['result']].description) 31 | break -------------------------------------------------------------------------------- /filter_data_msg_type.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example of how to filter for specific mavlink messages coming from the 3 | autopilot using pymavlink 4 | """ 5 | # Import mavutil 6 | from pymavlink import mavutil 7 | 8 | # Create the connection 9 | # From topside computer 10 | master = mavutil.mavlink_connection('tcp:127.0.0.1:5762') 11 | 12 | while True: 13 | msg = master.recv_match() 14 | if not msg: 15 | continue 16 | if msg.get_type() == 'HEARTBEAT': 17 | print("\n\n*****Got message: %s*****" % msg.get_type()) 18 | print("Message: %s" % msg) 19 | print("\nAs dictionary: %s" % msg.to_dict()) 20 | # Armed = MAV_STATE_STANDBY (4), Disarmed = MAV_STATE_ACTIVE (3) 21 | print("\nSystem status: %s" % msg.system_status) -------------------------------------------------------------------------------- /mavgen.py: -------------------------------------------------------------------------------- 1 | #import ardupilotmega module for mavlink1 2 | from pymavlink.dialects.v10 import ardupilotmega as mavlink1 3 | 4 | #import common module for mavlink 2 5 | from pymavlink.dialects.v20 import common as mavlink2 6 | 7 | from pymavlink import mavutil 8 | import time 9 | import sys 10 | 11 | # Start a connection listening to a UDP port 12 | the_connection = mavutil.mavlink_connection('tcp:127.0.0.1:5762') 13 | 14 | # Wait for the first heartbeat 15 | # This sets the system and component ID of remote system for the link 16 | the_connection.wait_heartbeat() 17 | print("Heartbeat from system (system %u component %u)" % (the_connection.target_system, the_connection.target_system)) 18 | 19 | msg = the_connection.recv_match(blocking=True) 20 | 21 | while True: 22 | msg = the_connection.recv_match(blocking=True) 23 | 24 | print(the_connection.messages['GPS_RAW_INT'].alt ) 25 | time.sleep(1) 26 | 27 | 28 | 29 | the_connection.mav.heartbeat_send( 30 | 6, # type 31 | 8, # autopilot 32 | 192, # base_mode 33 | 0, # custom_mode 34 | 4, # system_status 35 | 3 # mavlink_version 36 | ) 37 | 38 | the_connection.mav.command_long_send( 39 | 1, # autopilot system id 40 | 1, # autopilot component id 41 | 400, # command id, ARM/DISARM 42 | 0, # confirmation 43 | 1, # arm! 44 | 0,0,0,0,0,0 # unused parameters for this command 45 | ) 46 | 47 | mode = 'STABILIZE' 48 | 49 | # Check if mode is available 50 | # if mode not in the_connection.mode_mapping(): 51 | # print('Unknown mode : {}'.format(mode)) 52 | # print('Try:', list(the_connection.mode_mapping().keys())) 53 | # sys.exit(1) 54 | 55 | 56 | # Get mode ID 57 | mode_id = 4 58 | #mode_id = the_connection.mode_mapping()[mode] 59 | 60 | 61 | # Set new mode 62 | # the_connection.mav.command_long_send( 63 | # the_connection.target_system, the_connection.target_component, 64 | # mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0, 65 | # 0, mode_id, 0, 0, 0, 0, 0) or: 66 | # the_connection.set_mode(mode_id) or: 67 | 68 | # the_connection.mav.set_mode_send( 69 | # the_connection.target_system, 70 | # mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, 71 | # mode_id) 72 | 73 | mavutil.mavfile.set_mode(the_connection,4,0,0) 74 | 75 | # while True: 76 | # # Wait for ACK command 77 | # ack_msg = the_connection.recv_match(type='COMMAND_ACK', blocking=True) 78 | # ack_msg = ack_msg.to_dict() 79 | 80 | # # Check if command in the same in `set_mode` 81 | # if ack_msg['command'] != mavutil.mavlink.MAVLINK_MSG_ID_SET_MODE: 82 | # continue 83 | 84 | # # Print the ACK result ! 85 | # print(mavutil.mavlink.enums['MAV_RESULT'][ack_msg['result']].description) 86 | # break 87 | 88 | 89 | altitude = 10 90 | 91 | the_connection.mav.command_long_send(the_connection.target_system, 92 | the_connection.target_component, 93 | mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 94 | 0, 0, 0, 0, 0, 0, 0, altitude) 95 | 96 | print("Before disarm") 97 | time.sleep(20) 98 | print("After disarm") 99 | 100 | the_connection.mav.command_long_send( 101 | 1, # autopilot system id 102 | 1, # autopilot component id 103 | 400, # command id, ARM/DISARM 104 | 0, # confirmation 105 | 0, # disarm! 106 | 0,0,0,0,0,0 # unused parameters for this command 107 | ) 108 | 109 | 110 | 111 | #################################################### 112 | # the_connection.mav.command_long_send( 113 | # the_connection.target_system, 114 | # the_connection.target_component, 115 | # mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 116 | # 0, 117 | # 1, 0, 0, 0, 0, 0, 0) 118 | 119 | # altitude = 30 120 | 121 | # the_connection.mav.command_long_send(the_connection.target_system, 122 | # the_connection.target_component, 123 | # mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 124 | # 0, 0, 0, 0, 0, 0, 0, altitude) 125 | 126 | input() -------------------------------------------------------------------------------- /mavlink_message.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example of how to connect pymavlink to an autopilot via an UDP connection 3 | """ 4 | import time 5 | # Import mavutil 6 | from pymavlink import mavutil 7 | 8 | # Create the connection 9 | # If using a companion computer 10 | # the default connection is available 11 | # at ip 192.168.2.1 and the port 14550 12 | # Note: The connection is done with 'udpin' and not 'udpout'. 13 | # You can check in http:192.168.2.2:2770/mavproxy that the communication made for 14550 14 | # uses a 'udpbcast' (client) and not 'udpin' (server). 15 | # If you want to use QGroundControl in parallel with your python script, 16 | # it's possible to add a new output port in http:192.168.2.2:2770/mavproxy as a new line. 17 | # E.g: --out udpbcast:192.168.2.255:yourport 18 | master = mavutil.mavlink_connection('tcp:127.0.0.1:5762') 19 | count = 0 20 | # Get some information ! 21 | while True: 22 | msg = master.recv_match() 23 | if not msg: 24 | continue 25 | else: 26 | print(msg) 27 | if(msg.get_type() == 'ATTITUDE'): 28 | print("\r\n") -------------------------------------------------------------------------------- /mavtest.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | Generate a message using different MAVLink versions, put in a buffer and then read from it. 5 | """ 6 | 7 | from __future__ import print_function 8 | from builtins import object 9 | 10 | from pymavlink.dialects.v10 import ardupilotmega as mavlink1 11 | from pymavlink.dialects.v20 import ardupilotmega as mavlink2 12 | 13 | class fifo(object): 14 | def __init__(self): 15 | self.buf = [] 16 | def write(self, data): 17 | self.buf += data 18 | return len(data) 19 | def read(self): 20 | return self.buf.pop(0) 21 | 22 | def test_protocol(mavlink, signing=False): 23 | # we will use a fifo as an encode/decode buffer 24 | f = fifo() 25 | 26 | print("Creating MAVLink message...") 27 | # create a mavlink instance, which will do IO on file object 'f' 28 | mav = mavlink.MAVLink(f) 29 | 30 | if signing: 31 | mav.signing.secret_key = chr(42)*32 32 | mav.signing.link_id = 0 33 | mav.signing.timestamp = 0 34 | mav.signing.sign_outgoing = True 35 | 36 | # set the WP_RADIUS parameter on the MAV at the end of the link 37 | mav.param_set_send(7, 1, "WP_RADIUS", 101, mavlink.MAV_PARAM_TYPE_REAL32) 38 | 39 | # alternatively, produce a MAVLink_param_set object 40 | # this can be sent via your own transport if you like 41 | m = mav.param_set_encode(7, 1, "WP_RADIUS", 101, mavlink.MAV_PARAM_TYPE_REAL32) 42 | 43 | m.pack(mav) 44 | 45 | # get the encoded message as a buffer 46 | b = m.get_msgbuf() 47 | 48 | bi=[] 49 | for c in b: 50 | bi.append(int(c)) 51 | print("Buffer containing the encoded message:") 52 | print(bi) 53 | 54 | print("Decoding message...") 55 | # decode an incoming message 56 | m2 = mav.decode(b) 57 | 58 | # show what fields it has 59 | print("Got a message with id %u and fields %s" % (m2.get_msgId(), m2.get_fieldnames())) 60 | 61 | # print out the fields 62 | print(m2) 63 | 64 | 65 | print("Testing mavlink1\n") 66 | test_protocol(mavlink1) 67 | 68 | print("\nTesting mavlink2\n") 69 | test_protocol(mavlink2) 70 | 71 | print("\nTesting mavlink2 with signing\n") 72 | test_protocol(mavlink2, True) 73 | -------------------------------------------------------------------------------- /mavtest.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aabs7/pymavlink_tutorial/fc78fca35fcd2c638001fe347c48bbf12042a850/mavtest.pyc -------------------------------------------------------------------------------- /mission.txt: -------------------------------------------------------------------------------- 1 | QGC WPL 110 2 | 0 0 0 16 0.0 0.0 0.0 0.0 -35.3632621765 149.165237427 584.08001709 1 3 | 1 0 3 22 15.0 0.0 0.0 0.0 0.0 0.0 10.0 1 4 | 2 0 3 16 0.0 0.0 0.0 0.0 -35.3628540039 149.163864136 10.0 1 5 | 3 0 3 16 0.0 0.0 0.0 0.0 -35.3620910645 149.163192749 10.0 1 6 | 4 0 3 18 1.0 0.0 80.0 1.0 -35.3616333008 149.163070679 10.0 1 7 | 5 0 3 16 0.0 0.0 0.0 0.0 -35.3612213135 149.163101196 10.0 1 8 | 6 0 3 16 0.0 0.0 0.0 0.0 -35.360584259 149.163391113 10.0 1 9 | 7 0 3 16 0.0 0.0 0.0 0.0 -35.3603134155 149.164123535 10.0 1 10 | 8 0 3 16 0.0 0.0 0.0 0.0 -35.3602294922 149.165115356 10.0 1 11 | 9 0 0 20 0.0 0.0 0.0 0.0 0.0 0.0 0.0 1 12 | 10 0 3 16 0.0 0.0 0.0 0.0 -35.3607711792 149.166412354 10.0 1 13 | 11 0 3 16 0.0 0.0 0.0 0.0 -35.3611717224 149.167007446 10.0 1 14 | 12 0 3 16 0.0 0.0 0.0 0.0 -35.3617210388 149.166732788 10.0 1 15 | 13 0 3 16 0.0 0.0 0.0 0.0 -35.3624534607 149.166183472 10.0 1 16 | -------------------------------------------------------------------------------- /mission_read.py: -------------------------------------------------------------------------------- 1 | # Import mavutil 2 | from pymavlink import mavutil 3 | from pymavlink import mavwp 4 | import time 5 | 6 | # Create the connection 7 | # From topside computer 8 | 9 | master = mavutil.mavlink_connection('tcp:127.0.0.1:5762') 10 | 11 | master.wait_heartbeat() 12 | 13 | 14 | master.waypoint_request_list_send() 15 | waypoint_count = 0 16 | 17 | msg = master.recv_match(type=['MISSION_COUNT'],blocking=True) 18 | waypoint_count = msg.count 19 | print("msg count:",waypoint_count) 20 | 21 | output = 'QGC WPL 110\n' 22 | 23 | for i in range(waypoint_count): 24 | master.waypoint_request_send(i) 25 | msg = master.recv_match(type=['MISSION_ITEM'],blocking = True) 26 | 27 | #print("Receiving waypoint {0}".format(msg.seq)) 28 | commandline="%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\n" % (msg.seq,msg.current,msg.frame,msg.command,msg.param1,msg.param2,msg.param3,msg.param4,msg.x,msg.y,msg.z,msg.autocontinue) 29 | output += commandline 30 | 31 | 32 | with open('mission.txt','w') as file_: 33 | print("Write mission to file") 34 | file_.write(output) -------------------------------------------------------------------------------- /mission_upload.py: -------------------------------------------------------------------------------- 1 | from pymavlink import mavutil 2 | from pymavlink import mavwp 3 | import time 4 | 5 | # Create the connection 6 | # From topside computer 7 | 8 | master = mavutil.mavlink_connection('tcp:127.0.0.1:5762') 9 | 10 | master.wait_heartbeat() 11 | 12 | wp = mavwp.MAVWPLoader() 13 | 14 | def cmd_set_home(home_location, altitude): 15 | print('--- ', master.target_system, ',', master.target_component) 16 | master.mav.command_long_send( 17 | master.target_system, master.target_component, 18 | mavutil.mavlink.MAV_CMD_DO_SET_HOME, 19 | 1, # set position 20 | 0, # param1 21 | 0, # param2 22 | 0, # param3 23 | 0, # param4 24 | home_location[0], # lat 25 | home_location[1], # lon 26 | altitude) 27 | 28 | def uploadmission(aFileName): 29 | home_location = None 30 | home_altitude = None 31 | 32 | with open(aFileName) as f: 33 | for i, line in enumerate(f): 34 | if i==0: 35 | if not line.startswith('QGC WPL 110'): 36 | raise Exception('File is not supported WP version') 37 | else: 38 | linearray=line.split('\t') 39 | ln_seq = int(linearray[0]) 40 | ln_current = int(linearray[1]) 41 | ln_frame = int(linearray[2]) 42 | ln_command = int(linearray[3]) 43 | ln_param1=float(linearray[4]) 44 | ln_param2=float(linearray[5]) 45 | ln_param3=float(linearray[6]) 46 | ln_param4=float(linearray[7]) 47 | ln_x=(float(linearray[8])) 48 | ln_y=(float(linearray[9])) 49 | ln_z=float(linearray[10]) 50 | ln_autocontinue = int(float(linearray[11].strip())) 51 | if(i == 1): 52 | home_location = (ln_x,ln_y) 53 | home_altitude = ln_z 54 | p = mavutil.mavlink.MAVLink_mission_item_message(master.target_system, master.target_component, ln_seq, ln_frame, 55 | ln_command, 56 | ln_current, ln_autocontinue, ln_param1, ln_param2, ln_param3, ln_param4, ln_x, ln_y, ln_z) 57 | wp.add(p) 58 | 59 | 60 | cmd_set_home(home_location,home_altitude) 61 | msg = master.recv_match(type = ['COMMAND_ACK'],blocking = True) 62 | print(msg) 63 | print('Set home location: {0} {1}'.format(home_location[0],home_location[1])) 64 | time.sleep(1) 65 | 66 | #send waypoint to airframe 67 | master.waypoint_clear_all_send() 68 | master.waypoint_count_send(wp.count()) 69 | for i in range(wp.count()): 70 | msg = master.recv_match(type=['MISSION_REQUEST'],blocking=True) 71 | print(msg) 72 | master.mav.send(wp.wp(msg.seq)) 73 | #print(wp.wp(msg.seq)) 74 | print('Sending waypoint {0}'.format(msg.seq)) 75 | 76 | 77 | uploadmission('mission.txt') 78 | -------------------------------------------------------------------------------- /read_important.py: -------------------------------------------------------------------------------- 1 | # Import mavutil 2 | from pymavlink import mavutil 3 | 4 | # Create the connection 5 | # From topside computer 6 | master = mavutil.mavlink_connection('tcp:127.0.0.1:5762') 7 | 8 | MAV_STATE = { 9 | 0 : 'UNINIT', 10 | 1 : 'BOOT', 11 | 2 : 'CALIBRATING', 12 | 3 : 'STANDBY', 13 | 4 : 'ACTIVE', 14 | 5 : 'CRITICAL', 15 | 6 : 'EMERGENCY', 16 | 7 : 'POWEROFF', 17 | 8 : 'FLIGHT_TERMINATION' 18 | } 19 | 20 | while True: 21 | msg = master.recv_match() 22 | 23 | if not msg: 24 | continue 25 | else: 26 | dict_msg = msg.to_dict() 27 | if msg.get_type() == 'HEARTBEAT': 28 | if (dict_msg['type'] != 6): 29 | print("Flt Mode:",mavutil.mode_mapping_acm[dict_msg['custom_mode']]) 30 | print("Status:", MAV_STATE[dict_msg['system_status']]) 31 | 32 | if msg.get_type() == 'GLOBAL_POSITION_INT': 33 | print("Lat:",dict_msg['lat']," Lon:",dict_msg['lon']," Alt:",dict_msg['alt']," Alt R:",dict_msg['relative_alt']," Heading:",dict_msg['hdg']) 34 | 35 | if msg.get_type() == 'GPS_RAW_INT': 36 | print("Sat:", dict_msg['satellites_visible']," Hdop:",dict_msg['eph']," Fix:",dict_msg['fix_type']) 37 | 38 | if msg.get_type() == 'VFR_HUD': 39 | print("GS:",dict_msg['groundspeed']," AS:",dict_msg['airspeed']," Heading:",dict_msg['heading']) 40 | print("Armed:",master.motors_armed()) 41 | 42 | if msg.get_type() == 'BATTERY_STATUS': 43 | print("volt:",dict_msg['voltages'][0] * 0.001) 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /read_parameters.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example of how to read all the parameters from the Autopilot with pymavlink 3 | """ 4 | 5 | # Disable "Broad exception" warning 6 | # pylint: disable=W0703 7 | 8 | import time 9 | import sys 10 | 11 | # Import mavutil 12 | from pymavlink import mavutil 13 | 14 | 15 | # Create the connection 16 | master = mavutil.mavlink_connection('tcp:127.0.0.1:5762') 17 | # Wait a heartbeat before sending commands 18 | master.wait_heartbeat() 19 | 20 | # Request all parameters 21 | master.mav.param_request_list_send( 22 | master.target_system, master.target_component 23 | ) 24 | while True: 25 | time.sleep(0.01) 26 | try: 27 | message = master.recv_match(type='PARAM_VALUE', blocking=True).to_dict() 28 | print('name: %s\tvalue: %d' % (message['param_id'], 29 | message['param_value'])) 30 | except Exception as error: 31 | print(error) 32 | sys.exit(0) -------------------------------------------------------------------------------- /specific_msg.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example of how to connect pymavlink to an autopilot via an UDP connection 3 | """ 4 | import time 5 | # Import mavutil 6 | from pymavlink import mavutil 7 | 8 | import sys 9 | 10 | # Create the connection 11 | # If using a companion computer 12 | # the default connection is available 13 | # at ip 192.168.2.1 and the port 14550 14 | # Note: The connection is done with 'udpin' and not 'udpout'. 15 | # You can check in http:192.168.2.2:2770/mavproxy that the communication made for 14550 16 | # uses a 'udpbcast' (client) and not 'udpin' (server). 17 | # If you want to use QGroundControl in parallel with your python script, 18 | # it's possible to add a new output port in http:192.168.2.2:2770/mavproxy as a new line. 19 | # E.g: --out udpbcast:192.168.2.255:yourport 20 | master = mavutil.mavlink_connection('tcp:127.0.0.1:5762') 21 | 22 | # Get some information ! 23 | while True: 24 | try: 25 | print(master.recv_match(type = 'LOCAL_POSITION_NED',blocking=True)) 26 | except: 27 | pass 28 | time.sleep(0.1) 29 | -------------------------------------------------------------------------------- /takeoff.py: -------------------------------------------------------------------------------- 1 | # Import mavutil 2 | from pymavlink import mavutil 3 | 4 | # Create the connection 5 | # From topside computer 6 | master = mavutil.mavlink_connection('tcp:127.0.0.1:5762') 7 | 8 | # Wait a heartbeat before sending commands 9 | master.wait_heartbeat() 10 | 11 | mavutil.mavfile.set_mode(master,'GUIDED',0,0) 12 | 13 | # master.arducopter_arm() or: 14 | master.mav.command_long_send( 15 | master.target_system, 16 | master.target_component, 17 | mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 18 | 0, 19 | 1, 0, 0, 0, 0, 0, 0) 20 | altitude = 10 21 | #mavutil.mavfile.set_mode(master,'STABILIZE',0,0) 22 | master.mav.command_long_send(0, 0, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 23 | 0, 0, 0, 0, 0, 0, 0, altitude) 24 | 25 | mavutil.mavfile.set_mode(master,'AUTO',0,0) 26 | input() --------------------------------------------------------------------------------