def master_msg_handling(self, m, master): '''link message handling for an upstream link''' if self.settings.target_system != 0 and m.get_srcSystem( ) != self.settings.target_system: # don't process messages not from our target if m.get_type() == "BAD_DATA": if self.mpstate.settings.shownoise and mavutil.all_printable( m.data): out = m.data if type(m.data) == bytearray: out = m.data.decode('ascii') self.mpstate.console.write(out, bg='red') return if self.settings.target_system != 0 and master.target_system != self.settings.target_system: # keep the pymavlink level target system aligned with the MAVProxy setting master.target_system = self.settings.target_system if self.settings.target_component != 0 and master.target_component != self.settings.target_component: # keep the pymavlink level target component aligned with the MAVProxy setting print("change target_component %u" % self.settings.target_component) master.target_component = self.settings.target_component mtype = m.get_type() if mtype == 'HEARTBEAT' and m.type != mavutil.mavlink.MAV_TYPE_GCS: if self.settings.target_system == 0 and self.settings.target_system != m.get_srcSystem( ): self.settings.target_system = m.get_srcSystem() self.say("online system %u" % self.settings.target_system, 'message') for mav in self.mpstate.mav_master: mav.target_system = self.settings.target_system if self.status.heartbeat_error: self.status.heartbeat_error = False self.say("heartbeat OK") if master.linkerror: master.linkerror = False self.say("link %s OK" % (self.link_label(master))) self.status.last_heartbeat = time.time() master.last_heartbeat = self.status.last_heartbeat armed = self.master.motors_armed() if armed != self.status.armed: self.status.armed = armed if armed: self.say("ARMED") else: self.say("DISARMED") if master.flightmode != self.status.flightmode: self.status.flightmode = master.flightmode if self.mpstate.functions.input_handler is None: self.set_prompt(self.status.flightmode + "> ") if master.flightmode != self.status.last_mode_announced and time.time( ) > self.status.last_mode_announce + 2: self.status.last_mode_announce = time.time() self.status.last_mode_announced = master.flightmode self.say("Mode " + self.status.flightmode) if m.type == mavutil.mavlink.MAV_TYPE_FIXED_WING: self.mpstate.vehicle_type = 'plane' self.mpstate.vehicle_name = 'ArduPlane' elif m.type in [ mavutil.mavlink.MAV_TYPE_GROUND_ROVER, mavutil.mavlink.MAV_TYPE_SURFACE_BOAT ]: self.mpstate.vehicle_type = 'rover' self.mpstate.vehicle_name = 'APMrover2' elif m.type in [mavutil.mavlink.MAV_TYPE_SUBMARINE]: self.mpstate.vehicle_type = 'sub' self.mpstate.vehicle_name = 'ArduSub' elif m.type in [ mavutil.mavlink.MAV_TYPE_QUADROTOR, mavutil.mavlink.MAV_TYPE_COAXIAL, mavutil.mavlink.MAV_TYPE_HEXAROTOR, mavutil.mavlink.MAV_TYPE_OCTOROTOR, mavutil.mavlink.MAV_TYPE_TRICOPTER, mavutil.mavlink.MAV_TYPE_HELICOPTER, mavutil.mavlink.MAV_TYPE_DODECAROTOR ]: self.mpstate.vehicle_type = 'copter' self.mpstate.vehicle_name = 'ArduCopter' elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]: self.mpstate.vehicle_type = 'antenna' self.mpstate.vehicle_name = 'AntennaTracker' elif mtype == 'STATUSTEXT': if m.text != self.status.last_apm_msg or time.time( ) > self.status.last_apm_msg_time + 2: (fg, bg) = self.colors_for_severity(m.severity) self.mpstate.console.writeln("APM: %s" % mp_util.null_term(m.text), bg=bg, fg=fg) awsprint("APM: %s" % mp_util.null_term(m.text), bg=bg, fg=fg) self.status.last_apm_msg = m.text self.status.last_apm_msg_time = time.time() elif mtype == "VFR_HUD": have_gps_lock = False if 'GPS_RAW' in self.status.msgs and self.status.msgs[ 'GPS_RAW'].fix_type == 2: have_gps_lock = True elif 'GPS_RAW_INT' in self.status.msgs and self.status.msgs[ 'GPS_RAW_INT'].fix_type == 3: have_gps_lock = True if have_gps_lock and not self.status.have_gps_lock and m.alt != 0: self.say("GPS lock at %u meters" % m.alt, priority='notification') self.status.have_gps_lock = True elif mtype == "GPS_RAW": if self.status.have_gps_lock: if m.fix_type != 2 and not self.status.lost_gps_lock and ( time.time() - self.status.last_gps_lock) > 3: self.say("GPS fix lost") self.status.lost_gps_lock = True if m.fix_type == 2 and self.status.lost_gps_lock: self.say("GPS OK") self.status.lost_gps_lock = False if m.fix_type == 2: self.status.last_gps_lock = time.time() elif mtype == "GPS_RAW_INT": if self.status.have_gps_lock: if m.fix_type < 3 and not self.status.lost_gps_lock and ( time.time() - self.status.last_gps_lock) > 3: self.say("GPS fix lost") self.status.lost_gps_lock = True if m.fix_type >= 3 and self.status.lost_gps_lock: self.say("GPS OK") self.status.lost_gps_lock = False if m.fix_type >= 3: self.status.last_gps_lock = time.time() elif mtype == "NAV_CONTROLLER_OUTPUT" and self.status.flightmode == "AUTO" and self.mpstate.settings.distreadout: rounded_dist = int(m.wp_dist / self.mpstate.settings.distreadout ) * self.mpstate.settings.distreadout if math.fabs(rounded_dist - self.status.last_distance_announce ) >= self.mpstate.settings.distreadout: if rounded_dist != 0: self.say("%u" % rounded_dist, priority="progress") self.status.last_distance_announce = rounded_dist elif mtype == "GLOBAL_POSITION_INT": self.report_altitude(m.relative_alt * 0.001) elif mtype == "COMPASSMOT_STATUS": print(m) elif mtype == "SIMSTATE": self.mpstate.is_sitl = True elif mtype == "ATTITUDE": att_time = m.time_boot_ms * 0.001 self.mpstate.attitude_time_s = max(self.mpstate.attitude_time_s, att_time) if self.mpstate.attitude_time_s - att_time > 120: # cope with wrap self.mpstate.attitude_time_s = att_time elif mtype in ["COMMAND_ACK", "MISSION_ACK"]: self.mpstate.console.writeln("Got MAVLink msg: %s" % m) awsprint("Got MAVLink msg: %s" % m) if mtype == "COMMAND_ACK" and m.command == mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION: if m.result == mavutil.mavlink.MAV_RESULT_ACCEPTED: self.say("Calibrated") elif m.result == mavutil.mavlink.MAV_RESULT_FAILED: self.say("Calibration failed") elif m.result == mavutil.mavlink.MAV_RESULT_UNSUPPORTED: self.say("Calibration unsupported") elif m.result == mavutil.mavlink.MAV_RESULT_TEMPORARILY_REJECTED: self.say("Calibration temporarily rejected") else: self.say("Calibration response (%u)" % m.result) else: #self.mpstate.console.writeln("Got MAVLink msg: %s" % m) pass if self.status.watch is not None: for msg_type in self.status.watch: if fnmatch.fnmatch(mtype.upper(), msg_type.upper()): self.mpstate.console.writeln('< ' + str(m)) break
def master_msg_handling(self, m, master): '''link message handling for an upstream link''' if self.settings.target_system != 0 and m.get_srcSystem() != self.settings.target_system: # don't process messages not from our target if m.get_type() == "BAD_DATA": if self.mpstate.settings.shownoise and mavutil.all_printable(m.data): self.mpstate.console.write(str(m.data), bg='red') return if self.settings.target_system != 0 and master.target_system != self.settings.target_system: # keep the pymavlink level target system aligned with the MAVProxy setting master.target_system = self.settings.target_system if self.settings.target_component != 0 and master.target_component != self.settings.target_component: # keep the pymavlink level target component aligned with the MAVProxy setting print("change target_component %u" % self.settings.target_component) master.target_component = self.settings.target_component mtype = m.get_type() if mtype == 'HEARTBEAT' and m.type != mavutil.mavlink.MAV_TYPE_GCS: if self.settings.target_system == 0 and self.settings.target_system != m.get_srcSystem(): self.settings.target_system = m.get_srcSystem() self.say("online system %u" % self.settings.target_system,'message') for mav in self.mpstate.mav_master: mav.target_system = self.settings.target_system if self.status.heartbeat_error: self.status.heartbeat_error = False self.say("heartbeat OK") if master.linkerror: master.linkerror = False self.say("link %s OK" % (self.link_label(master))) self.status.last_heartbeat = time.time() master.last_heartbeat = self.status.last_heartbeat armed = self.master.motors_armed() if armed != self.status.armed: self.status.armed = armed if armed: self.say("ARMED") else: self.say("DISARMED") if master.flightmode != self.status.flightmode: self.status.flightmode = master.flightmode if self.mpstate.functions.input_handler is None: self.set_prompt(self.status.flightmode + "> ") if master.flightmode != self.status.last_mode_announced and time.time() > self.status.last_mode_announce + 2: self.status.last_mode_announce = time.time() self.status.last_mode_announced = master.flightmode self.say("Mode " + self.status.flightmode) if m.type == mavutil.mavlink.MAV_TYPE_FIXED_WING: self.mpstate.vehicle_type = 'plane' self.mpstate.vehicle_name = 'ArduPlane' elif m.type in [mavutil.mavlink.MAV_TYPE_GROUND_ROVER, mavutil.mavlink.MAV_TYPE_SURFACE_BOAT]: self.mpstate.vehicle_type = 'rover' self.mpstate.vehicle_name = 'APMrover2' elif m.type in [mavutil.mavlink.MAV_TYPE_SUBMARINE]: self.mpstate.vehicle_type = 'sub' self.mpstate.vehicle_name = 'ArduSub' elif m.type in [mavutil.mavlink.MAV_TYPE_QUADROTOR, mavutil.mavlink.MAV_TYPE_COAXIAL, mavutil.mavlink.MAV_TYPE_HEXAROTOR, mavutil.mavlink.MAV_TYPE_OCTOROTOR, mavutil.mavlink.MAV_TYPE_TRICOPTER, mavutil.mavlink.MAV_TYPE_HELICOPTER, mavutil.mavlink.MAV_TYPE_DODECAROTOR]: self.mpstate.vehicle_type = 'copter' self.mpstate.vehicle_name = 'ArduCopter' elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]: self.mpstate.vehicle_type = 'antenna' self.mpstate.vehicle_name = 'AntennaTracker' elif mtype == 'STATUSTEXT': if m.text != self.status.last_apm_msg or time.time() > self.status.last_apm_msg_time+2: (fg, bg) = self.colors_for_severity(m.severity) self.mpstate.console.writeln("APM: %s" % mp_util.null_term(m.text), bg=bg, fg=fg) self.status.last_apm_msg = m.text self.status.last_apm_msg_time = time.time() elif mtype == "VFR_HUD": have_gps_lock = False if 'GPS_RAW' in self.status.msgs and self.status.msgs['GPS_RAW'].fix_type == 2: have_gps_lock = True elif 'GPS_RAW_INT' in self.status.msgs and self.status.msgs['GPS_RAW_INT'].fix_type == 3: have_gps_lock = True if have_gps_lock and not self.status.have_gps_lock and m.alt != 0: self.say("GPS lock at %u meters" % m.alt, priority='notification') self.status.have_gps_lock = True elif mtype == "GPS_RAW": if self.status.have_gps_lock: if m.fix_type != 2 and not self.status.lost_gps_lock and (time.time() - self.status.last_gps_lock) > 3: self.say("GPS fix lost") self.status.lost_gps_lock = True if m.fix_type == 2 and self.status.lost_gps_lock: self.say("GPS OK") self.status.lost_gps_lock = False if m.fix_type == 2: self.status.last_gps_lock = time.time() elif mtype == "GPS_RAW_INT": if self.status.have_gps_lock: if m.fix_type < 3 and not self.status.lost_gps_lock and (time.time() - self.status.last_gps_lock) > 3: self.say("GPS fix lost") self.status.lost_gps_lock = True if m.fix_type >= 3 and self.status.lost_gps_lock: self.say("GPS OK") self.status.lost_gps_lock = False if m.fix_type >= 3: self.status.last_gps_lock = time.time() elif mtype == "NAV_CONTROLLER_OUTPUT" and self.status.flightmode == "AUTO" and self.mpstate.settings.distreadout: rounded_dist = int(m.wp_dist/self.mpstate.settings.distreadout)*self.mpstate.settings.distreadout if math.fabs(rounded_dist - self.status.last_distance_announce) >= self.mpstate.settings.distreadout: if rounded_dist != 0: self.say("%u" % rounded_dist, priority="progress") self.status.last_distance_announce = rounded_dist elif mtype == "GLOBAL_POSITION_INT": self.report_altitude(m.relative_alt*0.001) elif mtype == "COMPASSMOT_STATUS": print(m) elif mtype == "SIMSTATE": self.mpstate.is_sitl = True elif mtype == "ATTITUDE": att_time = m.time_boot_ms * 0.001 self.mpstate.attitude_time_s = max(self.mpstate.attitude_time_s, att_time) if self.mpstate.attitude_time_s - att_time > 120: # cope with wrap self.mpstate.attitude_time_s = att_time elif mtype in [ "COMMAND_ACK", "MISSION_ACK" ]: self.mpstate.console.writeln("Got MAVLink msg: %s" % m) if mtype == "COMMAND_ACK" and m.command == mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION: if m.result == mavutil.mavlink.MAV_RESULT_ACCEPTED: self.say("Calibrated") elif m.result == mavutil.mavlink.MAV_RESULT_FAILED: self.say("Calibration failed") elif m.result == mavutil.mavlink.MAV_RESULT_UNSUPPORTED: self.say("Calibration unsupported") elif m.result == mavutil.mavlink.MAV_RESULT_TEMPORARILY_REJECTED: self.say("Calibration temporarily rejected") else: self.say("Calibration response (%u)" % m.result) else: #self.mpstate.console.writeln("Got MAVLink msg: %s" % m) pass if self.status.watch is not None: for msg_type in self.status.watch: if fnmatch.fnmatch(mtype.upper(), msg_type.upper()): self.mpstate.console.writeln('< '+ str(m)) break
def master_callback(self, m, master): '''process mavlink message m on master, sending any messages to recipients''' # see if it is handled by a specialised sysid connection sysid = m.get_srcSystem() if sysid in self.mpstate.sysid_outputs: self.mpstate.sysid_outputs[sysid].write(m.get_msgbuf()) if m.get_type() == "GLOBAL_POSITION_INT": for modname in 'map', 'asterix', 'NMEA', 'NMEA2': mod = self.module(modname) if mod is not None: mod.set_secondary_vehicle_position(m) return if getattr(m, '_timestamp', None) is None: master.post_message(m) self.status.counters['MasterIn'][master.linknum] += 1 mtype = m.get_type() if mtype == 'GLOBAL_POSITION_INT': # send GLOBAL_POSITION_INT to 2nd GCS for 2nd vehicle display for sysid in self.mpstate.sysid_outputs: self.mpstate.sysid_outputs[sysid].write(m.get_msgbuf()) # and log them if mtype not in dataPackets and self.mpstate.logqueue: # put link number in bottom 2 bits, so we can analyse packet # delay in saved logs usec = self.get_usec() usec = (usec & ~3) | master.linknum self.mpstate.logqueue.put( bytearray(struct.pack('>Q', usec) + m.get_msgbuf())) # keep the last message of each type around self.status.msgs[m.get_type()] = m if not m.get_type() in self.status.msg_count: self.status.msg_count[m.get_type()] = 0 self.status.msg_count[m.get_type()] += 1 if m.get_srcComponent( ) == mavutil.mavlink.MAV_COMP_ID_GIMBAL and m.get_type( ) == 'HEARTBEAT': # silence gimbal heartbeat packets for now return if getattr( m, 'time_boot_ms', None ) is not None and self.settings.target_system == m.get_srcSystem(): # update link_delayed attribute self.handle_msec_timestamp(m, master) if mtype in activityPackets: if master.linkerror: master.linkerror = False self.say("link %s OK" % (self.link_label(master))) self.status.last_message = time.time() master.last_message = self.status.last_message if master.link_delayed: # don't process delayed packets that cause double reporting if mtype in delayedPackets: return if mtype == 'HEARTBEAT' and m.type != mavutil.mavlink.MAV_TYPE_GCS: if self.settings.target_system == 0 and self.settings.target_system != m.get_srcSystem( ): self.settings.target_system = m.get_srcSystem() self.say("online system %u" % self.settings.target_system, 'message') if self.status.heartbeat_error: self.status.heartbeat_error = False self.say("heartbeat OK") if master.linkerror: master.linkerror = False self.say("link %s OK" % (self.link_label(master))) self.status.last_heartbeat = time.time() master.last_heartbeat = self.status.last_heartbeat armed = self.master.motors_armed() if armed != self.status.armed: self.status.armed = armed if armed: self.say("ARMED") else: self.say("DISARMED") if master.flightmode != self.status.flightmode: self.status.flightmode = master.flightmode if self.mpstate.functions.input_handler is None: self.set_prompt(self.status.flightmode + "> ") if master.flightmode != self.status.last_mode_announced and time.time( ) > self.status.last_mode_announce + 2: self.status.last_mode_announce = time.time() self.status.last_mode_announced = master.flightmode self.say("Mode " + self.status.flightmode) if m.type == mavutil.mavlink.MAV_TYPE_FIXED_WING: self.mpstate.vehicle_type = 'plane' self.mpstate.vehicle_name = 'ArduPlane' elif m.type in [ mavutil.mavlink.MAV_TYPE_GROUND_ROVER, mavutil.mavlink.MAV_TYPE_SURFACE_BOAT ]: self.mpstate.vehicle_type = 'rover' self.mpstate.vehicle_name = 'APMrover2' elif m.type in [mavutil.mavlink.MAV_TYPE_SUBMARINE]: self.mpstate.vehicle_type = 'sub' self.mpstate.vehicle_name = 'ArduSub' elif m.type in [ mavutil.mavlink.MAV_TYPE_QUADROTOR, mavutil.mavlink.MAV_TYPE_COAXIAL, mavutil.mavlink.MAV_TYPE_HEXAROTOR, mavutil.mavlink.MAV_TYPE_OCTOROTOR, mavutil.mavlink.MAV_TYPE_TRICOPTER, mavutil.mavlink.MAV_TYPE_HELICOPTER, mavutil.mavlink.MAV_TYPE_DODECAROTOR ]: self.mpstate.vehicle_type = 'copter' self.mpstate.vehicle_name = 'ArduCopter' elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]: self.mpstate.vehicle_type = 'antenna' self.mpstate.vehicle_name = 'AntennaTracker' elif mtype == 'STATUSTEXT': if m.text != self.status.last_apm_msg or time.time( ) > self.status.last_apm_msg_time + 2: (fg, bg) = self.colors_for_severity(m.severity) self.mpstate.console.writeln("APM: %s" % mp_util.null_term(m.text), bg=bg, fg=fg) self.status.last_apm_msg = m.text self.status.last_apm_msg_time = time.time() elif mtype == "VFR_HUD": have_gps_lock = False if 'GPS_RAW' in self.status.msgs and self.status.msgs[ 'GPS_RAW'].fix_type == 2: have_gps_lock = True elif 'GPS_RAW_INT' in self.status.msgs and self.status.msgs[ 'GPS_RAW_INT'].fix_type == 3: have_gps_lock = True if have_gps_lock and not self.status.have_gps_lock and m.alt != 0: self.say("GPS lock at %u meters" % m.alt, priority='notification') self.status.have_gps_lock = True elif mtype == "GPS_RAW": if self.status.have_gps_lock: if m.fix_type != 2 and not self.status.lost_gps_lock and ( time.time() - self.status.last_gps_lock) > 3: self.say("GPS fix lost") self.status.lost_gps_lock = True if m.fix_type == 2 and self.status.lost_gps_lock: self.say("GPS OK") self.status.lost_gps_lock = False if m.fix_type == 2: self.status.last_gps_lock = time.time() elif mtype == "GPS_RAW_INT": if self.status.have_gps_lock: if m.fix_type < 3 and not self.status.lost_gps_lock and ( time.time() - self.status.last_gps_lock) > 3: self.say("GPS fix lost") self.status.lost_gps_lock = True if m.fix_type >= 3 and self.status.lost_gps_lock: self.say("GPS OK") self.status.lost_gps_lock = False if m.fix_type >= 3: self.status.last_gps_lock = time.time() elif mtype == "NAV_CONTROLLER_OUTPUT" and self.status.flightmode == "AUTO" and self.mpstate.settings.distreadout: rounded_dist = int(m.wp_dist / self.mpstate.settings.distreadout ) * self.mpstate.settings.distreadout if math.fabs(rounded_dist - self.status.last_distance_announce ) >= self.mpstate.settings.distreadout: if rounded_dist != 0: self.say("%u" % rounded_dist, priority="progress") self.status.last_distance_announce = rounded_dist elif mtype == "GLOBAL_POSITION_INT": self.report_altitude(m.relative_alt * 0.001) elif mtype == "COMPASSMOT_STATUS": print(m) elif mtype == "SIMSTATE": self.mpstate.is_sitl = True elif mtype == "ATTITUDE": att_time = m.time_boot_ms * 0.001 self.mpstate.attitude_time_s = max(self.mpstate.attitude_time_s, att_time) if self.mpstate.attitude_time_s - att_time > 120: # cope with wrap print("attitude time wrap") self.mpstate.attitude_time_s = att_time elif mtype == "BAD_DATA": if self.mpstate.settings.shownoise and mavutil.all_printable( m.data): self.mpstate.console.write(str(m.data), bg='red') elif mtype in ["COMMAND_ACK", "MISSION_ACK"]: self.mpstate.console.writeln("Got MAVLink msg: %s" % m) if mtype == "COMMAND_ACK" and m.command == mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION: if m.result == mavutil.mavlink.MAV_RESULT_ACCEPTED: self.say("Calibrated") elif m.result == mavutil.mavlink.MAV_RESULT_FAILED: self.say("Calibration failed") elif m.result == mavutil.mavlink.MAV_RESULT_UNSUPPORTED: self.say("Calibration unsupported") elif m.result == mavutil.mavlink.MAV_RESULT_TEMPORARILY_REJECTED: self.say("Calibration temporarily rejected") else: self.say("Calibration response (%u)" % m.result) else: #self.mpstate.console.writeln("Got MAVLink msg: %s" % m) pass if self.status.watch is not None: for msg_type in self.status.watch: if fnmatch.fnmatch(m.get_type().upper(), msg_type.upper()): self.mpstate.console.writeln('< ' + str(m)) break # don't pass along bad data if mtype != 'BAD_DATA': # pass messages along to listeners, except for REQUEST_DATA_STREAM, which # would lead a conflict in stream rate setting between mavproxy and the other # GCS if self.mpstate.settings.mavfwd_rate or mtype != 'REQUEST_DATA_STREAM': if not mtype in self.no_fwd_types: for r in self.mpstate.mav_outputs: r.write(m.get_msgbuf()) # pass to modules for (mod, pm) in self.mpstate.modules: if not hasattr(mod, 'mavlink_packet'): continue try: mod.mavlink_packet(m) except Exception as msg: if self.mpstate.settings.moddebug == 1: print(msg) elif self.mpstate.settings.moddebug > 1: exc_type, exc_value, exc_traceback = sys.exc_info() traceback.print_exception(exc_type, exc_value, exc_traceback, limit=2, file=sys.stdout)