mavutil.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 '''
00003 mavlink python utility functions
00004 
00005 Copyright Andrew Tridgell 2011
00006 Released under GNU GPL version 3 or later
00007 '''
00008 
00009 import socket, math, struct, time, os, fnmatch, array, sys, errno
00010 import select, mavexpression
00011 
00012 # adding these extra imports allows pymavlink to be used directly with pyinstaller
00013 # without having complex spec files. To allow for installs that don't have ardupilotmega
00014 # at all we avoid throwing an exception if it isn't installed
00015 try:
00016     import json
00017     from pymavlink.dialects.v10 import ardupilotmega
00018 except Exception:
00019     pass
00020 
00021 # maximum packet length for a single receive call - use the UDP limit
00022 UDP_MAX_PACKET_LEN = 65535
00023 
00024 # Store the MAVLink library for the currently-selected dialect
00025 # (set by set_dialect())
00026 mavlink = None
00027 
00028 # Store the mavlink file currently being operated on
00029 # (set by mavlink_connection())
00030 mavfile_global = None
00031 
00032 # If the caller hasn't specified a particular native/legacy version, use this
00033 default_native = False
00034 
00035 # link_id used for signing
00036 global_link_id = 0
00037 
00038 # Use a globally-set MAVLink dialect if one has been specified as an environment variable.
00039 if not 'MAVLINK_DIALECT' in os.environ:
00040     os.environ['MAVLINK_DIALECT'] = 'ardupilotmega'
00041 
00042 def mavlink10():
00043     '''return True if using MAVLink 1.0 or later'''
00044     return not 'MAVLINK09' in os.environ
00045 
00046 def mavlink20():
00047     '''return True if using MAVLink 2.0'''
00048     return 'MAVLINK20' in os.environ
00049 
00050 def evaluate_expression(expression, vars):
00051     '''evaluation an expression'''
00052     return mavexpression.evaluate_expression(expression, vars)
00053 
00054 def evaluate_condition(condition, vars):
00055     '''evaluation a conditional (boolean) statement'''
00056     if condition is None:
00057         return True
00058     v = evaluate_expression(condition, vars)
00059     if v is None:
00060         return False
00061     return v
00062 
00063 class location(object):
00064     '''represent a GPS coordinate'''
00065     def __init__(self, lat, lng, alt=0, heading=0):
00066         self.lat = lat
00067         self.lng = lng
00068         self.alt = alt
00069         self.heading = heading
00070 
00071     def __str__(self):
00072         return "lat=%.6f,lon=%.6f,alt=%.1f" % (self.lat, self.lng, self.alt)
00073 
00074 def set_dialect(dialect):
00075     '''set the MAVLink dialect to work with.
00076     For example, set_dialect("ardupilotmega")
00077     '''
00078     global mavlink, current_dialect
00079     from .generator import mavparse
00080     if 'MAVLINK20' in os.environ:
00081         print("Using MAVLink 2.0")
00082         wire_protocol = mavparse.PROTOCOL_2_0
00083         modname = "pymavlink.dialects.v20." + dialect
00084     elif mavlink is None or mavlink.WIRE_PROTOCOL_VERSION == "1.0" or not 'MAVLINK09' in os.environ:
00085         print("Using MAVLink 1.0")
00086         wire_protocol = mavparse.PROTOCOL_1_0
00087         modname = "pymavlink.dialects.v10." + dialect
00088     else:
00089         print("Using MAVLink 0.9")
00090         wire_protocol = mavparse.PROTOCOL_0_9
00091         modname = "pymavlink.dialects.v09." + dialect
00092 
00093     try:
00094         mod = __import__(modname)
00095     except Exception:
00096         # auto-generate the dialect module
00097         from .generator.mavgen import mavgen_python_dialect
00098         mavgen_python_dialect(dialect, wire_protocol)
00099         mod = __import__(modname)
00100     components = modname.split('.')
00101     for comp in components[1:]:
00102         mod = getattr(mod, comp)
00103     current_dialect = dialect
00104     mavlink = mod
00105 
00106 # Set the default dialect. This is done here as it needs to be after the function declaration
00107 set_dialect(os.environ['MAVLINK_DIALECT'])
00108 
00109 class mavfile(object):
00110     '''a generic mavlink port'''
00111     def __init__(self, fd, address, source_system=255, notimestamps=False, input=True, use_native=default_native):
00112         global mavfile_global
00113         if input:
00114             mavfile_global = self
00115         self.fd = fd
00116         self.address = address
00117         self.messages = { 'MAV' : self }
00118         if float(mavlink.WIRE_PROTOCOL_VERSION) >= 1:
00119             self.messages['HOME'] = mavlink.MAVLink_gps_raw_int_message(0,0,0,0,0,0,0,0,0,0)
00120             mavlink.MAVLink_waypoint_message = mavlink.MAVLink_mission_item_message
00121         else:
00122             self.messages['HOME'] = mavlink.MAVLink_gps_raw_message(0,0,0,0,0,0,0,0,0)
00123         self.params = {}
00124         self.target_system = 0
00125         self.target_component = 0
00126         self.source_system = source_system
00127         self.first_byte = True
00128         self.robust_parsing = True
00129         self.mav = mavlink.MAVLink(self, srcSystem=self.source_system, use_native=use_native)
00130         self.mav.robust_parsing = self.robust_parsing
00131         self.logfile = None
00132         self.logfile_raw = None
00133         self.param_fetch_in_progress = False
00134         self.param_fetch_complete = False
00135         self.start_time = time.time()
00136         self.flightmode = "UNKNOWN"
00137         self.vehicle_type = "UNKNOWN"
00138         self.mav_type = mavlink.MAV_TYPE_FIXED_WING
00139         self.base_mode = 0
00140         self.timestamp = 0
00141         self.message_hooks = []
00142         self.idle_hooks = []
00143         self.uptime = 0.0
00144         self.notimestamps = notimestamps
00145         self._timestamp = None
00146         self.ground_pressure = None
00147         self.ground_temperature = None
00148         self.altitude = 0
00149         self.WIRE_PROTOCOL_VERSION = mavlink.WIRE_PROTOCOL_VERSION
00150         self.last_seq = {}
00151         self.mav_loss = 0
00152         self.mav_count = 0
00153         self.stop_on_EOF = False
00154         self.portdead = False
00155 
00156     def auto_mavlink_version(self, buf):
00157         '''auto-switch mavlink protocol version'''
00158         global mavlink
00159         if len(buf) == 0:
00160             return
00161         try:
00162             magic = ord(buf[0])
00163         except:
00164             magic = buf[0]
00165         if not magic in [ 85, 254, 253 ]:
00166             return
00167         self.first_byte = False
00168         if self.WIRE_PROTOCOL_VERSION == "0.9" and magic == 254:
00169             self.WIRE_PROTOCOL_VERSION = "1.0"
00170             set_dialect(current_dialect)
00171         elif self.WIRE_PROTOCOL_VERSION == "1.0" and magic == 85:
00172             self.WIRE_PROTOCOL_VERSION = "0.9"
00173             os.environ['MAVLINK09'] = '1'
00174             set_dialect(current_dialect)
00175         elif self.WIRE_PROTOCOL_VERSION != "2.0" and magic == 253:
00176             self.WIRE_PROTOCOL_VERSION = "2.0"
00177             os.environ['MAVLINK20'] = '1'
00178             set_dialect(current_dialect)
00179         else:
00180             return
00181         # switch protocol 
00182         (callback, callback_args, callback_kwargs) = (self.mav.callback,
00183                                                       self.mav.callback_args,
00184                                                       self.mav.callback_kwargs)
00185         self.mav = mavlink.MAVLink(self, srcSystem=self.source_system)
00186         self.mav.robust_parsing = self.robust_parsing
00187         self.WIRE_PROTOCOL_VERSION = mavlink.WIRE_PROTOCOL_VERSION
00188         (self.mav.callback, self.mav.callback_args, self.mav.callback_kwargs) = (callback,
00189                                                                                  callback_args,
00190                                                                                  callback_kwargs)
00191 
00192     def recv(self, n=None):
00193         '''default recv method'''
00194         raise RuntimeError('no recv() method supplied')
00195 
00196     def close(self, n=None):
00197         '''default close method'''
00198         raise RuntimeError('no close() method supplied')
00199 
00200     def write(self, buf):
00201         '''default write method'''
00202         raise RuntimeError('no write() method supplied')
00203 
00204 
00205     def select(self, timeout):
00206         '''wait for up to timeout seconds for more data'''
00207         if self.fd is None:
00208             time.sleep(min(timeout,0.5))
00209             return True
00210         try:
00211             (rin, win, xin) = select.select([self.fd], [], [], timeout)
00212         except select.error:
00213             return False
00214         return len(rin) == 1
00215 
00216     def pre_message(self):
00217         '''default pre message call'''
00218         return
00219 
00220     def set_rtscts(self, enable):
00221         '''enable/disable RTS/CTS if applicable'''
00222         return
00223 
00224     def post_message(self, msg):
00225         '''default post message call'''
00226         if '_posted' in msg.__dict__:
00227             return
00228         msg._posted = True
00229         msg._timestamp = time.time()
00230         type = msg.get_type()
00231         if type != 'HEARTBEAT' or (msg.type != mavlink.MAV_TYPE_GCS and msg.type != mavlink.MAV_TYPE_GIMBAL):
00232             self.messages[type] = msg
00233 
00234         if 'usec' in msg.__dict__:
00235             self.uptime = msg.usec * 1.0e-6
00236         if 'time_boot_ms' in msg.__dict__:
00237             self.uptime = msg.time_boot_ms * 1.0e-3
00238 
00239         if self._timestamp is not None:
00240             if self.notimestamps:
00241                 msg._timestamp = self.uptime
00242             else:
00243                 msg._timestamp = self._timestamp
00244 
00245         src_system = msg.get_srcSystem()
00246         src_component = msg.get_srcComponent()
00247         src_tuple = (src_system, src_component)
00248         radio_tuple = (ord('3'), ord('D'))
00249         if not (src_tuple == radio_tuple or msg.get_type() == 'BAD_DATA'):
00250             if not src_tuple in self.last_seq:
00251                 last_seq = -1
00252             else:
00253                 last_seq = self.last_seq[src_tuple]
00254             seq = (last_seq+1) % 256
00255             seq2 = msg.get_seq()
00256             if seq != seq2 and last_seq != -1:
00257                 diff = (seq2 - seq) % 256
00258                 self.mav_loss += diff
00259                 #print("lost %u seq=%u seq2=%u last_seq=%u src_system=%u %s" % (diff, seq, seq2, last_seq, src_system, msg.get_type()))
00260             self.last_seq[src_tuple] = seq2
00261             self.mav_count += 1
00262         
00263         self.timestamp = msg._timestamp
00264         if type == 'HEARTBEAT' and msg.get_srcComponent() != mavlink.MAV_COMP_ID_GIMBAL:
00265             self.target_system = msg.get_srcSystem()
00266             self.target_component = msg.get_srcComponent()
00267             if float(mavlink.WIRE_PROTOCOL_VERSION) >= 1 and msg.type != mavlink.MAV_TYPE_GCS:
00268                 self.flightmode = mode_string_v10(msg)
00269                 self.mav_type = msg.type
00270                 self.base_mode = msg.base_mode
00271         elif type == 'PARAM_VALUE':
00272             s = str(msg.param_id)
00273             self.params[str(msg.param_id)] = msg.param_value
00274             if msg.param_index+1 == msg.param_count:
00275                 self.param_fetch_in_progress = False
00276                 self.param_fetch_complete = True
00277         elif type == 'SYS_STATUS' and mavlink.WIRE_PROTOCOL_VERSION == '0.9':
00278             self.flightmode = mode_string_v09(msg)
00279         elif type == 'GPS_RAW':
00280             if self.messages['HOME'].fix_type < 2:
00281                 self.messages['HOME'] = msg
00282         elif type == 'GPS_RAW_INT':
00283             if self.messages['HOME'].fix_type < 3:
00284                 self.messages['HOME'] = msg
00285         for hook in self.message_hooks:
00286             hook(self, msg)
00287 
00288         if (msg.get_signed() and
00289             self.mav.signing.link_id == 0 and
00290             msg.get_link_id() != 0 and
00291             self.target_system == msg.get_srcSystem() and
00292             self.target_component == msg.get_srcComponent()):
00293             # change to link_id from incoming packet
00294             self.mav.signing.link_id = msg.get_link_id()
00295 
00296 
00297     def packet_loss(self):
00298         '''packet loss as a percentage'''
00299         if self.mav_count == 0:
00300             return 0
00301         return (100.0*self.mav_loss)/(self.mav_count+self.mav_loss)
00302 
00303 
00304     def recv_msg(self):
00305         '''message receive routine'''
00306         self.pre_message()
00307         while True:
00308             n = self.mav.bytes_needed()
00309             s = self.recv(n)
00310             numnew = len(s)
00311 
00312             if numnew != 0:
00313                 if self.logfile_raw:
00314                     self.logfile_raw.write(str(s))
00315                 if self.first_byte:
00316                     self.auto_mavlink_version(s)
00317 
00318             # We always call parse_char even if the new string is empty, because the existing message buf might already have some valid packet
00319             # we can extract
00320             msg = self.mav.parse_char(s)
00321             if msg:
00322                 if self.logfile and  msg.get_type() != 'BAD_DATA' :
00323                     usec = int(time.time() * 1.0e6) & ~3
00324                     self.logfile.write(str(struct.pack('>Q', usec) + msg.get_msgbuf()))
00325                 self.post_message(msg)
00326                 return msg
00327             else:
00328                 # if we failed to parse any messages _and_ no new bytes arrived, return immediately so the client has the option to
00329                 # timeout
00330                 if numnew == 0:
00331                     return None
00332                 
00333     def recv_match(self, condition=None, type=None, blocking=False, timeout=None):
00334         '''recv the next MAVLink message that matches the given condition
00335         type can be a string or a list of strings'''
00336         if type is not None and not isinstance(type, list):
00337             type = [type]
00338         start_time = time.time()
00339         while True:
00340             if timeout is not None:
00341                 now = time.time()
00342                 if now < start_time:
00343                     start_time = now # If an external process rolls back system time, we should not spin forever.
00344                 if start_time + timeout < time.time():
00345                     return None
00346             m = self.recv_msg()
00347             if m is None:
00348                 if blocking:
00349                     for hook in self.idle_hooks:
00350                         hook(self)
00351                     if timeout is None:
00352                         self.select(0.05)
00353                     else:
00354                         self.select(timeout/2)
00355                     continue
00356                 return None
00357             if type is not None and not m.get_type() in type:
00358                 continue
00359             if not evaluate_condition(condition, self.messages):
00360                 continue
00361             return m
00362 
00363     def check_condition(self, condition):
00364         '''check if a condition is true'''
00365         return evaluate_condition(condition, self.messages)
00366 
00367     def mavlink10(self):
00368         '''return True if using MAVLink 1.0 or later'''
00369         return float(self.WIRE_PROTOCOL_VERSION) >= 1
00370 
00371     def mavlink20(self):
00372         '''return True if using MAVLink 2.0 or later'''
00373         return float(self.WIRE_PROTOCOL_VERSION) >= 2
00374 
00375     def setup_logfile(self, logfile, mode='w'):
00376         '''start logging to the given logfile, with timestamps'''
00377         self.logfile = open(logfile, mode=mode)
00378 
00379     def setup_logfile_raw(self, logfile, mode='w'):
00380         '''start logging raw bytes to the given logfile, without timestamps'''
00381         self.logfile_raw = open(logfile, mode=mode)
00382 
00383     def wait_heartbeat(self, blocking=True):
00384         '''wait for a heartbeat so we know the target system IDs'''
00385         return self.recv_match(type='HEARTBEAT', blocking=blocking)
00386 
00387     def param_fetch_all(self):
00388         '''initiate fetch of all parameters'''
00389         if time.time() - getattr(self, 'param_fetch_start', 0) < 2.0:
00390             # don't fetch too often
00391             return
00392         self.param_fetch_start = time.time()
00393         self.param_fetch_in_progress = True
00394         self.mav.param_request_list_send(self.target_system, self.target_component)
00395 
00396     def param_fetch_one(self, name):
00397         '''initiate fetch of one parameter'''
00398         try:
00399             idx = int(name)
00400             self.mav.param_request_read_send(self.target_system, self.target_component, "", idx)
00401         except Exception:
00402             self.mav.param_request_read_send(self.target_system, self.target_component, name, -1)
00403 
00404     def time_since(self, mtype):
00405         '''return the time since the last message of type mtype was received'''
00406         if not mtype in self.messages:
00407             return time.time() - self.start_time
00408         return time.time() - self.messages[mtype]._timestamp
00409 
00410     def param_set_send(self, parm_name, parm_value, parm_type=None):
00411         '''wrapper for parameter set'''
00412         if self.mavlink10():
00413             if parm_type == None:
00414                 parm_type = mavlink.MAVLINK_TYPE_FLOAT
00415             self.mav.param_set_send(self.target_system, self.target_component,
00416                                     parm_name, parm_value, parm_type)
00417         else:
00418             self.mav.param_set_send(self.target_system, self.target_component,
00419                                     parm_name, parm_value)
00420 
00421     def waypoint_request_list_send(self):
00422         '''wrapper for waypoint_request_list_send'''
00423         if self.mavlink10():
00424             self.mav.mission_request_list_send(self.target_system, self.target_component)
00425         else:
00426             self.mav.waypoint_request_list_send(self.target_system, self.target_component)
00427 
00428     def waypoint_clear_all_send(self):
00429         '''wrapper for waypoint_clear_all_send'''
00430         if self.mavlink10():
00431             self.mav.mission_clear_all_send(self.target_system, self.target_component)
00432         else:
00433             self.mav.waypoint_clear_all_send(self.target_system, self.target_component)
00434 
00435     def waypoint_request_send(self, seq):
00436         '''wrapper for waypoint_request_send'''
00437         if self.mavlink10():
00438             self.mav.mission_request_send(self.target_system, self.target_component, seq)
00439         else:
00440             self.mav.waypoint_request_send(self.target_system, self.target_component, seq)
00441 
00442     def waypoint_set_current_send(self, seq):
00443         '''wrapper for waypoint_set_current_send'''
00444         if self.mavlink10():
00445             self.mav.mission_set_current_send(self.target_system, self.target_component, seq)
00446         else:
00447             self.mav.waypoint_set_current_send(self.target_system, self.target_component, seq)
00448 
00449     def waypoint_current(self):
00450         '''return current waypoint'''
00451         if self.mavlink10():
00452             m = self.recv_match(type='MISSION_CURRENT', blocking=True)
00453         else:
00454             m = self.recv_match(type='WAYPOINT_CURRENT', blocking=True)
00455         return m.seq
00456 
00457     def waypoint_count_send(self, seq):
00458         '''wrapper for waypoint_count_send'''
00459         if self.mavlink10():
00460             self.mav.mission_count_send(self.target_system, self.target_component, seq)
00461         else:
00462             self.mav.waypoint_count_send(self.target_system, self.target_component, seq)
00463 
00464     def set_mode_flag(self, flag, enable):
00465         '''
00466         Enables/ disables MAV_MODE_FLAG
00467         @param flag The mode flag, 
00468           see MAV_MODE_FLAG enum
00469         @param enable Enable the flag, (True/False)
00470         '''
00471         if self.mavlink10():
00472             mode = self.base_mode
00473             if (enable == True):
00474                 mode = mode | flag
00475             elif (enable == False):
00476                 mode = mode & ~flag
00477             self.mav.command_long_send(self.target_system, self.target_component,
00478                                            mavlink.MAV_CMD_DO_SET_MODE, 0,
00479                                            mode,
00480                                            0, 0, 0, 0, 0, 0)
00481         else:
00482             print("Set mode flag not supported")
00483 
00484     def set_mode_auto(self):
00485         '''enter auto mode'''
00486         if self.mavlink10():
00487             self.mav.command_long_send(self.target_system, self.target_component,
00488                                        mavlink.MAV_CMD_MISSION_START, 0, 0, 0, 0, 0, 0, 0, 0)
00489         else:
00490             MAV_ACTION_SET_AUTO = 13
00491             self.mav.action_send(self.target_system, self.target_component, MAV_ACTION_SET_AUTO)
00492 
00493     def mode_mapping(self):
00494         '''return dictionary mapping mode names to numbers, or None if unknown'''
00495         mav_type = self.field('HEARTBEAT', 'type', self.mav_type)
00496         if mav_type is None:
00497             return None
00498         map = None
00499         if mav_type in [mavlink.MAV_TYPE_QUADROTOR,
00500                         mavlink.MAV_TYPE_HELICOPTER,
00501                         mavlink.MAV_TYPE_HEXAROTOR,
00502                         mavlink.MAV_TYPE_OCTOROTOR,
00503                         mavlink.MAV_TYPE_TRICOPTER]:
00504             map = mode_mapping_acm
00505         if mav_type == mavlink.MAV_TYPE_FIXED_WING:
00506             map = mode_mapping_apm
00507         if mav_type == mavlink.MAV_TYPE_GROUND_ROVER:
00508             map = mode_mapping_rover
00509         if mav_type == mavlink.MAV_TYPE_ANTENNA_TRACKER:
00510             map = mode_mapping_tracker
00511         if map is None:
00512             return None
00513         inv_map = dict((a, b) for (b, a) in list(map.items()))
00514         return inv_map
00515 
00516     def set_mode(self, mode):
00517         '''enter arbitrary mode'''
00518         if isinstance(mode, str):
00519             map = self.mode_mapping()
00520             if map is None or mode not in map:
00521                 print("Unknown mode '%s'" % mode)
00522                 return
00523             mode = map[mode]
00524         self.mav.set_mode_send(self.target_system,
00525                                mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
00526                                mode)
00527 
00528     def set_mode_rtl(self):
00529         '''enter RTL mode'''
00530         if self.mavlink10():
00531             self.mav.command_long_send(self.target_system, self.target_component,
00532                                        mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0, 0, 0, 0, 0, 0)
00533         else:
00534             MAV_ACTION_RETURN = 3
00535             self.mav.action_send(self.target_system, self.target_component, MAV_ACTION_RETURN)
00536 
00537     def set_mode_manual(self):
00538         '''enter MANUAL mode'''
00539         if self.mavlink10():
00540             self.mav.command_long_send(self.target_system, self.target_component,
00541                                        mavlink.MAV_CMD_DO_SET_MODE, 0,
00542                                        mavlink.MAV_MODE_MANUAL_ARMED,
00543                                        0, 0, 0, 0, 0, 0)
00544         else:
00545             MAV_ACTION_SET_MANUAL = 12
00546             self.mav.action_send(self.target_system, self.target_component, MAV_ACTION_SET_MANUAL)
00547 
00548     def set_mode_fbwa(self):
00549         '''enter FBWA mode'''
00550         if self.mavlink10():
00551             self.mav.command_long_send(self.target_system, self.target_component,
00552                                        mavlink.MAV_CMD_DO_SET_MODE, 0,
00553                                        mavlink.MAV_MODE_STABILIZE_ARMED,
00554                                        0, 0, 0, 0, 0, 0)
00555         else:
00556             print("Forcing FBWA not supported")
00557 
00558     def set_mode_loiter(self):
00559         '''enter LOITER mode'''
00560         if self.mavlink10():
00561             self.mav.command_long_send(self.target_system, self.target_component,
00562                                        mavlink.MAV_CMD_NAV_LOITER_UNLIM, 0, 0, 0, 0, 0, 0, 0, 0)
00563         else:
00564             MAV_ACTION_LOITER = 27
00565             self.mav.action_send(self.target_system, self.target_component, MAV_ACTION_LOITER)
00566 
00567     def set_servo(self, channel, pwm):
00568         '''set a servo value'''
00569         self.mav.command_long_send(self.target_system, self.target_component,
00570                                    mavlink.MAV_CMD_DO_SET_SERVO, 0,
00571                                    channel, pwm,
00572                                    0, 0, 0, 0, 0)
00573 
00574 
00575     def set_relay(self, relay_pin=0, state=True):
00576         '''Set relay_pin to value of state'''
00577         if self.mavlink10():
00578             self.mav.command_long_send(
00579                 self.target_system,  # target_system
00580                 self.target_component, # target_component
00581                 mavlink.MAV_CMD_DO_SET_RELAY, # command
00582                 0, # Confirmation
00583                 relay_pin, # Relay Number
00584                 int(state), # state (1 to indicate arm)
00585                 0, # param3 (all other params meaningless)
00586                 0, # param4
00587                 0, # param5
00588                 0, # param6
00589                 0) # param7
00590         else:
00591             print("Setting relays not supported.")
00592 
00593     def calibrate_level(self):
00594         '''calibrate accels (1D version)'''
00595         self.mav.command_long_send(self.target_system, self.target_component,
00596                                    mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, 0,
00597                                    1, 1, 0, 0, 0, 0, 0)
00598 
00599     def calibrate_pressure(self):
00600         '''calibrate pressure'''
00601         if self.mavlink10():
00602             self.mav.command_long_send(self.target_system, self.target_component,
00603                                        mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, 0,
00604                                        0, 0, 1, 0, 0, 0, 0)
00605         else:
00606             MAV_ACTION_CALIBRATE_PRESSURE = 20
00607             self.mav.action_send(self.target_system, self.target_component, MAV_ACTION_CALIBRATE_PRESSURE)
00608 
00609     def reboot_autopilot(self, hold_in_bootloader=False):
00610         '''reboot the autopilot'''
00611         if self.mavlink10():
00612             if hold_in_bootloader:
00613                 param1 = 3
00614             else:
00615                 param1 = 1
00616             self.mav.command_long_send(self.target_system, self.target_component,
00617                                        mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 0,
00618                                        param1, 0, 0, 0, 0, 0, 0)
00619             # send an old style reboot immediately afterwards in case it is an older firmware
00620             # that doesn't understand the new convention
00621             self.mav.command_long_send(self.target_system, self.target_component,
00622                                        mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 0,
00623                                        1, 0, 0, 0, 0, 0, 0)
00624 
00625     def wait_gps_fix(self):
00626         self.recv_match(type='VFR_HUD', blocking=True)
00627         if self.mavlink10():
00628             self.recv_match(type='GPS_RAW_INT', blocking=True,
00629                             condition='GPS_RAW_INT.fix_type==3 and GPS_RAW_INT.lat != 0 and GPS_RAW_INT.alt != 0')
00630         else:
00631             self.recv_match(type='GPS_RAW', blocking=True,
00632                             condition='GPS_RAW.fix_type==2 and GPS_RAW.lat != 0 and GPS_RAW.alt != 0')
00633 
00634     def location(self, relative_alt=False):
00635         '''return current location'''
00636         self.wait_gps_fix()
00637         # wait for another VFR_HUD, to ensure we have correct altitude
00638         self.recv_match(type='VFR_HUD', blocking=True)
00639         self.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
00640         if relative_alt:
00641             alt = self.messages['GLOBAL_POSITION_INT'].relative_alt*0.001
00642         else:
00643             alt = self.messages['VFR_HUD'].alt
00644         return location(self.messages['GPS_RAW_INT'].lat*1.0e-7,
00645                         self.messages['GPS_RAW_INT'].lon*1.0e-7,
00646                         alt,
00647                         self.messages['VFR_HUD'].heading)
00648 
00649     def arducopter_arm(self):
00650         '''arm motors (arducopter only)'''
00651         if self.mavlink10():
00652             self.mav.command_long_send(
00653                 self.target_system,  # target_system
00654                 self.target_component,
00655                 mavlink.MAV_CMD_COMPONENT_ARM_DISARM, # command
00656                 0, # confirmation
00657                 1, # param1 (1 to indicate arm)
00658                 0, # param2 (all other params meaningless)
00659                 0, # param3
00660                 0, # param4
00661                 0, # param5
00662                 0, # param6
00663                 0) # param7
00664 
00665     def arducopter_disarm(self):
00666         '''calibrate pressure'''
00667         if self.mavlink10():
00668             self.mav.command_long_send(
00669                 self.target_system,  # target_system
00670                 self.target_component,
00671                 mavlink.MAV_CMD_COMPONENT_ARM_DISARM, # command
00672                 0, # confirmation
00673                 0, # param1 (0 to indicate disarm)
00674                 0, # param2 (all other params meaningless)
00675                 0, # param3
00676                 0, # param4
00677                 0, # param5
00678                 0, # param6
00679                 0) # param7
00680 
00681     def motors_armed(self):
00682         '''return true if motors armed'''
00683         if not 'HEARTBEAT' in self.messages:
00684             return False
00685         m = self.messages['HEARTBEAT']
00686         return (m.base_mode & mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
00687 
00688     def motors_armed_wait(self):
00689         '''wait for motors to be armed'''
00690         while True:
00691             m = self.wait_heartbeat()
00692             if self.motors_armed():
00693                 return
00694 
00695     def motors_disarmed_wait(self):
00696         '''wait for motors to be disarmed'''
00697         while True:
00698             m = self.wait_heartbeat()
00699             if not self.motors_armed():
00700                 return
00701 
00702 
00703     def field(self, type, field, default=None):
00704         '''convenient function for returning an arbitrary MAVLink
00705            field with a default'''
00706         if not type in self.messages:
00707             return default
00708         return getattr(self.messages[type], field, default)
00709 
00710     def param(self, name, default=None):
00711         '''convenient function for returning an arbitrary MAVLink
00712            parameter with a default'''
00713         if not name in self.params:
00714             return default
00715         return self.params[name]
00716 
00717     def setup_signing(self, secret_key, sign_outgoing=True, allow_unsigned_callback=None, initial_timestamp=None, link_id=None):
00718         '''setup for MAVLink2 signing'''
00719         self.mav.signing.secret_key = secret_key
00720         self.mav.signing.sign_outgoing = sign_outgoing
00721         self.mav.signing.allow_unsigned_callback = allow_unsigned_callback
00722         if link_id is None:
00723             # auto-increment the link_id for each link
00724             global global_link_id
00725             link_id = global_link_id
00726             global_link_id = min(global_link_id + 1, 255)
00727         self.mav.signing.link_id = link_id
00728         if initial_timestamp is None:
00729             # timestamp is time since 1/1/2015
00730             epoch_offset = 1420070400
00731             now = max(time.time(), epoch_offset)
00732             initial_timestamp = now - epoch_offset
00733             initial_timestamp = int(initial_timestamp * 100 * 1000)
00734         # initial_timestamp is in 10usec units
00735         self.mav.signing.timestamp = initial_timestamp
00736 
00737     def disable_signing(self):
00738         '''disable MAVLink2 signing'''
00739         self.mav.signing.secret_key = None
00740         self.mav.signing.sign_outgoing = False
00741         self.mav.signing.allow_unsigned_callback = None
00742         self.mav.signing.link_id = 0
00743         self.mav.signing.timestamp = 0
00744 
00745 def set_close_on_exec(fd):
00746     '''set the clone on exec flag on a file descriptor. Ignore exceptions'''
00747     try:
00748         import fcntl
00749         flags = fcntl.fcntl(fd, fcntl.F_GETFD)
00750         flags |= fcntl.FD_CLOEXEC
00751         fcntl.fcntl(fd, fcntl.F_SETFD, flags)
00752     except Exception:
00753         pass
00754 
00755 class mavserial(mavfile):
00756     '''a serial mavlink port'''
00757     def __init__(self, device, baud=115200, autoreconnect=False, source_system=255, use_native=default_native):
00758         import serial
00759         if ',' in device and not os.path.exists(device):
00760             device, baud = device.split(',')
00761         self.baud = baud
00762         self.device = device
00763         self.autoreconnect = autoreconnect
00764         # we rather strangely set the baudrate initially to 1200, then change to the desired
00765         # baudrate. This works around a kernel bug on some Linux kernels where the baudrate
00766         # is not set correctly
00767         self.port = serial.Serial(self.device, 1200, timeout=0,
00768                                   dsrdtr=False, rtscts=False, xonxoff=False)
00769         try:
00770             fd = self.port.fileno()
00771             set_close_on_exec(fd)
00772         except Exception:
00773             fd = None
00774         self.set_baudrate(self.baud)
00775         mavfile.__init__(self, fd, device, source_system=source_system, use_native=use_native)
00776         self.rtscts = False
00777 
00778     def set_rtscts(self, enable):
00779         '''enable/disable RTS/CTS if applicable'''
00780         self.port.setRtsCts(enable)
00781         self.rtscts = enable
00782 
00783     def set_baudrate(self, baudrate):
00784         '''set baudrate'''
00785         try:
00786             self.port.setBaudrate(baudrate)
00787         except Exception:
00788             # for pySerial 3.0, which doesn't have setBaudrate()
00789             self.port.baudrate = baudrate
00790     
00791     def close(self):
00792         self.port.close()
00793 
00794     def recv(self,n=None):
00795         if n is None:
00796             n = self.mav.bytes_needed()
00797         if self.fd is None:
00798             waiting = self.port.inWaiting()
00799             if waiting < n:
00800                 n = waiting
00801         ret = self.port.read(n)
00802         return ret
00803 
00804     def write(self, buf):
00805         try:
00806             if not isinstance(buf, str):
00807                 buf = str(buf)
00808             return self.port.write(buf)
00809         except Exception:
00810             if not self.portdead:
00811                 print("Device %s is dead" % self.device)
00812             self.portdead = True
00813             if self.autoreconnect:
00814                 self.reset()
00815             return -1
00816             
00817     def reset(self):
00818         import serial
00819         try:
00820             newport = serial.Serial(self.device, self.baud, timeout=0,
00821                                     dsrdtr=False, rtscts=False, xonxoff=False)
00822             self.port.close()
00823             self.port = newport
00824             print("Device %s reopened OK" % self.device)
00825             self.portdead = False
00826             try:
00827                 self.fd = self.port.fileno()
00828             except Exception:
00829                 self.fd = None
00830             if self.rtscts:
00831                 self.set_rtscts(self.rtscts)
00832             return True
00833         except Exception:
00834             return False
00835         
00836 
00837 class mavudp(mavfile):
00838     '''a UDP mavlink socket'''
00839     def __init__(self, device, input=True, broadcast=False, source_system=255, use_native=default_native):
00840         a = device.split(':')
00841         if len(a) != 2:
00842             print("UDP ports must be specified as host:port")
00843             sys.exit(1)
00844         self.port = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
00845         self.udp_server = input
00846         self.broadcast = False
00847         if input:
00848             self.port.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
00849             self.port.bind((a[0], int(a[1])))
00850         else:
00851             self.destination_addr = (a[0], int(a[1]))
00852             if broadcast:
00853                 self.port.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
00854                 self.broadcast = True
00855         set_close_on_exec(self.port.fileno())
00856         self.port.setblocking(0)
00857         self.last_address = None
00858         mavfile.__init__(self, self.port.fileno(), device, source_system=source_system, input=input, use_native=use_native)
00859 
00860     def close(self):
00861         self.port.close()
00862 
00863     def recv(self,n=None):
00864         try:
00865             data, new_addr = self.port.recvfrom(UDP_MAX_PACKET_LEN)
00866         except socket.error as e:
00867             if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK, errno.ECONNREFUSED ]:
00868                 return ""
00869             raise
00870         if self.udp_server or self.broadcast:
00871             self.last_address = new_addr
00872         return data
00873 
00874     def write(self, buf):
00875         try:
00876             if self.udp_server:
00877                 if self.last_address:
00878                     self.port.sendto(buf, self.last_address)
00879             else:
00880                 if self.last_address and self.broadcast:
00881                     self.destination_addr = self.last_address
00882                     self.broadcast = False
00883                     self.port.connect(self.destination_addr)
00884                 self.port.sendto(buf, self.destination_addr)
00885         except socket.error:
00886             pass
00887 
00888     def recv_msg(self):
00889         '''message receive routine for UDP link'''
00890         self.pre_message()
00891         s = self.recv()
00892         if len(s) > 0:
00893             if self.first_byte:
00894                 self.auto_mavlink_version(s)
00895 
00896         m = self.mav.parse_char(s)
00897         if m is not None:
00898             self.post_message(m)
00899 
00900         return m
00901 
00902 
00903 class mavtcp(mavfile):
00904     '''a TCP mavlink socket'''
00905     def __init__(self, device, source_system=255, retries=3, use_native=default_native):
00906         a = device.split(':')
00907         if len(a) != 2:
00908             print("TCP ports must be specified as host:port")
00909             sys.exit(1)
00910         self.port = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
00911         self.destination_addr = (a[0], int(a[1]))
00912         while retries >= 0:
00913             retries -= 1
00914             if retries <= 0:
00915                 self.port.connect(self.destination_addr)
00916             else:
00917                 try:
00918                     self.port.connect(self.destination_addr)
00919                     break
00920                 except Exception as e:
00921                     if retries > 0:
00922                         print(e, "sleeping")
00923                         time.sleep(1)
00924                     continue
00925         self.port.setblocking(0)
00926         set_close_on_exec(self.port.fileno())
00927         self.port.setsockopt(socket.SOL_TCP, socket.TCP_NODELAY, 1)
00928         mavfile.__init__(self, self.port.fileno(), "tcp:" + device, source_system=source_system, use_native=use_native)
00929 
00930     def close(self):
00931         self.port.close()
00932 
00933     def recv(self,n=None):
00934         if n is None:
00935             n = self.mav.bytes_needed()
00936         try:
00937             data = self.port.recv(n)
00938         except socket.error as e:
00939             if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
00940                 return ""
00941             raise
00942         return data
00943 
00944     def write(self, buf):
00945         try:
00946             self.port.send(buf)
00947         except socket.error:
00948             pass
00949 
00950 
00951 class mavtcpin(mavfile):
00952     '''a TCP input mavlink socket'''
00953     def __init__(self, device, source_system=255, retries=3, use_native=default_native):
00954         a = device.split(':')
00955         if len(a) != 2:
00956             print("TCP ports must be specified as host:port")
00957             sys.exit(1)
00958         self.listen = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
00959         self.listen_addr = (a[0], int(a[1]))
00960         self.listen.bind(self.listen_addr)
00961         self.listen.listen(1)
00962         self.listen.setblocking(0)
00963         set_close_on_exec(self.listen.fileno())
00964         self.listen.setsockopt(socket.SOL_TCP, socket.TCP_NODELAY, 1)
00965         mavfile.__init__(self, self.listen.fileno(), "tcpin:" + device, source_system=source_system, use_native=use_native)
00966         self.port = None
00967 
00968     def close(self):
00969         self.listen.close()
00970 
00971     def recv(self,n=None):
00972         if not self.port:
00973             try:
00974                 (self.port, addr) = self.listen.accept()
00975             except Exception:
00976                 return ''
00977             self.port.setsockopt(socket.SOL_TCP, socket.TCP_NODELAY, 1) 
00978             self.port.setblocking(0) 
00979             set_close_on_exec(self.port.fileno())
00980             self.fd = self.port.fileno()
00981 
00982         if n is None:
00983             n = self.mav.bytes_needed()
00984         try:
00985             data = self.port.recv(n)
00986         except socket.error as e:
00987             if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
00988                 return ""
00989             self.port.close()
00990             self.port = None
00991             self.fd = self.listen.fileno()
00992             return ''
00993         return data
00994 
00995     def write(self, buf):
00996         if self.port is None:
00997             return
00998         try:
00999             self.port.send(buf)
01000         except socket.error as e:
01001             if e.errno in [ errno.EPIPE ]:
01002                 self.port.close()
01003                 self.port = None
01004                 self.fd = self.listen.fileno()
01005             pass
01006 
01007 
01008 class mavlogfile(mavfile):
01009     '''a MAVLink logfile reader/writer'''
01010     def __init__(self, filename, planner_format=None,
01011                  write=False, append=False,
01012                  robust_parsing=True, notimestamps=False, source_system=255, use_native=default_native):
01013         self.filename = filename
01014         self.writeable = write
01015         self.robust_parsing = robust_parsing
01016         self.planner_format = planner_format
01017         self._two64 = math.pow(2.0, 63)
01018         mode = 'rb'
01019         if self.writeable:
01020             if append:
01021                 mode = 'ab'
01022             else:
01023                 mode = 'wb'
01024         self.f = open(filename, mode)
01025         self.filesize = os.path.getsize(filename)
01026         self.percent = 0
01027         mavfile.__init__(self, None, filename, source_system=source_system, notimestamps=notimestamps, use_native=use_native)
01028         if self.notimestamps:
01029             self._timestamp = 0
01030         else:
01031             self._timestamp = time.time()
01032         self.stop_on_EOF = True
01033         self._last_message = None
01034         self._last_timestamp = None
01035 
01036     def close(self):
01037         self.f.close()
01038 
01039     def recv(self,n=None):
01040         if n is None:
01041             n = self.mav.bytes_needed()
01042         return self.f.read(n)
01043 
01044     def write(self, buf):
01045         self.f.write(buf)
01046 
01047     def scan_timestamp(self, tbuf):
01048         '''scan forward looking in a tlog for a timestamp in a reasonable range'''
01049         while True:
01050             (tusec,) = struct.unpack('>Q', tbuf)
01051             t = tusec * 1.0e-6
01052             if abs(t - self._last_timestamp) <= 3*24*60*60:
01053                 break
01054             c = self.f.read(1)
01055             if len(c) != 1:
01056                 break
01057             tbuf = tbuf[1:] + c
01058         return t
01059 
01060 
01061     def pre_message(self):
01062         '''read timestamp if needed'''
01063         # read the timestamp
01064         if self.filesize != 0:
01065             self.percent = (100.0 * self.f.tell()) / self.filesize
01066         if self.notimestamps:
01067             return
01068         if self.planner_format:
01069             tbuf = self.f.read(21)
01070             if len(tbuf) != 21 or tbuf[0] != '-' or tbuf[20] != ':':
01071                 raise RuntimeError('bad planner timestamp %s' % tbuf)
01072             hnsec = self._two64 + float(tbuf[0:20])
01073             t = hnsec * 1.0e-7         # convert to seconds
01074             t -= 719163 * 24 * 60 * 60 # convert to 1970 base
01075             self._link = 0
01076         else:
01077             tbuf = self.f.read(8)
01078             if len(tbuf) != 8:
01079                 return
01080             (tusec,) = struct.unpack('>Q', tbuf)
01081             t = tusec * 1.0e-6
01082             if (self._last_timestamp is not None and
01083                 self._last_message.get_type() == "BAD_DATA" and
01084                 abs(t - self._last_timestamp) > 3*24*60*60):
01085                 t = self.scan_timestamp(tbuf)
01086             self._link = tusec & 0x3
01087         self._timestamp = t
01088 
01089     def post_message(self, msg):
01090         '''add timestamp to message'''
01091         # read the timestamp
01092         super(mavlogfile, self).post_message(msg)
01093         if self.planner_format:
01094             self.f.read(1) # trailing newline
01095         self.timestamp = msg._timestamp
01096         self._last_message = msg
01097         if msg.get_type() != "BAD_DATA":
01098             self._last_timestamp = msg._timestamp
01099 
01100 
01101 class mavmemlog(mavfile):
01102     '''a MAVLink log in memory. This allows loading a log into
01103     memory to make it easier to do multiple sweeps over a log'''
01104     def __init__(self, mav):
01105         mavfile.__init__(self, None, 'memlog')
01106         self._msgs = []
01107         self._index = 0
01108         self._count = 0
01109         self.messages = {}
01110         while True:
01111             m = mav.recv_msg()
01112             if m is None:
01113                 break
01114             self._msgs.append(m)
01115         self._count = len(self._msgs)
01116 
01117     def recv_msg(self):
01118         '''message receive routine'''
01119         if self._index >= self._count:
01120             return None
01121         m = self._msgs[self._index]
01122         self._index += 1
01123         self.percent = (100.0 * self._index) / self._count
01124         self.messages[m.get_type()] = m
01125         return m
01126 
01127     def rewind(self):
01128         '''rewind to start'''
01129         self._index = 0
01130         self.percent = 0
01131         self.messages = {}
01132 
01133 class mavchildexec(mavfile):
01134     '''a MAVLink child processes reader/writer'''
01135     def __init__(self, filename, source_system=255, use_native=default_native):
01136         from subprocess import Popen, PIPE
01137         import fcntl
01138         
01139         self.filename = filename
01140         self.child = Popen(filename, shell=False, stdout=PIPE, stdin=PIPE, bufsize=0)
01141         self.fd = self.child.stdout.fileno()
01142 
01143         fl = fcntl.fcntl(self.fd, fcntl.F_GETFL)
01144         fcntl.fcntl(self.fd, fcntl.F_SETFL, fl | os.O_NONBLOCK)
01145 
01146         fl = fcntl.fcntl(self.child.stdout.fileno(), fcntl.F_GETFL)
01147         fcntl.fcntl(self.child.stdout.fileno(), fcntl.F_SETFL, fl | os.O_NONBLOCK)
01148 
01149         mavfile.__init__(self, self.fd, filename, source_system=source_system, use_native=use_native)
01150 
01151     def close(self):
01152         self.child.close()
01153 
01154     def recv(self,n=None):
01155         try:
01156             x = self.child.stdout.read(1)
01157         except Exception:
01158             return ''
01159         return x
01160 
01161     def write(self, buf):
01162         self.child.stdin.write(buf)
01163 
01164 
01165 def mavlink_connection(device, baud=115200, source_system=255,
01166                        planner_format=None, write=False, append=False,
01167                        robust_parsing=True, notimestamps=False, input=True,
01168                        dialect=None, autoreconnect=False, zero_time_base=False,
01169                        retries=3, use_native=default_native):
01170     '''open a serial, UDP, TCP or file mavlink connection'''
01171     global mavfile_global
01172 
01173     if dialect is not None:
01174         set_dialect(dialect)
01175     if device.startswith('tcp:'):
01176         return mavtcp(device[4:], source_system=source_system, retries=retries, use_native=use_native)
01177     if device.startswith('tcpin:'):
01178         return mavtcpin(device[6:], source_system=source_system, retries=retries, use_native=use_native)
01179     if device.startswith('udpin:'):
01180         return mavudp(device[6:], input=True, source_system=source_system, use_native=use_native)
01181     if device.startswith('udpout:'):
01182         return mavudp(device[7:], input=False, source_system=source_system, use_native=use_native)
01183     if device.startswith('udpbcast:'):
01184         return mavudp(device[9:], input=False, source_system=source_system, use_native=use_native, broadcast=True)
01185     # For legacy purposes we accept the following syntax and let the caller to specify direction
01186     if device.startswith('udp:'):
01187         return mavudp(device[4:], input=input, source_system=source_system, use_native=use_native)
01188 
01189     if device.lower().endswith('.bin') or device.lower().endswith('.px4log'):
01190         # support dataflash logs
01191         from pymavlink import DFReader
01192         m = DFReader.DFReader_binary(device, zero_time_base=zero_time_base)
01193         mavfile_global = m
01194         return m
01195 
01196     if device.endswith('.log'):
01197         # support dataflash text logs
01198         from pymavlink import DFReader
01199         if DFReader.DFReader_is_text_log(device):
01200             m = DFReader.DFReader_text(device, zero_time_base=zero_time_base)
01201             mavfile_global = m
01202             return m    
01203 
01204     # list of suffixes to prevent setting DOS paths as UDP sockets
01205     logsuffixes = ['mavlink', 'log', 'raw', 'tlog' ]
01206     suffix = device.split('.')[-1].lower()
01207     if device.find(':') != -1 and not suffix in logsuffixes:
01208         return mavudp(device, source_system=source_system, input=input, use_native=use_native)
01209     if os.path.isfile(device):
01210         if device.endswith(".elf") or device.find("/bin/") != -1:
01211             print("executing '%s'" % device)
01212             return mavchildexec(device, source_system=source_system, use_native=use_native)
01213         else:
01214             return mavlogfile(device, planner_format=planner_format, write=write,
01215                               append=append, robust_parsing=robust_parsing, notimestamps=notimestamps,
01216                               source_system=source_system, use_native=use_native)
01217     return mavserial(device, baud=baud, source_system=source_system, autoreconnect=autoreconnect, use_native=use_native)
01218 
01219 class periodic_event(object):
01220     '''a class for fixed frequency events'''
01221     def __init__(self, frequency):
01222         self.frequency = float(frequency)
01223         self.last_time = time.time()
01224 
01225     def force(self):
01226         '''force immediate triggering'''
01227         self.last_time = 0
01228         
01229     def trigger(self):
01230         '''return True if we should trigger now'''
01231         tnow = time.time()
01232 
01233         if tnow < self.last_time:
01234             print("Warning, time moved backwards. Restarting timer.")
01235             self.last_time = tnow
01236 
01237         if self.last_time + (1.0/self.frequency) <= tnow:
01238             self.last_time = tnow
01239             return True
01240         return False
01241 
01242 
01243 try:
01244     from curses import ascii
01245     have_ascii = True
01246 except:
01247     have_ascii = False
01248 
01249 def is_printable(c):
01250     '''see if a character is printable'''
01251     global have_ascii
01252     if have_ascii:
01253         return ascii.isprint(c)
01254     if isinstance(c, int):
01255         ic = c
01256     else:
01257         ic = ord(c)
01258     return ic >= 32 and ic <= 126
01259 
01260 def all_printable(buf):
01261     '''see if a string is all printable'''
01262     for c in buf:
01263         if not is_printable(c) and not c in ['\r', '\n', '\t']:
01264             return False
01265     return True
01266 
01267 class SerialPort(object):
01268     '''auto-detected serial port'''
01269     def __init__(self, device, description=None, hwid=None):
01270         self.device = device
01271         self.description = description
01272         self.hwid = hwid
01273 
01274     def __str__(self):
01275         ret = self.device
01276         if self.description is not None:
01277             ret += " : " + self.description
01278         if self.hwid is not None:
01279             ret += " : " + self.hwid
01280         return ret
01281 
01282 def auto_detect_serial_win32(preferred_list=['*']):
01283     '''try to auto-detect serial ports on win32'''
01284     try:
01285         from serial.tools.list_ports_windows import comports
01286         list = sorted(comports())
01287     except:
01288         return []
01289     ret = []
01290     others = []
01291     for port, description, hwid in list:
01292         matches = False
01293         p = SerialPort(port, description=description, hwid=hwid)
01294         for preferred in preferred_list:
01295             if fnmatch.fnmatch(description, preferred) or fnmatch.fnmatch(hwid, preferred):
01296                 matches = True
01297         if matches:
01298             ret.append(p)
01299         else:
01300             others.append(p)
01301     if len(ret) > 0:
01302         return ret
01303     # now the rest
01304     ret.extend(others)
01305     return ret
01306         
01307 
01308         
01309 
01310 def auto_detect_serial_unix(preferred_list=['*']):
01311     '''try to auto-detect serial ports on unix'''
01312     import glob
01313     glist = glob.glob('/dev/ttyS*') + glob.glob('/dev/ttyUSB*') + glob.glob('/dev/ttyACM*') + glob.glob('/dev/serial/by-id/*')
01314     ret = []
01315     others = []
01316     # try preferred ones first
01317     for d in glist:
01318         matches = False
01319         for preferred in preferred_list:
01320             if fnmatch.fnmatch(d, preferred):
01321                 matches = True
01322         if matches:
01323             ret.append(SerialPort(d))
01324         else:
01325             others.append(SerialPort(d))
01326     if len(ret) > 0:
01327         return ret
01328     ret.extend(others)
01329     return ret
01330 
01331 def auto_detect_serial(preferred_list=['*']):
01332     '''try to auto-detect serial port'''
01333     # see if 
01334     if os.name == 'nt':
01335         return auto_detect_serial_win32(preferred_list=preferred_list)
01336     return auto_detect_serial_unix(preferred_list=preferred_list)
01337 
01338 def mode_string_v09(msg):
01339     '''mode string for 0.9 protocol'''
01340     mode = msg.mode
01341     nav_mode = msg.nav_mode
01342 
01343     MAV_MODE_UNINIT = 0
01344     MAV_MODE_MANUAL = 2
01345     MAV_MODE_GUIDED = 3
01346     MAV_MODE_AUTO = 4
01347     MAV_MODE_TEST1 = 5
01348     MAV_MODE_TEST2 = 6
01349     MAV_MODE_TEST3 = 7
01350 
01351     MAV_NAV_GROUNDED = 0
01352     MAV_NAV_LIFTOFF = 1
01353     MAV_NAV_HOLD = 2
01354     MAV_NAV_WAYPOINT = 3
01355     MAV_NAV_VECTOR = 4
01356     MAV_NAV_RETURNING = 5
01357     MAV_NAV_LANDING = 6
01358     MAV_NAV_LOST = 7
01359     MAV_NAV_LOITER = 8
01360     
01361     cmode = (mode, nav_mode)
01362     mapping = {
01363         (MAV_MODE_UNINIT, MAV_NAV_GROUNDED)  : "INITIALISING",
01364         (MAV_MODE_MANUAL, MAV_NAV_VECTOR)    : "MANUAL",
01365         (MAV_MODE_TEST3,  MAV_NAV_VECTOR)    : "CIRCLE",
01366         (MAV_MODE_GUIDED, MAV_NAV_VECTOR)    : "GUIDED",
01367         (MAV_MODE_TEST1,  MAV_NAV_VECTOR)    : "STABILIZE",
01368         (MAV_MODE_TEST2,  MAV_NAV_LIFTOFF)   : "FBWA",
01369         (MAV_MODE_AUTO,   MAV_NAV_WAYPOINT)  : "AUTO",
01370         (MAV_MODE_AUTO,   MAV_NAV_RETURNING) : "RTL",
01371         (MAV_MODE_AUTO,   MAV_NAV_LOITER)    : "LOITER",
01372         (MAV_MODE_AUTO,   MAV_NAV_LIFTOFF)   : "TAKEOFF",
01373         (MAV_MODE_AUTO,   MAV_NAV_LANDING)   : "LANDING",
01374         (MAV_MODE_AUTO,   MAV_NAV_HOLD)      : "LOITER",
01375         (MAV_MODE_GUIDED, MAV_NAV_VECTOR)    : "GUIDED",
01376         (MAV_MODE_GUIDED, MAV_NAV_WAYPOINT)  : "GUIDED",
01377         (100,             MAV_NAV_VECTOR)    : "STABILIZE",
01378         (101,             MAV_NAV_VECTOR)    : "ACRO",
01379         (102,             MAV_NAV_VECTOR)    : "ALT_HOLD",
01380         (107,             MAV_NAV_VECTOR)    : "CIRCLE",
01381         (109,             MAV_NAV_VECTOR)    : "LAND",
01382         }
01383     if cmode in mapping:
01384         return mapping[cmode]
01385     return "Mode(%s,%s)" % cmode
01386 
01387 mode_mapping_apm = {
01388     0 : 'MANUAL',
01389     1 : 'CIRCLE',
01390     2 : 'STABILIZE',
01391     3 : 'TRAINING',
01392     4 : 'ACRO',
01393     5 : 'FBWA',
01394     6 : 'FBWB',
01395     7 : 'CRUISE',
01396     8 : 'AUTOTUNE',
01397     10 : 'AUTO',
01398     11 : 'RTL',
01399     12 : 'LOITER',
01400     14 : 'LAND',
01401     15 : 'GUIDED',
01402     16 : 'INITIALISING',
01403     17 : 'QSTABILIZE',
01404     18 : 'QHOVER',
01405     19 : 'QLOITER',
01406     20 : 'QLAND',
01407     21 : 'QRTL',
01408     }
01409 mode_mapping_acm = {
01410     0 : 'STABILIZE',
01411     1 : 'ACRO',
01412     2 : 'ALT_HOLD',
01413     3 : 'AUTO',
01414     4 : 'GUIDED',
01415     5 : 'LOITER',
01416     6 : 'RTL',
01417     7 : 'CIRCLE',
01418     8 : 'POSITION',
01419     9 : 'LAND',
01420     10 : 'OF_LOITER',
01421     11 : 'DRIFT',
01422     13 : 'SPORT',
01423     14 : 'FLIP',
01424     15 : 'AUTOTUNE',
01425     16 : 'POSHOLD'
01426     }
01427 mode_mapping_rover = {
01428     0 : 'MANUAL',
01429     2 : 'LEARNING',
01430     3 : 'STEERING',
01431     4 : 'HOLD',
01432     10 : 'AUTO',
01433     11 : 'RTL',
01434     15 : 'GUIDED',
01435     16 : 'INITIALISING'
01436     }
01437 
01438 mode_mapping_tracker = {
01439     0 : 'MANUAL',
01440     1 : 'STOP',
01441     2 : 'SCAN',
01442     10 : 'AUTO',
01443     16 : 'INITIALISING'
01444     }
01445 
01446 mode_mapping_px4 = {
01447     0 : 'MANUAL',
01448     1 : 'ATTITUDE',
01449     2 : 'EASY',
01450     3 : 'AUTO'
01451     }
01452 
01453 
01454 def mode_mapping_byname(mav_type):
01455     '''return dictionary mapping mode names to numbers, or None if unknown'''
01456     map = None
01457     if mav_type in [mavlink.MAV_TYPE_QUADROTOR,
01458                     mavlink.MAV_TYPE_HELICOPTER,
01459                     mavlink.MAV_TYPE_HEXAROTOR,
01460                     mavlink.MAV_TYPE_OCTOROTOR,
01461                     mavlink.MAV_TYPE_TRICOPTER]:
01462         map = mode_mapping_acm
01463     if mav_type == mavlink.MAV_TYPE_FIXED_WING:
01464         map = mode_mapping_apm
01465     if mav_type == mavlink.MAV_TYPE_GROUND_ROVER:
01466         map = mode_mapping_rover
01467     if mav_type == mavlink.MAV_TYPE_ANTENNA_TRACKER:
01468         map = mode_mapping_tracker
01469     if map is None:
01470         return None
01471     inv_map = dict((a, b) for (b, a) in map.items())
01472     return inv_map
01473 
01474 def mode_mapping_bynumber(mav_type):
01475     '''return dictionary mapping mode numbers to name, or None if unknown'''
01476     map = None
01477     if mav_type in [mavlink.MAV_TYPE_QUADROTOR,
01478                     mavlink.MAV_TYPE_HELICOPTER,
01479                     mavlink.MAV_TYPE_HEXAROTOR,
01480                     mavlink.MAV_TYPE_OCTOROTOR,
01481                     mavlink.MAV_TYPE_TRICOPTER]:
01482         map = mode_mapping_acm
01483     if mav_type == mavlink.MAV_TYPE_FIXED_WING:
01484         map = mode_mapping_apm
01485     if mav_type == mavlink.MAV_TYPE_GROUND_ROVER:
01486         map = mode_mapping_rover
01487     if mav_type == mavlink.MAV_TYPE_ANTENNA_TRACKER:
01488         map = mode_mapping_tracker
01489     if map is None:
01490         return None
01491     return map
01492 
01493 
01494 def mode_string_v10(msg):
01495     '''mode string for 1.0 protocol, from heartbeat'''
01496     if not msg.base_mode & mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED:
01497         return "Mode(0x%08x)" % msg.base_mode
01498     if msg.type in [ mavlink.MAV_TYPE_QUADROTOR, mavlink.MAV_TYPE_HEXAROTOR,
01499                      mavlink.MAV_TYPE_OCTOROTOR, mavlink.MAV_TYPE_TRICOPTER,
01500                      mavlink.MAV_TYPE_COAXIAL,
01501                      mavlink.MAV_TYPE_HELICOPTER ]:
01502         if msg.custom_mode in mode_mapping_acm:
01503             return mode_mapping_acm[msg.custom_mode]
01504     if msg.type == mavlink.MAV_TYPE_FIXED_WING:
01505         if msg.custom_mode in mode_mapping_apm:
01506             return mode_mapping_apm[msg.custom_mode]
01507     if msg.type == mavlink.MAV_TYPE_GROUND_ROVER:
01508         if msg.custom_mode in mode_mapping_rover:
01509             return mode_mapping_rover[msg.custom_mode]
01510     if msg.type == mavlink.MAV_TYPE_ANTENNA_TRACKER:
01511         if msg.custom_mode in mode_mapping_tracker:
01512             return mode_mapping_tracker[msg.custom_mode]
01513     return "Mode(%u)" % msg.custom_mode
01514 
01515 def mode_string_apm(mode_number):
01516     '''return mode string for APM:Plane'''
01517     if mode_number in mode_mapping_apm:
01518         return mode_mapping_apm[mode_number]
01519     return "Mode(%u)" % mode_number
01520 
01521 def mode_string_acm(mode_number):
01522     '''return mode string for APM:Copter'''
01523     if mode_number in mode_mapping_acm:
01524         return mode_mapping_acm[mode_number]
01525     return "Mode(%u)" % mode_number
01526 
01527 def mode_string_px4(mode_number):
01528     '''return mode string for PX4 flight stack'''
01529     if mode_number in mode_mapping_px4:
01530         return mode_mapping_px4[mode_number]
01531     return "Mode(%u)" % mode_number
01532 
01533 class x25crc(object):
01534     '''x25 CRC - based on checksum.h from mavlink library'''
01535     def __init__(self, buf=''):
01536         self.crc = 0xffff
01537         self.accumulate(buf)
01538 
01539     def accumulate(self, buf):
01540         '''add in some more bytes'''
01541         bytes = array.array('B')
01542         if isinstance(buf, array.array):
01543             bytes.extend(buf)
01544         else:
01545             bytes.fromstring(buf)
01546         accum = self.crc
01547         for b in bytes:
01548             tmp = b ^ (accum & 0xff)
01549             tmp = (tmp ^ (tmp<<4)) & 0xFF
01550             accum = (accum>>8) ^ (tmp<<8) ^ (tmp<<3) ^ (tmp>>4)
01551             accum = accum & 0xFFFF
01552         self.crc = accum
01553 
01554 class MavlinkSerialPort():
01555         '''an object that looks like a serial port, but
01556         transmits using mavlink SERIAL_CONTROL packets'''
01557         def __init__(self, portname, baudrate, devnum=0, devbaud=0, timeout=3, debug=0):
01558                 from . import mavutil
01559 
01560                 self.baudrate = 0
01561                 self.timeout = timeout
01562                 self._debug = debug
01563                 self.buf = ''
01564                 self.port = devnum
01565                 self.debug("Connecting with MAVLink to %s ..." % portname)
01566                 self.mav = mavutil.mavlink_connection(portname, autoreconnect=True, baud=baudrate)
01567                 self.mav.wait_heartbeat()
01568                 self.debug("HEARTBEAT OK\n")
01569                 if devbaud != 0:
01570                     self.setBaudrate(devbaud)
01571                 self.debug("Locked serial device\n")
01572 
01573         def debug(self, s, level=1):
01574                 '''write some debug text'''
01575                 if self._debug >= level:
01576                         print(s)
01577 
01578         def write(self, b):
01579                 '''write some bytes'''
01580                 from . import mavutil
01581                 self.debug("sending '%s' (0x%02x) of len %u\n" % (b, ord(b[0]), len(b)), 2)
01582                 while len(b) > 0:
01583                         n = len(b)
01584                         if n > 70:
01585                                 n = 70
01586                         buf = [ord(x) for x in b[:n]]
01587                         buf.extend([0]*(70-len(buf)))
01588                         self.mav.mav.serial_control_send(self.port,
01589                                                          mavutil.mavlink.SERIAL_CONTROL_FLAG_EXCLUSIVE |
01590                                                          mavutil.mavlink.SERIAL_CONTROL_FLAG_RESPOND,
01591                                                          0,
01592                                                          0,
01593                                                          n,
01594                                                          buf)
01595                         b = b[n:]
01596 
01597         def _recv(self):
01598                 '''read some bytes into self.buf'''
01599                 from . import mavutil
01600                 start_time = time.time()
01601                 while time.time() < start_time + self.timeout:
01602                         m = self.mav.recv_match(condition='SERIAL_CONTROL.count!=0',
01603                                                 type='SERIAL_CONTROL', blocking=False, timeout=0)
01604                         if m is not None and m.count != 0:
01605                                 break
01606                         self.mav.mav.serial_control_send(self.port,
01607                                                          mavutil.mavlink.SERIAL_CONTROL_FLAG_EXCLUSIVE |
01608                                                          mavutil.mavlink.SERIAL_CONTROL_FLAG_RESPOND,
01609                                                          0,
01610                                                          0,
01611                                                          0, [0]*70)
01612                         m = self.mav.recv_match(condition='SERIAL_CONTROL.count!=0',
01613                                                 type='SERIAL_CONTROL', blocking=True, timeout=0.01)
01614                         if m is not None and m.count != 0:
01615                                 break
01616                 if m is not None:
01617                         if self._debug > 2:
01618                                 print(m)
01619                         data = m.data[:m.count]
01620                         self.buf += ''.join(str(chr(x)) for x in data)
01621 
01622         def read(self, n):
01623                 '''read some bytes'''
01624                 if len(self.buf) == 0:
01625                         self._recv()
01626                 if len(self.buf) > 0:
01627                         if n > len(self.buf):
01628                                 n = len(self.buf)
01629                         ret = self.buf[:n]
01630                         self.buf = self.buf[n:]
01631                         if self._debug >= 2:
01632                             for b in ret:
01633                                 self.debug("read 0x%x" % ord(b), 2)
01634                         return ret
01635                 return ''
01636 
01637         def flushInput(self):
01638                 '''flush any pending input'''
01639                 self.buf = ''
01640                 saved_timeout = self.timeout
01641                 self.timeout = 0.5
01642                 self._recv()
01643                 self.timeout = saved_timeout
01644                 self.buf = ''
01645                 self.debug("flushInput")
01646 
01647         def setBaudrate(self, baudrate):
01648                 '''set baudrate'''
01649                 from . import mavutil
01650                 if self.baudrate == baudrate:
01651                         return
01652                 self.baudrate = baudrate
01653                 self.mav.mav.serial_control_send(self.port,
01654                                                  mavutil.mavlink.SERIAL_CONTROL_FLAG_EXCLUSIVE,
01655                                                  0,
01656                                                  self.baudrate,
01657                                                  0, [0]*70)
01658                 self.flushInput()
01659                 self.debug("Changed baudrate %u" % self.baudrate)
01660 
01661 if __name__ == '__main__':
01662         serial_list = auto_detect_serial(preferred_list=['*FTDI*',"*Arduino_Mega_2560*", "*3D_Robotics*", "*USB_to_UART*", '*PX4*', '*FMU*'])
01663         for port in serial_list:
01664             print("%s" % port)


mavlink
Author(s): Lorenz Meier
autogenerated on Thu Jun 6 2019 19:01:57