mrtautopilot 13__tar.gz → 14__tar.gz

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
@@ -1,6 +1,6 @@
1
1
  Metadata-Version: 2.1
2
2
  Name: mrtautopilot
3
- Version: 13
3
+ Version: 14
4
4
  Summary: Communicate with the MRT Autopilot Mavlink API
5
5
  Maintainer-email: Nathan Knotts <nathan@magothyrt.com>
6
6
  Project-URL: Homepage, https://www.magothyrt.com/
@@ -31,10 +31,11 @@ Example usage:
31
31
  import logging
32
32
  import selectors
33
33
 
34
- import mrtautopilot
35
-
36
34
  # example for send_protobuf_proxy
37
35
  from mrtproto import autopilot_pb2
36
+
37
+ import mrtautopilot
38
+
38
39
  PROTO_ID = 0x5433 # defined by protocol
39
40
 
40
41
 
@@ -47,7 +48,9 @@ def main():
47
48
 
48
49
  def got_low_bandwidth():
49
50
  d: mrtautopilot.LowBandwidth = mavlink.low_bandwidth_queue.get_nowait()
50
- logging.info(f"Got Vehicle Data: {d.latitude_deg}, {d.longitude_deg}")
51
+ logging.info(
52
+ f"Got Vehicle Data: {d.latitude_deg}, {d.longitude_deg}, has control: {mavlink.has_control()}"
53
+ )
51
54
 
52
55
  try:
53
56
  sel = selectors.DefaultSelector()
@@ -60,10 +63,14 @@ def main():
60
63
  count = 0
61
64
  mavlink.start()
62
65
  while True:
63
- mavlink.send_heartbeat()
66
+ if mavlink.has_control():
67
+ mavlink.send_heartbeat()
64
68
 
65
69
  count += 1
66
70
 
71
+ if count == 3:
72
+ mavlink.send_take_control()
73
+
67
74
  if count == 5:
68
75
  mavlink.send_protobuf_proxy(PROTO_ID, proto.SerializeToString())
69
76
 
@@ -98,6 +105,13 @@ def main():
98
105
  mavlink.set_motor_enablement(False)
99
106
  mavlink.send_autopilot_stop()
100
107
 
108
+ if count == 25:
109
+ mavlink.send_autopilot_manual_mode()
110
+
111
+ if count > 25:
112
+ # z = thrust, # r = rudder, 0=%0, 1000=100%
113
+ mavlink.send_manual_control(z=100, r=100)
114
+
101
115
  for key, mask in sel.select():
102
116
  key.data()
103
117
 
@@ -18,10 +18,11 @@ Example usage:
18
18
  import logging
19
19
  import selectors
20
20
 
21
- import mrtautopilot
22
-
23
21
  # example for send_protobuf_proxy
24
22
  from mrtproto import autopilot_pb2
23
+
24
+ import mrtautopilot
25
+
25
26
  PROTO_ID = 0x5433 # defined by protocol
26
27
 
27
28
 
@@ -34,7 +35,9 @@ def main():
34
35
 
35
36
  def got_low_bandwidth():
36
37
  d: mrtautopilot.LowBandwidth = mavlink.low_bandwidth_queue.get_nowait()
37
- logging.info(f"Got Vehicle Data: {d.latitude_deg}, {d.longitude_deg}")
38
+ logging.info(
39
+ f"Got Vehicle Data: {d.latitude_deg}, {d.longitude_deg}, has control: {mavlink.has_control()}"
40
+ )
38
41
 
39
42
  try:
40
43
  sel = selectors.DefaultSelector()
@@ -47,10 +50,14 @@ def main():
47
50
  count = 0
48
51
  mavlink.start()
49
52
  while True:
50
- mavlink.send_heartbeat()
53
+ if mavlink.has_control():
54
+ mavlink.send_heartbeat()
51
55
 
52
56
  count += 1
53
57
 
