def add_data(t, msg, vars, flightmode): '''add some data''' mtype = msg.get_type() if args.flightmode is not None and (len(modes) == 0 or modes[-1][1] != flightmode): modes.append((t, flightmode)) if mtype not in msg_types: return for i in range(0, len(fields)): if mtype not in field_types[i]: continue f = fields[i] if f.endswith(":2"): axes[i] = 2 f = f[:-2] if f.endswith(":1"): first_only[i] = True f = f[:-2] v = mavutil.evaluate_expression(f, vars) if v is None: continue if args.xaxis is None: xv = t else: xv = mavutil.evaluate_expression(args.xaxis, vars) if xv is None: continue y[i].append(v) x[i].append(xv)
def expression_ok(expression, msgs=None): '''return True if an expression is OK with current messages''' expression_ok = True if expression is None: return False fields = expression.split() if msgs is None: msgs = mestate.status.msgs for f in fields: try: if f.endswith(">"): a2 = f.rfind("<") if a2 != -1: f = f[:a2] if f.endswith(':2'): f = f[:-2] if f[-1] == '}': # avoid passing nocondition unless needed to allow us to work witih older # pymavlink versions res = mavutil.evaluate_expression(f, msgs, nocondition=True) else: res = mavutil.evaluate_expression(f, msgs) if res is None: expression_ok = False except Exception: expression_ok = False break return expression_ok
def add_data(t, msg, vars): '''add some data''' mtype = msg.get_type() if mtype not in msg_types: return for i in range(0, len(fields)): if mtype not in field_types[i]: continue f = fields[i] if f.endswith(":2"): axes[i] = 2 f = f[:-2] if f.endswith(":1"): first_only[i] = True f = f[:-2] v = mavutil.evaluate_expression(f, vars) if v is None: continue if opts.xaxis is None: xv = t else: xv = mavutil.evaluate_expression(opts.xaxis, vars) if xv is None: continue y[i].append(v) x[i].append(xv)
def add_data(self, t, msg, vars): '''add some data''' mtype = msg.get_type() for i in range(0, len(self.fields)): if mtype not in self.field_types[i]: continue f = self.fields[i] simple = self.simple_field[i] if simple is not None: v = getattr(vars[simple[0]], simple[1]) else: v = mavutil.evaluate_expression(f, vars) if v is None: continue if self.xaxis is None: xv = t else: xv = mavutil.evaluate_expression(self.xaxis, vars) if xv is None: continue try: v_f = float(v) xv_f = float(xv) except Exception: continue self.y[i].append(v_f) self.x[i].append(xv_f)
def add_data(self, t, msg, vars, flightmode): '''add some data''' mtype = msg.get_type() if self.show_flightmode and (len(self.modes) == 0 or self.modes[-1][1] != flightmode): self.modes.append((t, flightmode)) for i in range(0, len(self.fields)): if mtype not in self.field_types[i]: continue f = self.fields[i] if f.endswith(":2"): self.axes[i] = 2 f = f[:-2] if f.endswith(":1"): self.first_only[i] = True f = f[:-2] v = mavutil.evaluate_expression(f, vars) if v is None: continue if self.xaxis is None: xv = t else: xv = mavutil.evaluate_expression(self.xaxis, vars) if xv is None: continue self.y[i].append(v) self.x[i].append(xv)
def add_data(self, t, msg, vars): '''add some data''' mtype = msg.get_type() for i in range(0, len(self.fields)): if mtype not in self.field_types[i]: continue if self.instance_types[i] and hasattr(msg, 'fmt') and hasattr( msg.fmt, 'instance_field') and msg.fmt.instance_field is not None: mtype_instance = '%s[%s]' % ( mtype, getattr(msg, msg.fmt.instance_field)) if mtype_instance not in self.instance_types[i]: continue f = self.fields[i] simple = self.simple_field[i] if simple is not None: v = getattr(vars[simple[0]], simple[1]) else: v = mavutil.evaluate_expression(f, vars) if v is None: continue if self.xaxis is None: xv = t else: xv = mavutil.evaluate_expression(self.xaxis, vars) if xv is None: continue self.y[i].append(v) self.x[i].append(xv)
def add_data(self, t, msg, vars, flightmode): """add some data""" mtype = msg.get_type() if self.show_flightmode and (len(self.modes) == 0 or self.modes[-1][1] != flightmode): self.modes.append((t, flightmode)) for i in range(0, len(self.fields)): if mtype not in self.field_types[i]: continue f = self.fields[i] if f.endswith(":2"): self.axes[i] = 2 f = f[:-2] if f.endswith(":1"): self.first_only[i] = True f = f[:-2] v = mavutil.evaluate_expression(f, vars) if v is None: continue if self.xaxis is None: xv = t else: xv = mavutil.evaluate_expression(self.xaxis, vars) if xv is None: continue self.y[i].append(v) self.x[i].append(xv)
def process_messages(self, m): """ Process all incoming mavlink packets from the corresponding vehicle, and forward traffic to onboard components as necessary. """ mtype = m.get_type() #s = " MAV " + str(self.system_id) + ": From (" + str(m.get_header().srcSystem) + ", " + str(m.get_header().srcComponent) + ")" #s = s + ", To (" + m.tar #print(" MAV ") + str(self.system_id) + ": processing message from " + str(m.get_header().srcSystem) + ", " + str(m.get_header().srcComponent) if mtype == "HEARTBEAT": if self.param_fetched == False: self.param_fetched = True self.fetch_all_parameters() # DEBUG: temporary code block for debugging if mtype == "VFR_HUD": self.altitude = mavutil.evaluate_expression( "VFR_HUD.alt", self.master.messages) self.airspeed = mavutil.evaluate_expression( "VFR_HUD.airspeed", self.master.messages) self.heading = mavutil.evaluate_expression("VFR_HUD.heading", self.master.messages) #print(str(m.get_header().srcSystem) + ": Altitude: " + str(self.altitude)) if mtype == 'PARAM_VALUE': param_id = "%.16s" % m.param_id # Note: the xml specifies param_index is a uint16, so -1 in that field will show as 65535 # We accept both -1 and 65535 as 'unknown index' to future proof us against someday having that # xml fixed. if m.param_index != -1 and m.param_index != 65535 and m.param_index not in self.mav_param_set: added_new_parameter = True self.mav_param_set.add(m.param_index) else: added_new_parameter = False if m.param_count != -1: self.mav_param_count = m.param_count self.mav_param[str(param_id)] = m.param_value if self.fetch_one > 0: self.fetch_one -= 1 print("%s = %f" % (param_id, m.param_value)) if added_new_parameter and len( self.mav_param_set) == m.param_count: print("Received %u parameters" % m.param_count) #if self.logdir != None: # self.mav_param.save(os.path.join(self.logdir, self.parm_file), '*', verbose=True) for func in self.on_params_initialized: func()
def process_messages(self, m): """ Process all incoming mavlink packets from the corresponding vehicle, and forward traffic to onboard components as necessary. """ mtype = m.get_type() #s = " MAV " + str(self.system_id) + ": From (" + str(m.get_header().srcSystem) + ", " + str(m.get_header().srcComponent) + ")" #s = s + ", To (" + m.tar #print(" MAV ") + str(self.system_id) + ": processing message from " + str(m.get_header().srcSystem) + ", " + str(m.get_header().srcComponent) if mtype == "HEARTBEAT": if self.param_fetched == False: self.param_fetched = True self.fetch_all_parameters() # DEBUG: temporary code block for debugging if mtype == "VFR_HUD": self.altitude = mavutil.evaluate_expression("VFR_HUD.alt", self.master.messages) self.airspeed = mavutil.evaluate_expression("VFR_HUD.airspeed", self.master.messages) self.heading = mavutil.evaluate_expression("VFR_HUD.heading", self.master.messages) #print(str(m.get_header().srcSystem) + ": Altitude: " + str(self.altitude)) if mtype == 'PARAM_VALUE': param_id = "%.16s" % m.param_id # Note: the xml specifies param_index is a uint16, so -1 in that field will show as 65535 # We accept both -1 and 65535 as 'unknown index' to future proof us against someday having that # xml fixed. if m.param_index != -1 and m.param_index != 65535 and m.param_index not in self.mav_param_set: added_new_parameter = True self.mav_param_set.add(m.param_index) else: added_new_parameter = False if m.param_count != -1: self.mav_param_count = m.param_count self.mav_param[str(param_id)] = m.param_value if self.fetch_one > 0: self.fetch_one -= 1 print("%s = %f" % (param_id, m.param_value)) if added_new_parameter and len(self.mav_param_set) == m.param_count: print("Received %u parameters" % m.param_count) #if self.logdir != None: # self.mav_param.save(os.path.join(self.logdir, self.parm_file), '*', verbose=True) for func in self.on_params_initialized: func()
def load_graph_xml(self, xml): '''load a graph from one xml string''' try: root = objectify.fromstring(xml) except Exception: return False if root.tag != 'graphs': return False if not hasattr(root, 'graph'): return False for g in root.graph: name = g.attrib['name'] if self.have_graph(name): continue expressions = [e.text for e in g.expression] for e in expressions: graph_ok = True fields = e.split() for f in fields: try: if f.endswith(':2'): f = f[:-2] if mavutil.evaluate_expression(f, self.mestate.status.msgs) is None: graph_ok = False except Exception: graph_ok = False break if graph_ok: self.mestate.graphs.append(GraphDefinition(name, e, g.description.text)) break return True
def add_data(t, msg, vars, fields, field_types): '''add some data''' mtype = msg.get_type() if mtype not in msg_types: return for i in range(0, len(fields)): if mtype not in field_types[i]: continue f = fields[i] v = mavutil.evaluate_expression(f, vars) if v is None: continue # Check if we have position or state information if f == mainstate_field: # Handle main state information if v != add_data.mainstate_current and add_data.mainstate_current >= 0: # add_data.mainstate_current >= 0 : avoid starting a new linestring when mainstate comes after the first position data in the log add_data.new_linestring = True add_data.mainstate_current = v else: # Handle position information add_data.position_data[ i] = v # make sure lon, lat, alt is saved in the correct order in position_data (the same as in position_field_types) #check if we have a full gps measurement gps_measurement_ready = True for v in add_data.position_data: if v == None: gps_measurement_ready = False if gps_measurement_ready: #if new line string is needed (because of state change): add previous linestring to kml_linestrings list, add a new linestring to the kml multigeometry and append to the new linestring #else: append to current linestring if add_data.new_linestring: if add_data.current_kml_linestring != None: kml_linestrings.append(add_data.current_kml_linestring) add_data.current_kml_linestring = kml.newlinestring( name=opts.source + ":" + str(add_data.mainstate_current), altitudemode='absolute') #set rendering options if opts.extrude: add_data.current_kml_linestring.extrude = 1 add_data.current_kml_linestring.style.linestyle.color = colors[max( [add_data.mainstate_current, 0])] add_data.new_linestring = False add_to_linestring(add_data.position_data, add_data.current_kml_linestring) #reset position_data add_data.position_data = [None for n in position_field_types]
def add_mavlink_packet(self, msg): '''add data to the graph''' mtype = msg.get_type() if mtype not in self.msg_types: return for i in range(len(self.fields)): if mtype not in self.field_types[i]: continue f = self.fields[i] self.values[i] = mavutil.evaluate_expression(f, self.state.master.messages) if self.livegraph is not None: self.livegraph.add_values(self.values)
def mavlink_packet(self, msg): '''add data to the graph''' mtype = msg.get_type() if mtype not in self.msg_types: return for i in range(len(self.fields)): if mtype not in self.field_types[i]: continue f = self.fields[i] self.values[i] = mavutil.evaluate_expression(f, mpstate.master().messages) if self.livegraph is not None: self.livegraph.add_values(self.values)
def add_data(self, t, msg, vars): '''add some data''' mtype = msg.get_type() for i in range(0, len(self.fields)): if mtype not in self.field_types[i]: continue f = self.fields[i] simple = self.simple_field[i] if simple is not None: v = getattr(vars[simple[0]], simple[1]) else: v = mavutil.evaluate_expression(f, vars) if v is None: continue if self.xaxis is None: xv = t else: xv = mavutil.evaluate_expression(self.xaxis, vars) if xv is None: continue self.y[i].append(v) self.x[i].append(xv)
def add_data(t, msg, vars, fields, field_types): '''add some data''' mtype = msg.get_type() if mtype not in msg_types: return for i in range(0, len(fields)): if mtype not in field_types[i]: continue f = fields[i] v = mavutil.evaluate_expression(f, vars) if v is None: continue # Check if we have position or state information if f == mainstate_field: # Handle main state information if v != add_data.mainstate_current and add_data.mainstate_current >= 0: # add_data.mainstate_current >= 0 : avoid starting a new linestring when mainstate comes after the first position data in the log add_data.new_linestring = True add_data.mainstate_current = v else: # Handle position information add_data.position_data[i] = v # make sure lon, lat, alt is saved in the correct order in position_data (the same as in position_field_types) #check if we have a full gps measurement gps_measurement_ready = True; for v in add_data.position_data: if v == None: gps_measurement_ready = False if gps_measurement_ready: #if new line string is needed (because of state change): add previous linestring to kml_linestrings list, add a new linestring to the kml multigeometry and append to the new linestring #else: append to current linestring if add_data.new_linestring: if add_data.current_kml_linestring != None: kml_linestrings.append(add_data.current_kml_linestring) add_data.current_kml_linestring = kml.newlinestring(name=opts.source + ":" + str(add_data.mainstate_current), altitudemode='absolute') #set rendering options if opts.extrude: add_data.current_kml_linestring.extrude = 1 add_data.current_kml_linestring.style.linestyle.color = colors[max([add_data.mainstate_current, 0])] add_data.new_linestring = False add_to_linestring(add_data.position_data, add_data.current_kml_linestring) #reset position_data add_data.position_data = [None for n in position_field_types]
def expression_ok(expression): '''return True if an expression is OK with current messages''' expression_ok = True fields = expression.split() for f in fields: try: if f.endswith(':2'): f = f[:-2] if mavutil.evaluate_expression(f, mestate.status.msgs) is None: expression_ok = False except Exception: expression_ok = False break return expression_ok
def complete_variable(text): '''complete a MAVLink variable or expression''' if text == '': return list(rline_mpstate.status.msgs.keys()) if text.endswith(":2"): suffix = ":2" text = text[:-2] else: suffix = '' try: if mavutil.evaluate_expression(text, rline_mpstate.status.msgs) is not None: return [text + suffix] except Exception as ex: pass try: m1 = re.match("^(.*?)([A-Z0-9][A-Z0-9_]*)[.]([A-Za-z0-9]*)$", text) except Exception as ex: return [] if m1 is not None: prefix = m1.group(1) mtype = m1.group(2) fname = m1.group(3) if mtype in rline_mpstate.status.msgs: ret = [] for f in rline_mpstate.status.msgs[mtype].get_fieldnames(): if f.startswith(fname): ret.append(prefix + mtype + '.' + f + suffix) return ret return [] try: m2 = re.match("^(.*?)([A-Z0-9][A-Z0-9_]*)$", text) except Exception as ex: return [] prefix = m2.group(1) mtype = m2.group(2) ret = [] for k in list(rline_mpstate.status.msgs.keys()): if k.startswith(mtype): ret.append(prefix + k + suffix) return ret
def complete_variable(text): '''complete a MAVLink variable or expression''' if text == '': return list(rline_mpstate.status.msgs.keys()) if text.endswith(":2"): suffix = ":2" text = text[:-2] else: suffix = '' try: if mavutil.evaluate_expression(text, rline_mpstate.status.msgs) is not None: return [text+suffix] except Exception as ex: pass try: m1 = re.match("^(.*?)([A-Z0-9][A-Z0-9_]*)[.]([A-Za-z0-9_]*)$", text) except Exception as ex: return [] if m1 is not None: prefix = m1.group(1) mtype = m1.group(2) fname = m1.group(3) if mtype in rline_mpstate.status.msgs: ret = [] for f in rline_mpstate.status.msgs[mtype].get_fieldnames(): if f.startswith(fname): ret.append(prefix + mtype + '.' + f + suffix) return ret return [] try: m2 = re.match("^(.*?)([A-Z0-9][A-Z0-9_]*)$", text) except Exception as ex: return [] prefix = m2.group(1) mtype = m2.group(2) ret = [] for k in list(rline_mpstate.status.msgs.keys()): if k.startswith(mtype): ret.append(prefix + k + suffix) return ret
def mavlink_packet(self, msg): '''handle an incoming mavlink packet''' if not isinstance(self.console, wxconsole.MessageConsole): return if not self.console.is_alive(): self.mpstate.console = textconsole.SimpleConsole() return type = msg.get_type() sysid = msg.get_srcSystem() compid = msg.get_srcComponent() if type == 'HEARTBEAT' or type == 'HIGH_LATENCY2': if not sysid in self.vehicle_list: self.add_new_vehicle(msg) if sysid not in self.component_name: self.component_name[sysid] = {} if compid not in self.component_name[sysid]: self.component_name[sysid][compid] = self.component_type_string(msg) self.update_vehicle_menu() if self.last_param_sysid_timestamp != self.module('param').new_sysid_timestamp: '''a new component ID has appeared for parameters''' self.last_param_sysid_timestamp = self.module('param').new_sysid_timestamp self.update_vehicle_menu() if type in ['RADIO', 'RADIO_STATUS']: # handle RADIO msgs from all vehicles if msg.rssi < msg.noise+10 or msg.remrssi < msg.remnoise+10: fg = 'red' else: fg = 'black' self.console.set_status('Radio', 'Radio %u/%u %u/%u' % (msg.rssi, msg.noise, msg.remrssi, msg.remnoise), fg=fg) if type == 'SYS_STATUS': self.check_critical_error(msg) if not self.is_primary_vehicle(msg): # don't process msgs from other than primary vehicle, other than # updating vehicle list return master = self.master # add some status fields if type in [ 'GPS_RAW_INT', 'GPS2_RAW' ]: if type == 'GPS_RAW_INT': field = 'GPS' prefix = 'GPS:' else: field = 'GPS2' prefix = 'GPS2' nsats = msg.satellites_visible fix_type = msg.fix_type if fix_type >= 3: self.console.set_status(field, '%s OK%s (%u)' % (prefix, fix_type, nsats), fg='green') else: self.console.set_status(field, '%s %u (%u)' % (prefix, fix_type, nsats), fg='red') if type == 'GPS_RAW_INT': vfr_hud_heading = master.field('VFR_HUD', 'heading', None) gps_heading = int(msg.cog * 0.01) if vfr_hud_heading is None: vfr_hud_heading = '---' else: vfr_hud_heading = '%3u' % vfr_hud_heading self.console.set_status('Heading', 'Hdg %s/%3u' % (vfr_hud_heading, gps_heading)) elif type == 'VFR_HUD': if master.mavlink10(): alt = master.field('GPS_RAW_INT', 'alt', 0) / 1.0e3 else: alt = master.field('GPS_RAW', 'alt', 0) home = self.module('wp').get_home() if home is not None: home_lat = home.x home_lng = home.y else: home_lat = None home_lng = None lat = master.field('GLOBAL_POSITION_INT', 'lat', 0) * 1.0e-7 lng = master.field('GLOBAL_POSITION_INT', 'lon', 0) * 1.0e-7 rel_alt = master.field('GLOBAL_POSITION_INT', 'relative_alt', 0) * 1.0e-3 agl_alt = None if self.settings.basealt != 0: agl_alt = self.console.ElevationMap.GetElevation(lat, lng) if agl_alt is not None: agl_alt = self.settings.basealt - agl_alt else: try: agl_alt_home = self.console.ElevationMap.GetElevation(home_lat, home_lng) except Exception as ex: print(ex) agl_alt_home = None if agl_alt_home is not None: agl_alt = self.console.ElevationMap.GetElevation(lat, lng) if agl_alt is not None: agl_alt = agl_alt_home - agl_alt if agl_alt is not None: agl_alt += rel_alt vehicle_agl = master.field('TERRAIN_REPORT', 'current_height', None) if vehicle_agl is None: vehicle_agl = '---' else: vehicle_agl = self.height_string(vehicle_agl) self.console.set_status('AGL', 'AGL %s/%s' % (self.height_string(agl_alt), vehicle_agl)) self.console.set_status('Alt', 'Alt %s' % self.height_string(rel_alt)) self.console.set_status('AirSpeed', 'AirSpeed %s' % self.speed_string(msg.airspeed)) self.console.set_status('GPSSpeed', 'GPSSpeed %s' % self.speed_string(msg.groundspeed)) self.console.set_status('Thr', 'Thr %u' % msg.throttle) t = time.localtime(msg._timestamp) flying = False if self.mpstate.vehicle_type == 'copter': flying = self.master.motors_armed() else: flying = msg.groundspeed > 3 if flying and not self.in_air: self.in_air = True self.start_time = time.mktime(t) elif flying and self.in_air: self.total_time = time.mktime(t) - self.start_time self.console.set_status('FlightTime', 'FlightTime %u:%02u' % (int(self.total_time)/60, int(self.total_time)%60)) elif not flying and self.in_air: self.in_air = False self.total_time = time.mktime(t) - self.start_time self.console.set_status('FlightTime', 'FlightTime %u:%02u' % (int(self.total_time)/60, int(self.total_time)%60)) elif type == 'ATTITUDE': self.console.set_status('Roll', 'Roll %u' % math.degrees(msg.roll)) self.console.set_status('Pitch', 'Pitch %u' % math.degrees(msg.pitch)) elif type in ['SYS_STATUS']: sensors = { 'AS' : mavutil.mavlink.MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE, 'MAG' : mavutil.mavlink.MAV_SYS_STATUS_SENSOR_3D_MAG, 'INS' : mavutil.mavlink.MAV_SYS_STATUS_SENSOR_3D_ACCEL | mavutil.mavlink.MAV_SYS_STATUS_SENSOR_3D_GYRO, 'AHRS' : mavutil.mavlink.MAV_SYS_STATUS_AHRS, 'RC' : mavutil.mavlink.MAV_SYS_STATUS_SENSOR_RC_RECEIVER, 'TERR' : mavutil.mavlink.MAV_SYS_STATUS_TERRAIN, 'RNG' : mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION, 'LOG' : mavutil.mavlink.MAV_SYS_STATUS_LOGGING, 'PRX' : mavutil.mavlink.MAV_SYS_STATUS_SENSOR_PROXIMITY, 'PRE' : mavutil.mavlink.MAV_SYS_STATUS_PREARM_CHECK, } hide_if_not_present = set(['PRE', 'PRX']) for s in sensors.keys(): bits = sensors[s] present = ((msg.onboard_control_sensors_present & bits) == bits) enabled = ((msg.onboard_control_sensors_enabled & bits) == bits) healthy = ((msg.onboard_control_sensors_health & bits) == bits) if not present and s in hide_if_not_present: continue if not present: fg = 'black' elif not enabled: fg = 'grey' elif not healthy: fg = 'red' else: fg = 'green' # for terrain show yellow if still loading if s == 'TERR' and fg == 'green' and master.field('TERRAIN_REPORT', 'pending', 0) != 0: fg = 'yellow' self.console.set_status(s, s, fg=fg) announce_unhealthy = { 'RC': 'RC', 'PRE': 'pre-arm', } for s in announce_unhealthy.keys(): bits = sensors[s] enabled = ((msg.onboard_control_sensors_enabled & bits) == bits) healthy = ((msg.onboard_control_sensors_health & bits) == bits) was_healthy = ((self.last_sys_status_health & bits) == bits) if enabled and not healthy and was_healthy: self.say("%s fail" % announce_unhealthy[s]) announce_healthy = { 'PRE': 'pre-arm', } for s in announce_healthy.keys(): bits = sensors[s] enabled = ((msg.onboard_control_sensors_enabled & bits) == bits) healthy = ((msg.onboard_control_sensors_health & bits) == bits) was_healthy = ((self.last_sys_status_health & bits) == bits) if enabled and healthy and not was_healthy: self.say("%s good" % announce_healthy[s]) self.last_sys_status_health = msg.onboard_control_sensors_health if ((msg.onboard_control_sensors_enabled & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS) == 0): self.safety_on = True else: self.safety_on = False elif type == 'WIND': self.console.set_status('Wind', 'Wind %u/%s' % (msg.direction, self.speed_string(msg.speed))) elif type == 'EKF_STATUS_REPORT': highest = 0.0 vars = ['velocity_variance', 'pos_horiz_variance', 'pos_vert_variance', 'compass_variance', 'terrain_alt_variance'] for var in vars: v = getattr(msg, var, 0) highest = max(v, highest) if highest >= 1.0: fg = 'red' elif highest >= 0.5: fg = 'orange' else: fg = 'green' self.console.set_status('EKF', 'EKF', fg=fg) elif type == 'HWSTATUS': if msg.Vcc >= 4600 and msg.Vcc <= 5300: fg = 'green' else: fg = 'red' self.console.set_status('Vcc', 'Vcc %.2f' % (msg.Vcc * 0.001), fg=fg) elif type == 'POWER_STATUS': if msg.flags & mavutil.mavlink.MAV_POWER_STATUS_CHANGED: fg = 'red' else: fg = 'green' status = 'PWR:' if msg.flags & mavutil.mavlink.MAV_POWER_STATUS_USB_CONNECTED: status += 'U' if msg.flags & mavutil.mavlink.MAV_POWER_STATUS_BRICK_VALID: status += 'B' if msg.flags & mavutil.mavlink.MAV_POWER_STATUS_SERVO_VALID: status += 'S' if msg.flags & mavutil.mavlink.MAV_POWER_STATUS_PERIPH_OVERCURRENT: status += 'O1' if msg.flags & mavutil.mavlink.MAV_POWER_STATUS_PERIPH_HIPOWER_OVERCURRENT: status += 'O2' self.console.set_status('PWR', status, fg=fg) self.console.set_status('Srv', 'Srv %.2f' % (msg.Vservo*0.001), fg='green') elif type == 'HEARTBEAT': fmode = master.flightmode if self.settings.vehicle_name: fmode = self.settings.vehicle_name + ':' + fmode self.console.set_status('Mode', '%s' % fmode, fg='blue') if len(self.vehicle_list) > 1: self.console.set_status('SysID', 'Sys:%u' % sysid, fg='blue') if self.master.motors_armed(): arm_colour = 'green' else: arm_colour = 'red' armstring = 'ARM' # add safety switch state if self.safety_on: armstring += '(SAFE)' self.console.set_status('ARM', armstring, fg=arm_colour) if self.max_link_num != len(self.mpstate.mav_master): for i in range(self.max_link_num): self.console.set_status('Link%u'%(i+1), '', row=1) self.max_link_num = len(self.mpstate.mav_master) for m in self.mpstate.mav_master: if self.mpstate.settings.checkdelay: highest_msec_key = (sysid, compid) linkdelay = (self.mpstate.status.highest_msec.get(highest_msec_key, 0) - m.highest_msec.get(highest_msec_key,0))*1.0e-3 else: linkdelay = 0 linkline = "Link %s " % (self.link_label(m)) fg = 'dark green' if m.linkerror: linkline += "down" fg = 'red' else: packets_rcvd_percentage = 100 if (m.mav_count+m.mav_loss) != 0: #avoid divide-by-zero packets_rcvd_percentage = (100.0 * m.mav_count) / (m.mav_count + m.mav_loss) linkbits = ["%u pkts" % m.mav_count, "%u lost" % m.mav_loss, "%.2fs delay" % linkdelay, ] try: if m.mav.signing.sig_count: # other end is sending us signed packets if not m.mav.signing.secret_key: # we've received signed packets but # can't verify them fg = 'orange' linkbits.append("!KEY") elif not m.mav.signing.sign_outgoing: # we've received signed packets but aren't # signing outselves; this can lead to hairloss fg = 'orange' linkbits.append("!SIGNING") if m.mav.signing.badsig_count: fg = 'orange' linkbits.append("%u badsigs" % m.mav.signing.badsig_count) except AttributeError as e: # mav.signing.sig_count probably doesn't exist pass linkline += "OK {rcv_pct:.1f}% ({bits})".format( rcv_pct=packets_rcvd_percentage, bits=", ".join(linkbits)) if linkdelay > 1 and fg == 'dark green': fg = 'orange' self.console.set_status('Link%u'%m.linknum, linkline, row=1, fg=fg) elif type in ['WAYPOINT_CURRENT', 'MISSION_CURRENT']: wpmax = self.module('wp').wploader.count() if wpmax > 0: wpmax = "/%u" % wpmax else: wpmax = "" self.console.set_status('WP', 'WP %u%s' % (msg.seq, wpmax)) lat = master.field('GLOBAL_POSITION_INT', 'lat', 0) * 1.0e-7 lng = master.field('GLOBAL_POSITION_INT', 'lon', 0) * 1.0e-7 if lat != 0 and lng != 0: airspeed = master.field('VFR_HUD', 'airspeed', 30) if abs(airspeed - self.speed) > 5: self.speed = airspeed else: self.speed = 0.98*self.speed + 0.02*airspeed self.speed = max(1, self.speed) time_remaining = int(self.estimated_time_remaining(lat, lng, msg.seq, self.speed)) self.console.set_status('ETR', 'ETR %u:%02u' % (time_remaining/60, time_remaining%60)) elif type == 'NAV_CONTROLLER_OUTPUT': self.console.set_status('WPDist', 'Distance %s' % self.dist_string(msg.wp_dist)) self.console.set_status('WPBearing', 'Bearing %u' % msg.target_bearing) if msg.alt_error > 0: alt_error_sign = "(L)" else: alt_error_sign = "(H)" if msg.aspd_error > 0: aspd_error_sign = "(L)" else: aspd_error_sign = "(H)" if math.isnan(msg.alt_error): alt_error = "NaN" else: alt_error = "%s%s" % (self.height_string(msg.alt_error), alt_error_sign) self.console.set_status('AltError', 'AltError %s' % alt_error) self.console.set_status('AspdError', 'AspdError %s%s' % (self.speed_string(msg.aspd_error*0.01), aspd_error_sign)) elif type == 'PARAM_VALUE': rec, tot = self.module('param').param_status() self.console.set_status('Params', 'Param %u/%u' % (rec,tot)) elif type == 'HIGH_LATENCY2': self.console.set_status('WPDist', 'Distance %s' % self.dist_string(msg.target_distance * 10)) # The -180 here for for consistency with NAV_CONTROLLER_OUTPUT (-180->180), whereas HIGH_LATENCY2 is (0->360) self.console.set_status('WPBearing', 'Bearing %u' % ((msg.target_heading * 2) - 180)) alt_error = "%s%s" % (self.height_string(msg.target_altitude - msg.altitude), "(L)" if (msg.target_altitude - msg.altitude) > 0 else "(L)") self.console.set_status('AltError', 'AltError %s' % alt_error) self.console.set_status('AspdError', 'AspdError %s%s' % (self.speed_string((msg.airspeed_sp - msg.airspeed)/5), "(L)" if (msg.airspeed_sp - msg.airspeed) > 0 else "(L)")) # The -180 here for for consistency with WIND (-180->180), whereas HIGH_LATENCY2 is (0->360) self.console.set_status('Wind', 'Wind %u/%s' % ((msg.wind_heading * 2) - 180, self.speed_string(msg.windspeed / 5))) self.console.set_status('Alt', 'Alt %s' % self.height_string(msg.altitude - self.console.ElevationMap.GetElevation(msg.latitude / 1E7, msg.longitude / 1E7))) self.console.set_status('AirSpeed', 'AirSpeed %s' % self.speed_string(msg.airspeed / 5)) self.console.set_status('GPSSpeed', 'GPSSpeed %s' % self.speed_string(msg.groundspeed / 5)) self.console.set_status('Thr', 'Thr %u' % msg.throttle) self.console.set_status('Heading', 'Hdg %s/---' % (msg.heading * 2)) self.console.set_status('WP', 'WP %u/--' % (msg.wp_num)) #re-map sensors sensors = { 'AS' : mavutil.mavlink.HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE, 'MAG' : mavutil.mavlink.HL_FAILURE_FLAG_3D_MAG, 'INS' : mavutil.mavlink.HL_FAILURE_FLAG_3D_ACCEL | mavutil.mavlink.HL_FAILURE_FLAG_3D_GYRO, 'AHRS' : mavutil.mavlink.HL_FAILURE_FLAG_ESTIMATOR, 'RC' : mavutil.mavlink.HL_FAILURE_FLAG_RC_RECEIVER, 'TERR' : mavutil.mavlink.HL_FAILURE_FLAG_TERRAIN } for s in sensors.keys(): bits = sensors[s] failed = ((msg.failure_flags & bits) == bits) if failed: fg = 'red' else: fg = 'green' self.console.set_status(s, s, fg=fg) # do the remaining non-standard system mappings fence_failed = ((msg.failure_flags & mavutil.mavlink.HL_FAILURE_FLAG_GEOFENCE) == mavutil.mavlink.HL_FAILURE_FLAG_GEOFENCE) if fence_failed: fg = 'red' else: fg = 'green' self.console.set_status('Fence', 'FEN', fg=fg) gps_failed = ((msg.failure_flags & mavutil.mavlink.HL_FAILURE_FLAG_GPS) == mavutil.mavlink.HL_FAILURE_FLAG_GPS) if gps_failed: self.console.set_status('GPS', 'GPS FAILED', fg='red') else: self.console.set_status('GPS', 'GPS OK', fg='green') batt_failed = ((msg.failure_flags & mavutil.mavlink.HL_FAILURE_FLAG_GPS) == mavutil.mavlink.HL_FAILURE_FLAG_BATTERY) if batt_failed: self.console.set_status('PWR', 'PWR FAILED', fg='red') else: self.console.set_status('PWR', 'PWR OK', fg='green') for id in self.user_added.keys(): if type in self.user_added[id].msg_types: d = self.user_added[id] val = mavutil.evaluate_expression(d.expression, self.master.messages) try: self.console.set_status(id, d.format % val, row = d.row) except Exception as ex: print(ex) pass