Skip to content

Commit 774fe23

Browse files
tridgetridge
committed
warning: improved warning module
display on console and make more configurable
1 parent 7444521 commit 774fe23

File tree

1 file changed

+112
-147
lines changed

1 file changed

+112
-147
lines changed

‎MAVProxy/modules/mavproxy_warning.py‎

Lines changed: 112 additions & 147 deletions
Original file line numberDiff line numberDiff line change
@@ -1,180 +1,145 @@
11
#!/usr/bin/env python
2-
'''Warning'''
2+
'''module to display and announce warnings about system failures'''
33

44
import time
55
from MAVProxy.modules.lib import mp_module
66
from MAVProxy.modules.lib import mp_settings
7-
from pymavlink import mavutil
87

98
class WarningModule(mp_module.MPModule):
109
def __init__(self, mpstate):
1110
super(WarningModule, self).__init__(mpstate, "warning", "warning module")
12-
self.add_command('warning', self.cmd_warning, "warning")
13-
self.prev_call = time.time()
14-
self.sensor_present = dict()
15-
self.sensor_enabled = dict()
16-
self.sensor_healthy = dict()
11+
self.add_command('warning', self.cmd_warning, "warning", ["details", "set (WARNINGSETTING)"])
12+
self.check_time = time.time()
13+
self.warn_time = time.time()
14+
self.failure = None
1715
self.details = ""
1816
self.warning_settings = mp_settings.MPSettings(
19-
[ ('check_freq', int, 5),
17+
[ ('warn_time', int, 5),
2018
('details', bool, True),
21-
('debug', bool, False),
2219
('min_sat', int, 10),
2320
('sat_diff', int, 5),
24-
('rpm_diff', int, 1000),
2521
('airspd_diff', float, 5),
26-
('max_wind', float, 10),
27-
('max_agl_alt', float, 123),
28-
('fwd_escs', int, 5),
29-
('vtol_escs', int, 10),
22+
('esc_group1', int, 0),
23+
('esc_group2', int, 0),
24+
('esc_min_rpm', int, 100),
3025
])
26+
self.add_completion_function('(WARNINGSETTING)',
27+
self.warning_settings.completion)
3128

3229
def cmd_warning(self, args):
3330
'''warning commands'''
3431
state = self
35-
if args and args[0] == 'set':
36-
if len(args) < 3:
37-
state.warning_settings.show_all()
38-
else:
39-
state.warning_settings.set(args[1], args[2])
40-
else:
41-
print('usage: warning set')
42-
43-
def monitor(self):
44-
fwd_escs = list(f'{self.warning_settings.fwd_escs:04b}')
45-
fwd_escs.reverse()
46-
vtol_escs = list(f'{self.warning_settings.vtol_escs:04b}')
47-
vtol_escs.reverse()
48-
self.debug(fwd_escs)
49-
self.debug(vtol_escs)
50-
51-
gps1 = self.master.messages.get("GPS_RAW_INT")
52-
gps2 = self.master.messages.get("GPS2_RAW")
53-
esc = self.master.messages.get("ESC_TELEMETRY_1_TO_4")
54-
vfr_hud = self.master.messages.get("VFR_HUD")
55-
terr = self.master.messages.get("TERRAIN_REPORT")
56-
nvf_as2 = self.master.messages.get("NAMED_VALUE_FLOAT")
57-
if nvf_as2 != None:
58-
nvf_as2 = nvf_as2['AS2']
59-
60-
if gps1 is None or gps2 is None:
61-
self.details = "No GPS1 and/or GPS2 message"
62-
self.warn("GPS")
63-
else:
64-
#check min visible sat
65-
if gps1.satellites_visible < self.warning_settings.min_sat:
66-
self.details = "GPS1 satellites less than " + str(self.warning_settings.min_sat)
67-
self.warn("GPS")
68-
69-
if gps2.satellites_visible < self.warning_settings.min_sat:
70-
self.details = "GPS2 satellites less than " + str(self.warning_settings.min_sat)
71-
self.warn("GPS")
72-
73-
#check difference in sat counts
74-
if abs(gps1.satellites_visible - gps2.satellites_visible) > self.warning_settings.sat_diff:
75-
self.details = "Satellite diff between GPS1 and GPS2 more than " + str(self.warning_settings.sat_diff)
76-
self.warn("GPS")
77-
78-
#check fwd esc rpms
79-
if esc is None:
80-
self.details = "No ESC message"
81-
self.warn("ESC")
82-
else:
83-
max_esc = 0
84-
min_esc = 1e9
85-
for no, act in enumerate(fwd_escs):
86-
if act == '1':
87-
self.debug("FWD ESC number: " + str(no))
88-
if esc.rpm[no] > max_esc:
89-
max_esc = esc.rpm[no]
90-
if esc.rpm[no] < min_esc:
91-
min_esc = esc.rpm[no]
92-
93-
if (max_esc - min_esc) > self.warning_settings.rpm_diff:
94-
self.details = "FWD ESC difference between max and min RPM more than " + str(self.warning_settings.rpm_diff)
95-
self.warn("ESC")
96-
97-
#check vtol esc rpms
98-
max_esc = 0
99-
min_esc = 1e9
100-
for no, act in enumerate(vtol_escs):
101-
if act == '1':
102-
self.debug("VTOL ESC number: " + str(no))
103-
if esc.rpm[no] > max_esc:
104-
max_esc = esc.rpm[no]
105-
if esc.rpm[no] < min_esc:
106-
min_esc = esc.rpm[no]
107-
108-
if (max_esc - min_esc) > self.warning_settings.rpm_diff:
109-
self.details = "VTOL ESC difference between max and min RPM more than " + str(self.warning_settings.rpm_diff)
110-
self.warn("ESC")
111-
112-
if not (self.sensor_enabled['AS'] and self.sensor_present['AS'] and self.sensor_healthy['AS']):
113-
self.details = "Airspeed sensor issue: Enabled: " + str(self.sensor_enabled['AS']) + ", Present: " + str(self.sensor_present['AS']) + ", Healthy: " + str(self.sensor_healthy['AS'])
114-
self.warn("Airspeed")
115-
116-
if vfr_hud is None or nvf_as2 is None:
117-
self.details = "No HUD and/or AS2 message"
118-
self.warn("Airspeed")
32+
if len(args) > 0:
33+
if args[0] == 'set':
34+
state.warning_settings.command(args[1:])
35+
if args[0] == 'details':
36+
print("warning: %s" % self.details)
11937
else:
38+
print('usage: warning set|details')
39+
40+
def get_esc_rpms(self):
41+
'''get a dictionary of ESC RPMs'''
42+
ret = {}
43+
esc_messages = [ ('ESC_TELEMETRY_1_TO_4', 1),
44+
('ESC_TELEMETRY_5_TO_8', 5),
45+
('ESC_TELEMETRY_9_TO_12', 9),
46+
('ESC_TELEMETRY_13_TO_16', 13) ]
47+
for (mname, base) in esc_messages:
48+
m = self.master.messages.get(mname, None)
49+
if m:
50+
for i in range(4):
51+
ret[base+i] = m.rpm[i]
52+
return ret
53+
54+
55+
def check_esc_group(self, group_mask, rpms):
56+
'''check one ESC group for consistency. Either all running or none running'''
57+
escs = []
58+
for i in range(16):
59+
if group_mask & (1<<i):
60+
escs.append(i+1)
61+
num_running = 0
62+
for e in escs:
63+
rpm = rpms.get(e,0)
64+
if rpm >= self.warning_settings.esc_min_rpm:
65+
num_running += 1
66+
return num_running != 0 and num_running != len(escs)
67+
68+
def check_escs(self):
69+
'''check for ESC consistency'''
70+
rpms = self.get_esc_rpms()
71+
if self.check_esc_group(self.warning_settings.esc_group1, rpms):
72+
self.details = "ESC group1 fail"
73+
return True
74+
if self.check_esc_group(self.warning_settings.esc_group2, rpms):
75+
self.details = "ESC group2 fail"
76+
return True
77+
return False
78+
79+
def check_gps(self):
80+
'''check GPS issues'''
81+
gps1 = self.master.messages.get("GPS_RAW_INT", None)
82+
gps2 = self.master.messages.get("GPS2_RAW", None)
83+
if gps1 and gps2:
84+
if abs(gps1.satellites_visible - gps2.satellites_visible) > self.warning_settings.sat_diff:
85+
self.details = "GPS sat diff"
86+
return True
87+
if gps1 and gps1.satellites_visible < self.warning_settings.min_sat:
88+
self.details = "GPS1 low sat count"
89+
return True
90+
if gps2 and gps2.satellites_visible < self.warning_settings.min_sat:
91+
self.details = "GPS2 low sat count"
92+
return True
93+
return False
94+
95+
def check_airspeed(self):
96+
'''check airspeed sensors'''
97+
vfr_hud = self.master.messages.get("VFR_HUD", None)
98+
nvf_as2 = self.master.messages.get("NAMED_VALUE_FLOAT[AS2]", None)
99+
if vfr_hud and nvf_as2:
120100
if abs(vfr_hud.airspeed - nvf_as2.value) > self.warning_settings.airspd_diff:
121-
self.details = "Airspeed 1 vs airspeed 2 difference greater than " + str(self.warning_settings.airspd_diff)
122-
self.warn("Airspeed")
123-
124-
if abs(vfr_hud.groundspeed - (vfr_hud.airspeed + nvf_as2.value)/2) > self.warning_settings.max_wind:
125-
self.details = "Airspeed vs groundspeed difference more than " + str(self.warning_settings.max_wind)
126-
self.warn("Airspeed")
127-
128-
if terr is None:
129-
self.details = "No terrain message."
130-
self.warn("Terrain")
101+
self.details = "Airspeed difference"
102+
return True
103+
return False
104+
105+
def check_all(self):
106+
if self.check_escs():
107+
self.failure = 'ESC'
108+
elif self.check_gps():
109+
self.failure = 'GPS'
110+
elif self.check_airspeed():
111+
self.failure = 'AIRSPEED'
112+
113+
def monitor(self):
114+
last_fail = self.failure
115+
self.failure = None
116+
self.check_all()
117+
if self.failure and self.failure != last_fail:
118+
self.warn(self.failure)
119+
if not self.failure:
120+
self.console.set_status('WARN', 'WARN:OK', fg='green', row=2)
131121
else:
132-
if terr.pending > 0:
133-
self.details = "Pending terrain items."
134-
self.warn("Terrain")
135-
136-
if terr.current_height > self.warning_settings.max_agl_alt:
137-
self.details = "AGL Alt more than " + str(self.warning_settings.max_agl_alt)
138-
self.warn("Terrain")
139-
140-
def update_status(self):
141-
sys_status = self.master.messages.get("SYS_STATUS")
142-
sensors = { 'AS' : mavutil.mavlink.MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE,
143-
'MAG' : mavutil.mavlink.MAV_SYS_STATUS_SENSOR_3D_MAG,
144-
'INS' : mavutil.mavlink.MAV_SYS_STATUS_SENSOR_3D_ACCEL | mavutil.mavlink.MAV_SYS_STATUS_SENSOR_3D_GYRO,
145-
'AHRS' : mavutil.mavlink.MAV_SYS_STATUS_AHRS,
146-
'RC' : mavutil.mavlink.MAV_SYS_STATUS_SENSOR_RC_RECEIVER,
147-
'TERR' : mavutil.mavlink.MAV_SYS_STATUS_TERRAIN,
148-
'RNG' : mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION,
149-
'LOG' : mavutil.mavlink.MAV_SYS_STATUS_LOGGING,
150-
'PRX' : mavutil.mavlink.MAV_SYS_STATUS_SENSOR_PROXIMITY,
151-
'PRE' : mavutil.mavlink.MAV_SYS_STATUS_PREARM_CHECK,
152-
'FLO' : mavutil.mavlink.MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW,
153-
}
154-
for s in sensors.keys():
155-
bits = sensors[s]
156-
self.sensor_present[s] = ((sys_status.onboard_control_sensors_present & bits) == bits)
157-
self.sensor_enabled[s] = ((sys_status.onboard_control_sensors_enabled & bits) == bits)
158-
self.sensor_healthy[s] = ((sys_status.onboard_control_sensors_health & bits) == bits)
159-
160-
def warn(self, err):
161-
self.say("Warning " + err + " failure")
162-
if self.warning_settings.details == True and self.details != "":
163-
self.console.writeln(self.details, fg = 'grey')
164-
self.details = ""
122+
self.console.set_status('WARN', 'WARN:%s' % self.failure, fg='red', row=2)
165123

166-
def debug(self, msg):
167-
if self.warning_settings.debug == True:
168-
self.console.writeln(msg, fg = 'blue')
124+
def warn(self, err):
125+
self.say("Warning " + err)
126+
self.warn_time = time.time()
169127

170128
def idle_task(self):
171129
'''called on idle'''
172130
now = time.time()
173-
if (now - self.prev_call) > self.warning_settings.check_freq:
174-
self.prev_call = now
175-
self.update_status()
131+
132+
# at 1Hz update status. If status changes warn immediately
133+
if (now - self.check_time) >= 1:
134+
self.check_time = now
176135
self.monitor()
177136

137+
# periodically announce the failure if any
138+
if (self.failure and
139+
self.warning_settings.warn_time > 0 and
140+
(now - self.warn_time) >= self.warning_settings.warn_time):
141+
self.warn(self.failure)
142+
178143

179144
def init(mpstate):
180145
'''initialise module'''

0 commit comments

Comments
 (0)