58
+ if count == 3:
59
+ mavlink.send_take_control()
60
+
54
61
  if count == 5:
55
62
  mavlink.send_protobuf_proxy(PROTO_ID, proto.SerializeToString())
56
63
 
@@ -85,6 +92,13 @@ def main():
85
92
  mavlink.set_motor_enablement(False)
86
93
  mavlink.send_autopilot_stop()
87
94
 
95
+ if count == 25:
96
+ mavlink.send_autopilot_manual_mode()
97
+
98
+ if count > 25:
99
+ # z = thrust, # r = rudder, 0=%0, 1000=100%
100
+ mavlink.send_manual_control(z=100, r=100)
101
+
88
102
  for key, mask in sel.select():
89
103
  key.data()
90
104
 
@@ -1,4 +1,4 @@
1
- __version__ = "13"
1
+ __version__ = "14"
2
2
 
3
3
  from .autopilot import *
4
4
  from .mission import *
@@ -1,21 +1,21 @@
1
- import time
2
- import threading
3
- import multiprocessing
4
- import socket
5
1
  import asyncio
6
2
  import datetime
7
3
  import logging
4
+ import multiprocessing
5
+ import random
6
+ import socket
8
7
  import struct
8
+ import threading
9
+ import time
9
10
  import uuid
10
11
  from dataclasses import dataclass
11
12
  from enum import Enum
12
- from typing import List, Union, Tuple, Callable
13
+ from typing import Callable, List, Tuple, Union
13
14
 
14
15
  import mrtmavlink
15
- from . import mission
16
-
17
16
  import zstandard
18
17
 
18
+ from . import mission
19
19
 
20
20
  SYSTEM_ID = 254 # system id for the mavlink thread
21
21
 
@@ -246,6 +246,7 @@ class LowBandwidth:
246
246
  break
247
247
 
248
248
  assert self.fault_response is not None
249
+ self.gcs_set_mode_uid = msg.gcs_set_mode_uuid_lsb
249
250
 
250
251
  autopilot_mode: AutopilotMode
251
252
  main_mode: MagothyCustomMainMode
@@ -266,6 +267,7 @@ class LowBandwidth:
266
267
  position_error_m: Union[float, None]
267
268
  triggered_health_items: List[HealthItem]
268
269
  fault_response: Union[HealthResponse, None]
270
+ gcs_set_mode_uid: int
269
271
 
270
272
 
271
273
  class GpsFixType(Enum):
@@ -449,6 +451,15 @@ class ParamValue:
449
451
  num_param: int
450
452
 
451
453
 
454
+ def random_uint32():
455
+ return random.randint(0, 1 << 32 - 1)
456
+
457
+
458
+ def uint32_to_float(value: int) -> float:
459
+ packed_bytes = struct.pack("<I", value)
460
+ return struct.unpack("<f", packed_bytes)[0]
461
+
462
+
452
463
  class MavlinkThread:
453
464
  def __init__(
454
465
  self,
@@ -461,6 +472,9 @@ class MavlinkThread:
461
472
  self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
462
473
  self.sock.setblocking(False)
463
474
  self.msg_callback: Callable[[mrtmavlink.MAVLink_message], None] = lambda x: None
475
+ self.uid = random_uint32()
476
+ self.has_control_lock = threading.Lock()
477
+ self.has_control_var = False
464
478
 
465
479
  if multicast_group:
466
480
  self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
@@ -567,6 +581,12 @@ class MavlinkThread:
567
581
  ):
568
582
  self.msg_callback = callback
569
583
 
584
+ def has_control(self) -> bool:
585
+ control = False
586
+ with self.has_control_lock:
587
+ control = self.has_control_var
588
+ return control
589
+
570
590
  def _msg_callback(self, msg: mrtmavlink.MAVLink_message):
571
591
  self.msg_callback(msg)
572
592
  sys_id = msg.get_srcSystem()
@@ -580,6 +600,10 @@ class MavlinkThread:
580
600
 
