Skip to content

Commit 78a5520

Browse files
peterbarkerpeterbarker
committed
Add operator_control module
1 parent 521ba5b commit 78a5520

2 files changed

Lines changed: 182 additions & 0 deletions

File tree

‎MAVProxy/modules/mavproxy_operator_control/__init__.py‎

Lines changed: 181 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,181 @@
1+
#!/usr/bin/env python3
2+
'''
3+
MAVProxy operator control module.
4+
5+
Implements the GCS operator control protocol using
6+
MAV_CMD_REQUEST_OPERATOR_CONTROL and CONTROL_STATUS.
7+
8+
AP_FLAKE8_CLEAN
9+
'''
10+
11+
from MAVProxy.modules.lib import mp_module
12+
from pymavlink import mavutil
13+
14+
MAV_CMD_REQUEST_OPERATOR_CONTROL = 32100
15+
GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER = 1
16+
GCS_CONTROL_STATUS_FLAGS_TAKEOVER_ALLOWED = 2
17+
18+
19+
class OperatorControlModule(mp_module.MPModule):
20+
def __init__(self, mpstate):
21+
super(OperatorControlModule, self).__init__(
22+
mpstate, "operator_control", "GCS operator control", public=True)
23+
self.manager_system = None
24+
self.manager_component = None
25+
self.control_status = None
26+
self._last_gcs_main = None
27+
self._last_flags = None
28+
29+
self.add_command('oc', self.cmd_oc, 'operator control commands', [
30+
'request [allow_takeover=0|1]',
31+
'release',
32+
'grant',
33+
'deny',
34+
'status',
35+
])
36+
37+
def _flags_str(self, flags):
38+
parts = []
39+
if flags & GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER:
40+
parts.append('SYSTEM_MANAGER')
41+
if flags & GCS_CONTROL_STATUS_FLAGS_TAKEOVER_ALLOWED:
42+
parts.append('TAKEOVER_ALLOWED')
43+
return '|'.join(parts) if parts else 'none'
44+
45+
def _result_str(self, result):
46+
result_map = {
47+
mavutil.mavlink.MAV_RESULT_ACCEPTED: 'ACCEPTED',
48+
mavutil.mavlink.MAV_RESULT_TEMPORARILY_REJECTED: 'TEMPORARILY_REJECTED',
49+
mavutil.mavlink.MAV_RESULT_DENIED: 'DENIED',
50+
mavutil.mavlink.MAV_RESULT_UNSUPPORTED: 'UNSUPPORTED',
51+
mavutil.mavlink.MAV_RESULT_FAILED: 'FAILED',
52+
mavutil.mavlink.MAV_RESULT_IN_PROGRESS: 'IN_PROGRESS',
53+
}
54+
return result_map.get(result, 'UNKNOWN(%d)' % result)
55+
56+
def _parse_named_args(self, args, valid_keys):
57+
parsed = {}
58+
for arg in args:
59+
if '=' not in arg:
60+
print("oc: expected key=value, got: %s" % arg)
61+
return None
62+
key, val = arg.split('=', 1)
63+
if key not in valid_keys:
64+
print("oc: unknown parameter '%s'" % key)
65+
return None
66+
try:
67+
parsed[key] = int(val)
68+
except ValueError:
69+
print("oc: '%s' must be an integer" % key)
70+
return None
71+
return parsed
72+
73+
def cmd_oc(self, args):
74+
'''operator control command'''
75+
usage = "Usage: oc <request|release|status>"
76+
if len(args) == 0:
77+
print(usage)
78+
return
79+
if args[0] == 'request':
80+
self._cmd_request(args[1:])
81+
elif args[0] == 'release':
82+
self._cmd_release()
83+
elif args[0] == 'grant':
84+
self._cmd_grant()
85+
elif args[0] == 'deny':
86+
self._cmd_request([])
87+
elif args[0] == 'status':
88+
self._cmd_status()
89+
else:
90+
print(usage)
91+
92+
def _cmd_request(self, args):
93+
if self.manager_system is None:
94+
print("oc: no system manager found yet (waiting for CONTROL_STATUS)")
95+
return
96+
parsed = self._parse_named_args(args, {'allow_takeover'})
97+
if parsed is None:
98+
return
99+
allow_takeover = parsed.get('allow_takeover', 0)
100+
self.master.mav.command_long_send(
101+
self.manager_system,
102+
self.manager_component,
103+
MAV_CMD_REQUEST_OPERATOR_CONTROL,
104+
0,
105+
1.0,
106+
float(allow_takeover),
107+
10.0,
108+
float(self.settings.source_system),
109+
0.0, 0.0, 0.0)
110+
print("oc: requesting control (manager sysid=%d compid=%d allow_takeover=%d)" %
111+
(self.manager_system, self.manager_component, allow_takeover))
112+
113+
def _cmd_grant(self):
114+
'''re-request control with allow_takeover=1, giving the pending requester a 10s window'''
115+
self._cmd_request(['allow_takeover=1'])
116+
117+
def _cmd_release(self):
118+
if self.manager_system is None:
119+
print("oc: no system manager found yet (waiting for CONTROL_STATUS)")
120+
return
121+
self.master.mav.command_long_send(
122+
self.manager_system,
123+
self.manager_component,
124+
MAV_CMD_REQUEST_OPERATOR_CONTROL,
125+
0,
126+
0.0, 0.0, 0.0,
127+
float(self.settings.source_system),
128+
0.0, 0.0, 0.0)
129+
print("oc: releasing control")
130+
131+
def _cmd_status(self):
132+
if self.control_status is None:
133+
print("oc: no CONTROL_STATUS received yet")
134+
return
135+
m = self.control_status
136+
secondary = [x for x in m.gcs_secondary if x != 0]
137+
print("oc status: manager=sysid:%d/compid:%d flags=%s gcs_main=%d gcs_secondary=%s" % (
138+
self.manager_system, self.manager_component,
139+
self._flags_str(m.flags),
140+
m.gcs_main,
141+
str(secondary) if secondary else 'none'))
142+
143+
def mavlink_packet(self, m):
144+
mtype = m.get_type()
145+
146+
if mtype == 'CONTROL_STATUS':
147+
if not (m.flags & GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER):
148+
return
149+
self.manager_system = m.get_srcSystem()
150+
self.manager_component = m.get_srcComponent()
151+
self.control_status = m
152+
if m.gcs_main != self._last_gcs_main or m.flags != self._last_flags:
153+
self._last_gcs_main = m.gcs_main
154+
self._last_flags = m.flags
155+
secondary = [x for x in m.gcs_secondary if x != 0]
156+
takeover = bool(m.flags & GCS_CONTROL_STATUS_FLAGS_TAKEOVER_ALLOWED)
157+
print("oc: CONTROL_STATUS gcs_main=%d secondary=%s takeover_allowed=%s" % (
158+
m.gcs_main,
159+
str(secondary) if secondary else 'none',
160+
takeover))
161+
162+
elif mtype == 'COMMAND_ACK':
163+
if m.command == MAV_CMD_REQUEST_OPERATOR_CONTROL:
164+
print("oc: REQUEST_OPERATOR_CONTROL ack: %s" % self._result_str(m.result))
165+
166+
elif mtype == 'COMMAND_LONG':
167+
if (m.command == MAV_CMD_REQUEST_OPERATOR_CONTROL and
168+
m.target_system == self.settings.source_system):
169+
requester = int(m.param4)
170+
print("oc: takeover request from GCS sysid=%d (use 'oc grant', 'oc deny', or 'oc release')" % requester)
171+
self.master.mav.command_ack_send(
172+
MAV_CMD_REQUEST_OPERATOR_CONTROL,
173+
mavutil.mavlink.MAV_RESULT_ACCEPTED,
174+
255, 0,
175+
m.get_srcSystem(),
176+
m.get_srcComponent())
177+
178+
179+
def init(mpstate):
180+
'''initialise module'''
181+
return OperatorControlModule(mpstate)

‎setup.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -90,6 +90,7 @@ def package_files(directory):
9090
'MAVProxy.modules.mavproxy_nokov',
9191
'MAVProxy.modules.mavproxy_SIYI',
9292
'MAVProxy.modules.mavproxy_chat',
93+
'MAVProxy.modules.mavproxy_operator_control',
9394
'MAVProxy.modules.lib',
9495
'MAVProxy.modules.lib.ANUGA',
9596
'MAVProxy.modules.lib.MacOS',

0 commit comments

Comments
 (0)