def __init__(self):
self.console = textconsole.SimpleConsole()
self.map = None
self.map_functions = {}
self.vehicle_type = None
self.settings = mp_settings.MPSettings(
[ ('link', int, 1),
('altreadout', int, 10),
('distreadout', int, 200),
('heartbeat', int, 1),
('numcells', int, 1),
('mavfwd', int, 1),
('mavfwd_rate', int, 0),
('streamrate', int, 4),
('streamrate2', int, 4),
('heartbeatreport', int, 1),
('moddebug', int, 0),
('rc1mul', int, 1),
('rc2mul', int, 1),
('rc4mul', int, 1),
('shownoise', int, 1),
('basealt', int, 0),
('wpalt', int, 100),
('parambatch', int, 10),
('requireexit', int, 0)]
)
self.completions = {
"script" : ["(FILENAME)"],
"set" : ["(SETTING)"]
}
self.status = MPStatus()
# master mavlink device
self.mav_master = None
# mavlink outputs
self.mav_outputs = []
# SITL output
self.sitl_output = None
self.mav_param = mavparm.MAVParmDict()
self.modules = []
self.functions = MAVFunctions()
self.select_extra = {}
self.continue_mode = False
self.aliases = {}
def __init__(self):
self.console = textconsole.SimpleConsole()
self.map = None
self.map_functions = {}
self.vehicle_type = None
self.vehicle_name = None
from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting
self.settings = MPSettings(
[ MPSetting('link', int, 1, 'Primary Link', tab='Link', range=(0,4), increment=1),
MPSetting('streamrate', int, 4, 'Stream rate link1', range=(-1,100), increment=1),
MPSetting('streamrate2', int, 4, 'Stream rate link2', range=(-1,100), increment=1),
MPSetting('heartbeat', int, 1, 'Heartbeat rate', range=(0,5), increment=1),
MPSetting('mavfwd', bool, True, 'Allow forwarded control'),
MPSetting('mavfwd_rate', bool, False, 'Allow forwarded rate control'),
MPSetting('shownoise', bool, True, 'Show non-MAVLink data'),
MPSetting('baudrate', int, opts.baudrate, 'baudrate for new links', range=(0,10000000), increment=1),
MPSetting('rtscts', bool, opts.rtscts, 'enable flow control'),
MPSetting('select_timeout', float, 0.01, 'select timeout'),
MPSetting('altreadout', int, 10, 'Altitude Readout',
range=(0,100), increment=1, tab='Announcements'),
MPSetting('distreadout', int, 200, 'Distance Readout', range=(0,10000), increment=1),
MPSetting('moddebug', int, opts.moddebug, 'Module Debug Level', range=(0,3), increment=1, tab='Debug'),
MPSetting('compdebug', int, 0, 'Computation Debug Mask', range=(0,3), tab='Debug'),
MPSetting('flushlogs', bool, False, 'Flush logs on every packet'),
MPSetting('requireexit', bool, False, 'Require exit command'),
MPSetting('wpupdates', bool, True, 'Announce waypoint updates'),
MPSetting('basealt', int, 0, 'Base Altitude', range=(0,30000), increment=1, tab='Altitude'),
MPSetting('wpalt', int, 100, 'Default WP Altitude', range=(0,10000), increment=1),
MPSetting('rallyalt', int, 90, 'Default Rally Altitude', range=(0,10000), increment=1),
MPSetting('terrainalt', str, 'Auto', 'Use terrain altitudes', choice=['Auto','True','False']),
MPSetting('rally_breakalt', int, 40, 'Default Rally Break Altitude', range=(0,10000), increment=1),
MPSetting('rally_flags', int, 0, 'Default Rally Flags', range=(0,10000), increment=1),
MPSetting('source_system', int, 255, 'MAVLink Source system', range=(0,255), increment=1, tab='MAVLink'),
MPSetting('source_component', int, 0, 'MAVLink Source component', range=(0,255), increment=1),
MPSetting('target_system', int, 0, 'MAVLink target system', range=(0,255), increment=1),
MPSetting('target_component', int, 0, 'MAVLink target component', range=(0,255), increment=1),
MPSetting('state_basedir', str, None, 'base directory for logs and aircraft directories'),
MPSetting('allow_unsigned', bool, True, 'whether unsigned packets will be accepted'),
MPSetting('dist_unit', str, 'm', 'distance unit', choice=['m', 'nm', 'miles'], tab='Units'),
MPSetting('height_unit', str, 'm', 'height unit', choice=['m', 'feet']),
MPSetting('speed_unit', str, 'm/s', 'height unit', choice=['m/s', 'knots', 'mph']),
MPSetting('vehicle_name', str, '', 'Vehicle Name', tab='Vehicle'),
])
self.completions = {
"script" : ["(FILENAME)"],
"set" : ["(SETTING)"],
"status" : ["(VARIABLE)"],
"module" : ["list",
"load (AVAILMODULES)",
"<unload|reload> (LOADEDMODULES)"]
}
self.status = MPStatus()
# master mavlink device
self.mav_master = None
# mavlink outputs
self.mav_outputs = []
self.sysid_outputs = {}
# SITL output
self.sitl_output = None
self.mav_param = mavparm.MAVParmDict()
self.modules = []
self.public_modules = {}
self.functions = MAVFunctions()
self.select_extra = {}
self.continue_mode = False
self.aliases = {}
import platform
self.system = platform.system()
def unload(self):
'''unload module'''
self.mpstate.console.close()
self.mpstate.console = textconsole.SimpleConsole()
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()
if type == 'HEARTBEAT':
sysid = msg.get_srcSystem()
if not sysid in self.vehicle_list:
self.add_new_vehicle(msg)
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 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', 'GPS_RAW_INT']:
if type == "GPS_RAW":
num_sats1 = master.field('GPS_STATUS', 'satellites_visible', 0)
else:
num_sats1 = msg.satellites_visible
num_sats2 = master.field('GPS2_RAW', 'satellites_visible', -1)
if num_sats2 == -1:
sats_string = "%u" % num_sats1
else:
sats_string = "%u/%u" % (num_sats1, num_sats2)
if ((msg.fix_type >= 3 and master.mavlink10())
or (msg.fix_type == 2 and not master.mavlink10())):
if (msg.fix_type >= 4):
fix_type = "%u" % msg.fix_type
else:
fix_type = ""
self.console.set_status('GPS',
'GPS: OK%s (%s)' %
(fix_type, sats_string),
fg='green')
else:
self.console.set_status('GPS',
'GPS: %u (%s)' %
(msg.fix_type, sats_string),
fg='red')
if master.mavlink10():
gps_heading = int(self.mpstate.status.msgs['GPS_RAW_INT'].cog *
0.01)
else:
gps_heading = self.mpstate.status.msgs['GPS_RAW'].hdg
self.console.set_status(
'Heading', 'Hdg %s/%u' %
(master.field('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,
}
announce = ['RC']
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:
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)
for s in announce:
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" % s)
self.last_sys_status_health = msg.onboard_control_sensors_health
elif type == 'WIND':
self.console.set_status(
'Wind', 'Wind %u/%.2f' % (msg.direction, 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' % msg.get_srcSystem(),
fg='blue')
if self.master.motors_armed():
arm_colour = 'green'
else:
arm_colour = 'red'
armstring = 'ARM'
# add safety switch state
if 'SYS_STATUS' in self.mpstate.status.msgs:
if (self.mpstate.status.msgs['SYS_STATUS'].
onboard_control_sensors_enabled
& mavutil.mavlink.MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS
) == 0:
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:
linkdelay = (self.mpstate.status.highest_msec -
m.highest_msec) * 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 = "%d%s" % (msg.alt_error, alt_error_sign)
self.console.set_status('AltError', 'AltError %s' % alt_error)
self.console.set_status(
'AspdError',
'AspdError %.1f%s' % (msg.aspd_error * 0.01, aspd_error_sign))
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()
master = self.master
# add some status fields
if type in ['GPS_RAW', 'GPS_RAW_INT']:
if type == "GPS_RAW":
num_sats1 = master.field('GPS_STATUS', 'satellites_visible', 0)
else:
num_sats1 = msg.satellites_visible
num_sats2 = master.field('GPS2_RAW', 'satellites_visible', -1)
if num_sats2 == -1:
sats_string = "%u" % num_sats1
else:
sats_string = "%u/%u" % (num_sats1, num_sats2)
if ((msg.fix_type == 3 and master.mavlink10())
or (msg.fix_type == 2 and not master.mavlink10())):
self.console.set_status('GPS',
'GPS: OK (%s)' % sats_string,
fg='green')
else:
self.console.set_status('GPS',
'GPS: %u (%s)' %
(msg.fix_type, sats_string),
fg='red')
if master.mavlink10():
gps_heading = int(self.mpstate.status.msgs['GPS_RAW_INT'].cog *
0.01)
else:
gps_heading = self.mpstate.status.msgs['GPS_RAW'].hdg
self.console.set_status(
'Heading', 'Hdg %s/%u' %
(master.field('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)
if self.module('wp').wploader.count() > 0:
wp = self.module('wp').wploader.wp(0)
home_lat = wp.x
home_lng = wp.y
else:
home_lat = master.field('HOME', 'lat') * 1.0e-7
home_lng = master.field('HOME', 'lon') * 1.0e-7
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:
agl_alt_home = self.console.ElevationMap.GetElevation(
home_lat, home_lng)
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 = int(vehicle_agl)
self.console.set_status('AGL',
'AGL %u/%s' % (agl_alt, vehicle_agl))
self.console.set_status('Alt', 'Alt %u' % rel_alt)
self.console.set_status('AirSpeed', 'AirSpeed %u' % msg.airspeed)
self.console.set_status('GPSSpeed',
'GPSSpeed %u' % msg.groundspeed)
self.console.set_status('Thr', 'Thr %u' % msg.throttle)
t = time.localtime(msg._timestamp)
if msg.groundspeed > 3 and not self.in_air:
self.in_air = True
self.start_time = time.mktime(t)
elif msg.groundspeed > 3 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 msg.groundspeed < 3 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,
'TERR':
mavutil.mavlink.MAV_SYS_STATUS_TERRAIN,
'RNG':
mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION
}
for s in sensors.keys():
bits = sensors[s]
present = ((msg.onboard_control_sensors_enabled
& bits) == bits)
healthy = ((msg.onboard_control_sensors_health & bits) == bits)
if not present:
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)
elif type == 'WIND':
self.console.set_status(
'Wind', 'Wind %u/%.2f' % (msg.direction, msg.speed))
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 in ['RADIO', 'RADIO_STATUS']:
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)
elif type == 'HEARTBEAT':
self.console.set_status('Mode',
'%s' % master.flightmode,
fg='blue')
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:
linkdelay = (self.mpstate.status.highest_msec -
m.highest_msec) * 1.0e-3
linkline = "Link %u " % (m.linknum + 1)
if m.linkerror:
linkline += "down"
fg = 'red'
else:
packets_rcvd_percentage = 100
if (m.mav_loss != 0): #avoid divide-by-zero
packets_rcvd_percentage = (
1.0 -
(float(m.mav_loss) / float(m.mav_count))) * 100.0
linkline += "OK (%u pkts, %.2fs delay, %u lost) %u%%" % (
m.mav_count, linkdelay, m.mav_loss,
packets_rcvd_percentage)
if linkdelay > 1:
fg = 'orange'
else:
fg = 'darkgreen'
self.console.set_status('Link%u' % m.linknum,
linkline,
row=1,
fg=fg)
elif type in ['WAYPOINT_CURRENT', 'MISSION_CURRENT']:
self.console.set_status('WP', 'WP %u' % msg.seq)
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 %u' % 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"
self.console.set_status(
'AltError', 'AltError %d%s' % (msg.alt_error, alt_error_sign))
self.console.set_status(
'AspdError',
'AspdError %.1f%s' % (msg.aspd_error * 0.01, aspd_error_sign))
def __init__(self):
self.console = textconsole.SimpleConsole()
self.map = None
self.map_functions = {}
self.vehicle_type = None
self.vehicle_name = None
from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting
self.settings = MPSettings([
MPSetting('link',
int,
1,
'Primary Link',
tab='Link',
range=(0, 4),
increment=1),
MPSetting('streamrate',
int,
4,
'Stream rate link1',
range=(-1, 20),
increment=1),
MPSetting('streamrate2',
int,
4,
'Stream rate link2',
range=(-1, 20),
increment=1),
MPSetting('heartbeat',
int,
1,
'Heartbeat rate',
range=(0, 5),
increment=1),
MPSetting('mavfwd', bool, True, 'Allow forwarded control'),
MPSetting('mavfwd_rate', bool, False,
'Allow forwarded rate control'),
MPSetting('shownoise', bool, True, 'Show non-MAVLink data'),
MPSetting('baudrate',
int,
opts.baudrate,
'baudrate for new links',
range=(0, 10000000),
increment=1),
MPSetting('rtscts', bool, opts.rtscts, 'enable flow control'),
MPSetting('select_timeout', float, 0.5, 'select timeout'),
MPSetting('altreadout',
int,
10,
'Altitude Readout',
range=(0, 100),
increment=1,
tab='Announcements'),
MPSetting('distreadout',
int,
200,
'Distance Readout',
range=(0, 10000),
increment=1),
MPSetting('moddebug',
int,
0,
'Module Debug Level',
range=(0, 3),
increment=1,
tab='Debug'),
MPSetting('compdebug',
int,
0,
'Computation Debug Mask',
range=(0, 3),
tab='Debug'),
MPSetting('flushlogs', bool, False, 'Flush logs on every packet'),
MPSetting('requireexit', bool, False, 'Require exit command'),
MPSetting('wpupdates', bool, True, 'Announce waypoint updates'),
MPSetting('basealt',
int,
0,
'Base Altitude',
range=(0, 30000),
increment=1,
tab='Altitude'),
MPSetting('wpalt',
int,
100,
'Default WP Altitude',
range=(0, 10000),
increment=1),
MPSetting('rallyalt',
int,
90,
'Default Rally Altitude',
range=(0, 10000),
increment=1),
MPSetting('terrainalt',
str,
'Auto',
'Use terrain altitudes',
choice=['Auto', 'True', 'False']),
MPSetting('rally_breakalt',
int,
40,
'Default Rally Break Altitude',
range=(0, 10000),
increment=1),
MPSetting('rally_flags',
int,
0,
'Default Rally Flags',
range=(0, 10000),
increment=1),
MPSetting('source_system',
int,
255,
'MAVLink Source system',
range=(0, 255),
increment=1,
tab='MAVLink'),
MPSetting('source_component',
int,
0,
'MAVLink Source component',
range=(0, 255),
increment=1),
MPSetting('target_system',
int,
0,
'MAVLink target system',
range=(0, 255),
increment=1),
MPSetting('target_component',
int,
0,
'MAVLink target component',
range=(0, 255),
increment=1)
])
self.completions = {
"script": ["(FILENAME)"],
"set": ["(SETTING)"],
"module":
["list", "load (AVAILMODULES)", "<unload|reload> (LOADEDMODULES)"]
}
self.status = MPStatus()
# master mavlink device
self.mav_master = None
# mavlink outputs
self.mav_outputs = []
# SITL output
self.sitl_output = None
self.mav_param = mavparm.MAVParmDict()
self.modules = []
self.modules_idle = [] # modules with idle callbacks
self.modules_packet = [] # modules that want to read packets
self.public_modules = {}
self.functions = MAVFunctions()
self.select_extra = {}
self.continue_mode = False
self.aliases = {}
self.max_rx_packets = None
self.logqueue = None
self.logqueue_raw = None
self.rx_blacklist = set(
) # A set of message types which could never be delegated to packet handlers (for use by DroneAPI high speed processing)
def __init__(self):
self.console = textconsole.SimpleConsole()
self.map = None
self.map_functions = {}
self.vehicle_type = None
self.vehicle_name = None
from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting
self.settings = MPSettings([
MPSetting('link',
int,
1,
'Primary Link',
tab='Link',
range=(0, 4),
increment=1),
MPSetting('streamrate',
int,
4,
'Stream rate link1',
range=(-1, 20),
increment=1),
MPSetting('streamrate2',
int,
4,
'Stream rate link2',
range=(-1, 20),
increment=1),
MPSetting('heartbeat',
int,
1,
'Heartbeat rate',
range=(0, 5),
increment=1),
MPSetting('mavfwd', bool, True, 'Allow forwarded control'),
MPSetting('mavfwd_rate', bool, False,
'Allow forwarded rate control'),
MPSetting('shownoise', bool, True, 'Show non-MAVLink data'),
MPSetting('altreadout',
int,
10,
'Altitude Readout',
range=(0, 100),
increment=1,
tab='Announcements'),
MPSetting('distreadout',
int,
200,
'Distance Readout',
range=(0, 10000),
increment=1),
MPSetting('moddebug',
int,
0,
'Module Debug Level',
range=(0, 3),
increment=1,
tab='Debug'),
MPSetting('flushlogs', bool, False, 'Flush logs on every packet'),
MPSetting('requireexit', bool, False, 'Require exit command'),
MPSetting('basealt',
int,
0,
'Base Altitude',
range=(0, 30000),
increment=1,
tab='Altitude'),
MPSetting('wpalt',
int,
100,
'Default WP Altitude',
range=(0, 10000),
increment=1),
MPSetting('rallyalt',
int,
90,
'Default Rally Altitude',
range=(0, 10000),
increment=1),
MPSetting('terrainalt',
str,
'Auto',
'Use terrain altitudes',
choice=['Auto', 'True', 'False']),
MPSetting('wpupdates', bool, True, 'Show waypoint updates'),
MPSetting('rally_breakalt',
int,
40,
'Default Rally Break Altitude',
range=(0, 10000),
increment=1),
MPSetting('rally_flags',
int,
0,
'Default Rally Flags`',
range=(0, 10000),
increment=1),
MPSetting('baudrate',
int,
opts.baudrate,
'baudrate for new links',
range=(0, 10000000),
increment=1),
MPSetting('rtscts', bool, opts.rtscts, 'enable flow control'),
MPSetting('select_timeout', float, 0.01, 'select timeout')
])
self.completions = {"script": ["(FILENAME)"], "set": ["(SETTING)"]}
self.status = MPStatus()
# master mavlink device
self.mav_master = None
# mavlink outputs
self.mav_outputs = []
# SITL output
self.sitl_output = None
self.mav_param = mavparm.MAVParmDict()
self.modules = []
self.public_modules = {}
self.functions = MAVFunctions()
self.select_extra = {}
self.continue_mode = False
self.aliases = {}
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
def unload():
'''unload module'''
mpstate.console = textconsole.SimpleConsole()
def mavlink_packet(msg):
'''handle an incoming mavlink packet'''
if not isinstance(mpstate.console, wxconsole.MessageConsole):
return
if not mpstate.console.is_alive():
mpstate.console = textconsole.SimpleConsole()
return
type = msg.get_type()
master = mpstate.master()
state = mpstate.console_state
# add some status fields
if type in ['GPS_RAW', 'GPS_RAW_INT']:
if type == "GPS_RAW":
num_sats = master.field('GPS_STATUS', 'satellites_visible', 0)
else:
num_sats = msg.satellites_visible
if ((msg.fix_type == 3 and master.mavlink10())
or (msg.fix_type == 2 and not master.mavlink10())):
mpstate.console.set_status('GPS',
'GPS: OK (%u)' % num_sats,
fg='green')
else:
mpstate.console.set_status('GPS',
'GPS: %u (%u)' %
(msg.fix_type, num_sats),
fg='red')
if master.mavlink10():
gps_heading = int(mpstate.status.msgs['GPS_RAW_INT'].cog * 0.01)
else:
gps_heading = mpstate.status.msgs['GPS_RAW'].hdg
mpstate.console.set_status(
'Heading', 'Hdg %s/%u' %
(master.field('VFR_HUD', 'heading', '-'), gps_heading))
elif type == 'VFR_HUD':
mpstate.console.set_status('Mode', '%s' % master.flightmode, fg='blue')
if master.mavlink10():
alt = master.field('GPS_RAW_INT', 'alt', 0) / 1.0e3
else:
alt = master.field('GPS_RAW', 'alt', 0)
if mpstate.status.wploader.count() > 0:
wp = mpstate.status.wploader.wp(0)
home_lat = wp.x
home_lng = wp.y
else:
home_lat = master.field('HOME', 'lat') * 1.0e-7
home_lng = master.field('HOME', 'lon') * 1.0e-7
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
if mpstate.settings.basealt != 0:
agl_alt = mpstate.settings.basealt - mpstate.console.ElevationMap.GetElevation(
lat, lng)
else:
agl_alt = mpstate.console.ElevationMap.GetElevation(
home_lat,
home_lng) - mpstate.console.ElevationMap.GetElevation(
lat, lng)
agl_alt += rel_alt
mpstate.console.set_status('AGL', 'AGL %u' % agl_alt)
mpstate.console.set_status('Alt', 'Alt %u' % rel_alt)
mpstate.console.set_status('AirSpeed', 'AirSpeed %u' % msg.airspeed)
mpstate.console.set_status('GPSSpeed', 'GPSSpeed %u' % msg.groundspeed)
mpstate.console.set_status('Thr', 'Thr %u' % msg.throttle)
t = time.localtime(msg._timestamp)
if msg.groundspeed > 3 and not state.in_air:
state.in_air = True
state.start_time = time.mktime(t)
elif msg.groundspeed > 3 and state.in_air:
state.total_time = time.mktime(t) - state.start_time
mpstate.console.set_status(
'FlightTime', 'FlightTime %u:%02u' %
(int(state.total_time) / 60, int(state.total_time) % 60))
elif msg.groundspeed < 3 and state.in_air:
state.in_air = False
state.total_time = time.mktime(t) - state.start_time
mpstate.console.set_status(
'FlightTime', 'FlightTime %u:%02u' %
(int(state.total_time) / 60, int(state.total_time) % 60))
elif type == 'ATTITUDE':
mpstate.console.set_status('Roll', 'Roll %u' % math.degrees(msg.roll))
mpstate.console.set_status('Pitch',
'Pitch %u' % math.degrees(msg.pitch))
elif type == 'HWSTATUS':
if msg.Vcc >= 4600 and msg.Vcc <= 5300:
fg = 'green'
else:
fg = 'red'
mpstate.console.set_status('Vcc',
'Vcc %.2f' % (msg.Vcc * 0.001),
fg=fg)
elif type == 'RADIO':
if msg.rssi < msg.noise + 10 or msg.remrssi < msg.remnoise + 10:
fg = 'red'
else:
fg = 'black'
mpstate.console.set_status(
'Radio',
'Radio %u/%u %u/%u' %
(msg.rssi, msg.noise, msg.remrssi, msg.remnoise),
fg=fg)
elif type == 'HEARTBEAT':
for m in mpstate.mav_master:
linkdelay = (mpstate.status.highest_msec - m.highest_msec) * 1.0e-3
linkline = "Link %u " % (m.linknum + 1)
if m.linkerror:
linkline += "down"
fg = 'red'
else:
linkline += "OK (%u pkts, %.2fs delay, %u lost)" % (
m.mav_count, linkdelay, m.mav_loss)
if linkdelay > 1:
fg = 'yellow'
else:
fg = 'darkgreen'
mpstate.console.set_status('Link%u' % m.linknum,
linkline,
row=1,
fg=fg)
elif type in ['WAYPOINT_CURRENT', 'MISSION_CURRENT']:
mpstate.console.set_status('WP', 'WP %u' % msg.seq)
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 - state.speed) > 5:
state.speed = airspeed
else:
state.speed = 0.98 * state.speed + 0.02 * airspeed
state.speed = max(1, state.speed)
time_remaining = int(
estimated_time_remaining(lat, lng, msg.seq, state.speed))
mpstate.console.set_status(
'ETR',
'ETR %u:%02u' % (time_remaining / 60, time_remaining % 60))
elif type == 'NAV_CONTROLLER_OUTPUT':
mpstate.console.set_status('WPDist', 'Distance %u' % msg.wp_dist)
mpstate.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"
mpstate.console.set_status(
'AltError', 'AltError %d%s' % (msg.alt_error, alt_error_sign))
mpstate.console.set_status(
'AspdError',
'AspdError %.1f%s' % (msg.aspd_error * 0.01, aspd_error_sign))