581
601
  if not self.low_bandwidth_queue.full():
582
602
  self.low_bandwidth_queue.put_nowait(data)
603
+
604
+ control = self.uid == data.gcs_set_mode_uid
605
+ with self.has_control_lock:
606
+ self.has_control_var = control
583
607
  elif type(msg) is mrtmavlink.MAVLink_statustext_message:
584
608
  if (
585
609
  msg.severity == mrtmavlink.MAV_SEVERITY_CRITICAL
@@ -796,6 +820,10 @@ class MavlinkThread:
796
820
  self.conn.system_time_send(int(time.time() * 1e6), 0)
797
821
 
798
822
  def _send_param_request_list(self):
823
+ if self.system_id is None:
824
+ logging.warning("System ID not set, not requesting parameters")
825
+ return
826
+
799
827
  self.conn.param_request_list_send(
800
828
  self.system_id, # target_system
801
829
  mrtmavlink.MAV_COMP_ID_AUTOPILOT1, # target_component
@@ -841,14 +869,17 @@ class MavlinkThread:
841
869
 
842
870
  callback(params)
843
871
 
844
- def send_autopilot_stop(self):
845
- self.loop.call_soon_threadsafe(lambda: self._send_autopilot_stop())
872
+ def _uuid(self) -> List[float]:
873
+ uid = uuid.uuid4()
874
+ uuid_params = list(struct.unpack("<ffff", uid.bytes))
875
+ uuid_params[0] = uint32_to_float(self.uid)
876
+ return uuid_params
846
877
 
847
- def _send_autopilot_stop(self):
848
- if self.system_id is None:
849
- logging.warning("System ID not set, not sending STOP")
878
+ def _set_mode(self, main_mode: int, sub_mode: int):
879
+ sys_id = self.system_id
880
+ if sys_id is None:
881
+ logging.warning("System ID not set, not sending SET_MODE")
850
882
  return
851
- logging.info("Sending STOP")
852
883
 
853
884
  # custom_mode is a uint32
854
885
  # param2 is a float
@@ -857,25 +888,45 @@ class MavlinkThread:
857
888
  [
858
889
  0,
859
890
  0,
860
- MagothyCustomMainMode.GUIDED.value,
861
- MagothyCustomSubModeGuided.READY.value,
891
+ main_mode,
892
+ sub_mode,
862
893
  ]
863
894
  )
864
895
 
896
+ uuid_params = self._uuid()
865
897
  self.conn.command_long_send(
866
- self.system_id, # target_system
898
+ sys_id, # target_system
867
899
  mrtmavlink.MAV_COMP_ID_AUTOPILOT1, # target_component
868
900
  mrtmavlink.MAV_CMD_DO_SET_MODE, # command
869
901
  0, # confirmation
870
902
  0, # param1
871
- struct.unpack("f", custom_mode)[0], # param2 (custom_mode)
903
+ struct.unpack("<f", custom_mode)[0], # param2 (custom_mode)
872
904
  0, # param3
873
- 0, # param4
874
- 0, # param5
875
- 0, # param6
876
- 0, # param7
905
+ uuid_params[0], # param4 // UUID LSB
906
+ uuid_params[1], # param5 // UUID
907
+ uuid_params[2], # param6 // UUID
908
+ uuid_params[3], # param7 // UUID MSB
909
+ )
910
+
911
+ def send_autopilot_stop(self):
912
+ self.loop.call_soon_threadsafe(lambda: self._send_autopilot_stop())
913
+
914
+ def _send_autopilot_stop(self):
915
+ logging.info("Sending STOP")
916
+ self._set_mode(
917
+ MagothyCustomMainMode.GUIDED.value, MagothyCustomSubModeGuided.READY.value
877
918
  )
878
919
 
920
+ def send_take_control(self):
921
+ self.send_autopilot_stop()
922
+
923
+ def send_autopilot_manual_mode(self):
924
+ self.loop.call_soon_threadsafe(lambda: self._send_autopilot_manual_mode())
925
+
926
+ def _send_autopilot_manual_mode(self):
927
+ logging.info("Sending MANUAL MODE")
928
+ self._set_mode(MagothyCustomMainMode.MANUAL.value, 0)
929
+
879
930
  def send_waypoint(self, lat_deg: float, lon_deg: float, speed_mps: float):
880
931
  self.loop.call_soon_threadsafe(
881
932
  lambda: self._send_waypoint(lat_deg, lon_deg, speed_mps)
@@ -889,18 +940,19 @@ class MavlinkThread:
889
940
  f"Sending Waypoint to lat {lat_deg}°, lon {lon_deg}°, speed {speed_mps} m/s"
890
941
  )
891
942
 
943
+ uuid_params = self._uuid()
892
944
  self.conn.command_long_send(
893
945
  self.system_id, # target_system
894
946
  mrtmavlink.MAV_COMP_ID_AUTOPILOT1, # target_component
895
947
  mrtmavlink.MAV_CMD_WAYPOINT_USER_1, # command
896
948
  0, # confirmation
897
- 0, # param1
949
+ uuid_params[1], # param1
898
950
  speed_mps, # param2 (speed_mps)
899
951
  0, # param3
900
952
  0, # param4
901
953
  lat_deg * 1e7, # param5 (latitude_deg * 1e7)
902
954
  lon_deg * 1e7, # param6 (longitude_deg * 1e7)
903
- 0, # param7
955
+ uuid_params[0], # param7
904
956
  )
905
957
 
906
958
  def send_loiter(self, speed_mps: float, radius_m: float, duration_s: float):
@@ -916,6 +968,7 @@ class MavlinkThread:
916
968
  f"Sending Loiter: speed {speed_mps} m/s, radius {radius_m} m, duration {duration_s} s"
917
969
  )
918
970
 
971
+ uuid_params = self._uuid()
919
972
  self.conn.command_long_send(
920
973
  self.system_id, # target_system
921
974
  mrtmavlink.MAV_COMP_ID_AUTOPILOT1, # target_component
@@ -924,10 +977,10 @@ class MavlinkThread:
924
977
  duration_s, # param1 - duration (seconds)
925
978
  speed_mps, # param2 - speed (m/s)
926
979
  radius_m, # param3 - radiums (m)
927
- 0, # param4 - unused
980
+ uuid_params[0], # param4 - uuid 1/4
928
981
  float("nan"), # param5 (latitude_deg * 1e7)
929
982
  float("nan"), # param6 (longitude_deg * 1e7)
930
- 0, # param7 - unused
983
+ uuid_params[1], # param7 - uuid 2/4
931
984
  )
932
985
 
933
986
  def send_protobuf_proxy(self, proto_id: int, buf: bytes):
@@ -1037,9 +1090,10 @@ class MavlinkThread:
1037
1090
 
1038
1091
  logging.info("Mission upload successful, starting mission")
1039
1092
 
1040
- uid = uuid.uuid4()
1041
- uuid_params = struct.unpack("<ffff", uid.bytes)
1042
- self.expected_cmd_uuid = struct.unpack("<iiii", uid.bytes)[0]
1093
+ uuid_params = self._uuid()
1094
+ self.expected_cmd_uuid = struct.unpack(
1095
+ "<iiii", struct.pack("<ffff", *uuid_params)
1096
+ )[0]
1043
1097
 
1044
1098
  # we have successfully sent the mission, now start it
1045
1099
  cmd = mrtmavlink.MAVLink_command_long_message(
@@ -1121,3 +1175,47 @@ class MavlinkThread:
1121
1175
  pitch_rate_rps,
1122
1176
  yaw_rate_rps,
1123
1177
  )
1178
+
1179
+ def send_manual_control(
1180
+ self,
1181
+ x: int = 0x7FFF,
1182
+ y: int = 0x7FFF,
1183
+ z: int = 0x7FFF,
1184
+ r: int = 0x7FFF,
1185
+ s: int = 0x7FFF,
1186
+ t: int = 0x7FFF,
1187
+ aux1: int = 0x7FFF,
1188
+ aux2: int = 0x7FFF,
1189
+ aux3: int = 0x7FFF,
1190
+ aux4: int = 0x7FFF,
1191
+ aux5: int = 0x7FFF,
1192
+ aux6: int = 0x7FFF,
1193
+ enabled_extensions: int = 0,
1194
+ buttons: int = 0,
1195
+ buttons2: int = 0,
1196
+ ):
1197
+ sys_id = self.system_id
1198
+ if sys_id is None:
1199
+ logging.warning("System ID not set, not sending manual control")
1200
+ return
1201
+
1202
+ self.loop.call_soon_threadsafe(
1203
+ lambda: self.conn.manual_control_send(
1204
+ target=sys_id,
1205
+ x=x,
1206
+ y=y,
1207
+ z=z,
1208
+ r=r,
1209
+ s=s,
1210
+ t=t,
1211
+ buttons=buttons,
1212
+ buttons2=buttons2,
1213
+ aux1=aux1,
1214
+ aux2=aux2,
1215
+ aux3=aux3,
1216
+ aux4=aux4,
1217
+ aux5=aux5,
1218
+ aux6=aux6,
1219
+ enabled_extensions=enabled_extensions,
1220
+ )
1221
+ )
@@ -1,6 +1,6 @@
1
1
  Metadata-Version: 2.1
2
2
  Name: mrtautopilot
3
- Version: 13
3
+ Version: 14
4
4
  Summary: Communicate with the MRT Autopilot Mavlink API
5
5
  Maintainer-email: Nathan Knotts <nathan@magothyrt.com>
6
6
  Project-URL: Homepage, https://www.magothyrt.com/
@@ -31,10 +31,11 @@ Example usage:
31
31
  import logging
32
32
  import selectors
33
33
 
34
- import mrtautopilot
35
-
36
34
  # example for send_protobuf_proxy
37
35
  from mrtproto import autopilot_pb2
36
+
37
+ import mrtautopilot
38
+
38
39
  PROTO_ID = 0x5433 # defined by protocol
39
40
 
40
41
 
@@ -47,7 +48,9 @@ def main():
47
48
 
48
49
  def got_low_bandwidth():
49
50
  d: mrtautopilot.LowBandwidth = mavlink.low_bandwidth_queue.get_nowait()
50
- logging.info(f"Got Vehicle Data: {d.latitude_deg}, {d.longitude_deg}")
51
+ logging.info(
52
+ f"Got Vehicle Data: {d.latitude_deg}, {d.longitude_deg}, has control: {mavlink.has_control()}"
53
+ )
51
54
 
52
55
  try:
53
56
  sel = selectors.DefaultSelector()
@@ -60,10 +63,14 @@ def main():
60
63
  count = 0
61
64
  mavlink.start()
62
65
  while True:
63
- mavlink.send_heartbeat()
66
+ if mavlink.has_control():
67
+ mavlink.send_heartbeat()
64
68
 
65
69
  count += 1
66
70
 
71
+ if count == 3:
72
+ mavlink.send_take_control()
73
+
67
74
  if count == 5:
68
75
  mavlink.send_protobuf_proxy(PROTO_ID, proto.SerializeToString())
69
76
 
@@ -98,6 +105,13 @@ def main():
98
105
  mavlink.set_motor_enablement(False)
99
106
  mavlink.send_autopilot_stop()
100
107
 
108
+ if count == 25:
109
+ mavlink.send_autopilot_manual_mode()
110
+
111
+ if count > 25:
112
+ # z = thrust, # r = rudder, 0=%0, 1000=100%
113
+ mavlink.send_manual_control(z=100, r=100)
114
+
101
115
  for key, mask in sel.select():
102
116
  key.data()
103
117
 
File without changes
File without changes