pyMAVLinCS 1.0.4__tar.gz → 1.1.0__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.4
2
2
  Name: pyMAVLinCS
3
- Version: 1.0.4
3
+ Version: 1.1.0
4
4
  Summary: Python API for autonomous drone control based on pymavlink
5
5
  Author-email: Noah Redon <redonnoah.cs@gmail.com>
6
6
  License: GPL-3.0-or-later
@@ -23,55 +23,62 @@ class MAVLinCS:
23
23
  """Allows connection to one or more flight controllers on one link."""
24
24
  def __init__(self,
25
25
  address: str,
26
+ baud: int = 57600,
26
27
  source_system: int = 255,
27
28
  source_component: int = mavutil.mavlink.MAV_COMP_ID_MISSIONPLANNER,
28
29
  target_system: int|None = None,
29
- baud: int = 57600,
30
- timeout_heartbeat: float|None = None,
31
- send_automatic_timesync: bool = True,
32
- timesync_delay: int = 3,
33
- send_automatic_home_request: bool = True,
34
- home_request_delay: int = 3,
35
- sysid_to_request_home: None|int|set|list[int] = None,
36
- pos_rate: float = 4,
37
30
  dialect: str = "ardupilotmega",
31
+ mcc_class = MCC,
38
32
  logger: logging.Logger = default_logger,
39
33
  mavlogfile: str | None = 'mav.tlog',
40
34
  relative_path_to_cwd: bool = True,
41
- command_ack_to_hide: None|int|set|list[int] = None,
42
- mcc_class = MCC
35
+ **kwargs
43
36
  ):
44
37
  """Initializes the connection.
45
38
 
46
39
  Args:
47
40
  address (str): Connection address.
41
+ baud (int): Connection baudrate.
48
42
  source_system (int): Ground station MAVLink system ID.
49
43
  source_component (int): Ground station MAV_COMPONENT.
50
44
  target_system (int|None): MAVLink system ID of the target flight controller.
51
- baud (int): Connection baudrate.
52
- timeout_heartbeat (float): Timeout (in seconds) while waiting for a HEARTBEAT before considering the connection impossible (raises an Exception).
53
- send_automatic_timesync (bool): Whether to send TIMESYNC messages at regular intervals.
54
- timesync_delay (int): Delay (in seconds) between two automatic TIMESYNC messages.
55
- send_automatic_home_request (bool): Whether to request the HOME position at regular intervals.
56
- home_request_delay (int): Delay (in seconds) between two automatic HOME position requests.
57
- sysid_to_request_home (None|int|set|list[int]): List of system IDs to which HOME position requests should be sent regularly.
58
- pos_rate (float): Rate at which the flight controller sends position updates to the ground station.
59
45
  dialect (str): MAVLink dialect, essential if timeout_heartbeat == 0. 'ardupilotmega' for ArduPilot, 'common' for PX4.
46
+ mcc_class: MCC base class used in your code (gives access to your custom MCCs).
60
47
  logger (logging.Logger): Logger to use.
61
48
  mavlogfile (str|None): Filename for MAVLink message logging.
62
49
  relative_path_to_cwd (bool): Whether the tlog path is relative to the current working directory.
63
- command_ack_to_hide (None|int|set|list[int]|): List of COMMAND_ACK (MAV_CMD) to show only in debug mode.
64
- mcc_class: MCC base class used in your code (gives access to your custom MCCs).
50
+ **timeout_heartbeat (float): Timeout (in seconds) while waiting for a HEARTBEAT before considering the connection impossible (raises an Exception). Default: None.
51
+ **pos_rate (float): Rate at which the flight controller sends position updates to the ground station. Default: 4.
52
+ **send_automatic_timesync (bool): Whether to send TIMESYNC messages at regular intervals. Default: False.
53
+ **timesync_delay (int): Delay (in seconds) between two automatic TIMESYNC messages. Default: 3.
54
+ **send_automatic_home_request (bool): Whether to request the HOME position at regular intervals. Default: True.
55
+ **home_request_delay (int): Delay (in seconds) between two automatic HOME position requests. Default: 3.
56
+ **sysid_to_request_home (None|int|set|list[int]): List of system IDs to which HOME position requests should be sent regularly. Default: None.
57
+ **command_ack_to_hide (None|int|set|list[int]|): List of COMMAND_ACK (MAV_CMD) to show only in debug mode. Default: None.
65
58
 
66
59
  Note:
67
60
  - If target_system is None, the MAVLink ID of the first detected vehicle is used as target.
68
- - If timeout_heartbeat is None, blocking wait until a valid HEARTBEAT is received (does not raise an Exception).
69
- - If timeout_heartbeat is 0, no HEARTBEAT wait is performed. Requires the correct MAVLink dialect and assumes MAVLink 2.
70
- - If sysid_to_request_home is None, HOME position requests are sent only to the target flight controller.
71
61
  - If mavlogfile is None, no .tlog file will be generated.
72
- - If pos_rate<=0, doesn't request position.
62
+ - If **timeout_heartbeat is None, blocking wait until a valid HEARTBEAT is received (does not raise an Exception).
63
+ - If **timeout_heartbeat is 0, no HEARTBEAT wait is performed. Requires the correct MAVLink dialect and assumes MAVLink 2.
64
+ - If **sysid_to_request_home is None, HOME position requests are sent only to the target flight controller.
65
+ - If **pos_rate<=0, doesn't request position.
73
66
  """
74
67
  logger.debug("Initializing MAVLinCS..")
68
+
69
+ # ------
70
+ # kwargs
71
+ # ------
72
+
73
+ timeout_heartbeat = kwargs.get('timeout_heartbeat', None)
74
+ pos_rate = kwargs.get('pos_rate', 4)
75
+ send_automatic_timesync = kwargs.get('send_automatic_timesync', False)
76
+ timesync_delay = kwargs.get('timesync_delay', 3)
77
+ send_automatic_home_request = kwargs.get('send_automatic_home_request', True)
78
+ home_request_delay = kwargs.get('home_request_delay', 3)
79
+ sysid_to_request_home = kwargs.get('sysid_to_request_home', None)
80
+ command_ack_to_hide = kwargs.get('command_ack_to_hide', None)
81
+
75
82
  # -----------------------
76
83
  # User-provided variables
77
84
  # -----------------------
@@ -145,13 +152,13 @@ class MAVLinCS:
145
152
  # Open connection
146
153
  self.open(
147
154
  address=address,
155
+ baud=baud,
148
156
  source_system=source_system,
149
157
  source_component=source_component,
150
158
  target_system=target_system,
151
- baud=baud,
159
+ dialect=dialect,
152
160
  timeout_heartbeat=timeout_heartbeat,
153
- pos_rate=pos_rate,
154
- dialect=dialect
161
+ pos_rate=pos_rate
155
162
  )
156
163
 
157
164
  # ----------------------------------------------
@@ -208,7 +215,7 @@ class MAVLinCS:
208
215
 
209
216
  Note:
210
217
  All returned values are NaN if no data has been received.
211
- """
218
+ """
212
219
  m = self.get_msg(
213
220
  msg_type="GLOBAL_POSITION_INT",
214
221
  source_system=source_system
@@ -830,21 +837,22 @@ class MAVLinCS:
830
837
  def set_servo(self,
831
838
  servo_channel: int,
832
839
  pwm: int,
833
- timeout_ack: float|None = 2,
834
- target_system: int|None = None
840
+ target_system: int|None = None,
841
+ **kwargs
835
842
  ) -> bool:
836
843
  """Sets the position of a servo.
837
844
 
838
845
  Args:
839
846
  servo_channel (int): Servo channel.
840
847
  pwm (int): PWM value (between 1000 and 2000. 1000=closed, 2000=open).
841
- timeout_ack (float|None): Acknowledgment wait time.
842
848
  target_system (int|None): Target MAVLink system ID for the command. Defaults to the master's ID.
849
+ **timeout_ack (float|None): Acknowledgment wait time. Default: 2.
843
850
 
844
851
  Returns:
845
852
  bool: Result of the operation.
846
853
  """
847
854
  self.logger.debug("Setting servo..")
855
+ timeout_ack = kwargs.get('timeout_ack', 2)
848
856
  if self.mission_ended:
849
857
  self.logger.critical("Mission stopped: Impossible to set servo")
850
858
  return False
@@ -871,8 +879,8 @@ class MAVLinCS:
871
879
  )
872
880
  accepted = self.wait_command_ack(
873
881
  command=mavutil.mavlink.MAV_CMD_DO_SET_SERVO,
874
- timeout=timeout_ack,
875
- source_system=target_system
882
+ source_system=target_system,
883
+ timeout=timeout_ack
876
884
  )
877
885
  if accepted:
878
886
  self.logger.info("Servo set")
@@ -889,7 +897,7 @@ class MAVLinCS:
889
897
  latitude: float,
890
898
  longitude: float,
891
899
  altitude: float,
892
- timeout_ack: float = 3
900
+ **kwargs
893
901
  ) -> bool:
894
902
  """Sends a target GPS position.
895
903
 
@@ -897,13 +905,14 @@ class MAVLinCS:
897
905
  latitude (float): Latitude in degrees.
898
906
  longitude (float): Longitude in degrees.
899
907
  altitude (float): Absolute altitude (MSL) in meters.
900
- timeout_ack (float): ACK wait timeout in seconds.
908
+ **timeout_ack (float): ACK wait timeout in seconds. Default: 3.
901
909
 
902
910
  Returns:
903
911
  bool: Result of the operation.
904
912
  """
905
913
  # Function not tested but should work by changing the gimbal mode (function to implement)
906
914
  self.logger.debug("Setting Gimbal Target..")
915
+ timeout_ack = kwargs.get('timeout_ack', 3)
907
916
  if self.mission_ended:
908
917
  self.logger.critical("Mission stopped: Impossible to set Gimbal target")
909
918
  return False
@@ -941,24 +950,25 @@ class MAVLinCS:
941
950
  pitch_rate: float = 0.0, yaw_rate: float = 0.0,
942
951
  lock_pitch: bool = True, lock_yaw: bool = True,
943
952
  yaw_in_earth_frame: bool = True,
944
- timeout_ack: float = 3.0
953
+ **kwargs
945
954
  ) -> bool:
946
955
  """Orients the gimbal.
947
956
 
948
957
  Args:
949
- pitch (float): Pitch angle (rad).
950
- yaw (float): Yaw angle (rad).
951
- pitch_rate (float): Pitch rotation speed (rad/s).
952
- yaw_rate (float): Yaw rotation speed (rad/s).
958
+ pitch (float): Pitch angle (deg).
959
+ yaw (float): Yaw angle (deg).
960
+ pitch_rate (float): Pitch rotation speed (deg/s).
961
+ yaw_rate (float): Yaw rotation speed (deg/s).
953
962
  lock_pitch (bool): True = lock pitch relative to the horizon.
954
963
  lock_yaw (bool): True = lock yaw relative to North (earth frame).
955
964
  yaw_in_earth_frame (bool): True = yaw expressed in Earth frame, otherwise vehicle frame.
956
- timeout_ack (float): Maximum wait time for ACK (seconds).
965
+ **timeout_ack (float): Maximum wait time for ACK (seconds). Default: 3.
957
966
 
958
967
  Returns:
959
968
  bool: True if ACK received (command accepted), False otherwise.
960
969
  """
961
970
  self.logger.debug("Setting Gimbal Angles..")
971
+ timeout_ack = kwargs.get('timeout_ack', 3)
962
972
  if self.mission_ended:
963
973
  self.logger.critical("Mission stopped: Impossible to set Gimbal angles")
964
974
  return False
@@ -987,7 +997,7 @@ class MAVLinCS:
987
997
  self.master.target_system,
988
998
  self.master.target_component,
989
999
  mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW,
990
- 1, pitch, yaw, pitch_rate, yaw_rate, # Speeds (rad/s)
1000
+ 1, pitch, yaw, pitch_rate, yaw_rate, # Speeds (deg/s)
991
1001
  flags, 0, 0
992
1002
  )
993
1003
  accepted = self.wait_command_ack(
@@ -1005,16 +1015,17 @@ class MAVLinCS:
1005
1015
  self.logger.error("Gimbal Angles setting failed")
1006
1016
  return bool(accepted)
1007
1017
 
1008
- def set_gimbal_retract(self, timeout_ack: float = 3.0) -> bool:
1018
+ def set_gimbal_retract(self, **kwargs) -> bool:
1009
1019
  """Sets the gimbal to retract position (no stabilization).
1010
1020
 
1011
1021
  Args:
1012
- timeout_ack (float): Maximum wait time for ACK (seconds).
1022
+ **timeout_ack (float): Maximum wait time for ACK (seconds). Default: 3.
1013
1023
 
1014
1024
  Returns:
1015
1025
  bool: True if ACK received (command accepted), False otherwise.
1016
1026
  """
1017
1027
  self.logger.debug("Setting Gimbal Retract..")
1028
+ timeout_ack = kwargs.get('timeout_ack', 3)
1018
1029
  if self.mission_ended:
1019
1030
  self.logger.critical("Mission stopped: Impossible to set Gimbal Retract")
1020
1031
  return False
@@ -1047,16 +1058,17 @@ class MAVLinCS:
1047
1058
  self.logger.error("Gimbal Retract setting failed")
1048
1059
  return bool(accepted)
1049
1060
 
1050
- def set_gimbal_neutral(self, timeout_ack: float = 3.0) -> bool:
1061
+ def set_gimbal_neutral(self, **kwargs) -> bool:
1051
1062
  """Sets the gimbal to neutral position (roll=pitch=yaw=0).
1052
1063
 
1053
1064
  Args:
1054
- timeout_ack (float): Maximum wait time for ACK (seconds).
1065
+ **timeout_ack (float): Maximum wait time for ACK (seconds). Default: 3.
1055
1066
 
1056
1067
  Returns:
1057
1068
  bool: True if ACK received (command accepted), False otherwise.
1058
1069
  """
1059
1070
  self.logger.debug("Setting Gimbal Neutral..")
1071
+ timeout_ack = kwargs.get('timeout_ack', 3)
1060
1072
  if self.mission_ended:
1061
1073
  self.logger.critical("Mission stopped: Impossible to set Gimbal Neutral")
1062
1074
  return False
@@ -1091,15 +1103,14 @@ class MAVLinCS:
1091
1103
 
1092
1104
  def request_home_position(self,
1093
1105
  target_system: int|None = None,
1094
- timeout_ack: float|None = 2,
1095
- timeout_home_reception: float|None = 2
1106
+ **kwargs
1096
1107
  ) -> mavtypes.HomePosition:
1097
1108
  """Requests and waits to receive the HOME position.
1098
1109
 
1099
1110
  Args:
1100
1111
  target_system (int|None): Target MAVLink system ID for the request. Defaults to the master's ID.
1101
- timeout_ack (float|None): ACK wait timeout. 0 if no wait (returns NaN, NaN, NaN). None if no timeout.
1102
- timeout_home_reception (float|None): Timeout for receiving the HOME position after ACK. 0 if no wait (returns NaN, NaN, NaN). None if no timeout.
1112
+ **timeout_ack (float|None): ACK wait timeout. 0 if no wait (returns NaN, NaN, NaN). None if no timeout. Default: 2.
1113
+ **timeout_home_reception (float|None): Timeout for receiving the HOME position after ACK. 0 if no wait (returns NaN, NaN, NaN). None if no timeout. Default: 2.
1103
1114
 
1104
1115
  Returns:
1105
1116
  mavtypes.HomePosition:
@@ -1111,6 +1122,8 @@ class MAVLinCS:
1111
1122
  All returned values are NaN if no data has been received.
1112
1123
  """
1113
1124
  self.logger.info("Requesting HOME..")
1125
+ timeout_ack = kwargs.get('timeout_ack', 2)
1126
+ timeout_home_reception = kwargs.get('timeout_home_reception', 2)
1114
1127
  if self.mission_ended:
1115
1128
  self.logger.critical("Mission stopped: Impossible to request HOME")
1116
1129
  return mavtypes.HomePosition(latitude_home=math.nan, longitude_home=math.nan, altitude_home=math.nan)
@@ -1165,7 +1178,7 @@ class MAVLinCS:
1165
1178
  msg_type: str | int,
1166
1179
  rate: float,
1167
1180
  target_system: int|None = None,
1168
- timeout_ack: float|None = 2
1181
+ **kwargs
1169
1182
  ) -> bool:
1170
1183
  """Changes the sending frequency of a specific flight controller message.
1171
1184
 
@@ -1173,12 +1186,13 @@ class MAVLinCS:
1173
1186
  msg_type (str | int): Message type (name or ID).
1174
1187
  rate (float): Transmission frequency in Hz.
1175
1188
  target_system (int|None): Target MAVLink system ID. Defaults to the master's ID.
1176
- timeout_ack (float|None): ACK wait timeout. 0 if no wait (returns False). None if no timeout.
1189
+ **timeout_ack (float|None): ACK wait timeout. 0 if no wait (returns False). None if no timeout. Default: 2.
1177
1190
 
1178
1191
  Returns:
1179
1192
  bool: True if the request was accepted by the flight controller, False otherwise.
1180
1193
  """
1181
1194
  self.logger.debug("Setting Msg Rate..")
1195
+ timeout_ack = kwargs.get('timeout_ack', 2)
1182
1196
  if self.mission_ended:
1183
1197
  self.logger.critical("Mission stopped: Impossible to set msg rate")
1184
1198
  return False
@@ -1389,22 +1403,23 @@ class MAVLinCS:
1389
1403
 
1390
1404
  def arm(self,
1391
1405
  force: bool = False,
1392
- timeout_ack: float|None = 2,
1393
- timeout_arming: float|None = 4,
1394
- target_system: int|None = None
1406
+ target_system: int|None = None,
1407
+ **kwargs
1395
1408
  ) -> bool:
1396
1409
  """Arms the drone.
1397
1410
 
1398
1411
  Args:
1399
1412
  force (bool): Whether to force arming or not.
1400
- timeout_ack (float|None): ACK wait timeout. 0 if no wait (returns False). None if no timeout.
1401
- timeout_arming (float|None): Arming wait timeout. 0 if no wait after ACK (returns the ACK wait result as bool). None if no timeout.
1402
1413
  target_system (int|None): Target MAVLink system ID. Defaults to the master's ID.
1414
+ **timeout_ack (float|None): ACK wait timeout. 0 if no wait (returns False). None if no timeout. Default: 2.
1415
+ **timeout_arming (float|None): Arming wait timeout. 0 if no wait after ACK (returns the ACK wait result as bool). None if no timeout. Default: 4.
1403
1416
 
1404
1417
  Returns:
1405
1418
  bool: Whether the drone was armed or not.
1406
1419
  """
1407
1420
  self.logger.info("Arming..")
1421
+ timeout_ack = kwargs.get('timeout_ack', 2)
1422
+ timeout_arming = kwargs.get('timeout_arming', 4)
1408
1423
  if self.mission_ended:
1409
1424
  self.logger.critical("Mission stopped: Impossible to arm")
1410
1425
  return False
@@ -1433,9 +1448,7 @@ class MAVLinCS:
1433
1448
  self.logger.warning("Mode %s not armable: trying to change mode to GUIDED", custom_mode)
1434
1449
  mod_changed = self.set_mode(
1435
1450
  mode="GUIDED",
1436
- target_system=target_system,
1437
- timeout_ack=3,
1438
- timeout_set_mode=5
1451
+ target_system=target_system
1439
1452
  )
1440
1453
  if not force and not mod_changed:
1441
1454
  self.logger.critical("Not the right mode, impossible to arm")
@@ -1455,8 +1468,8 @@ class MAVLinCS:
1455
1468
  )
1456
1469
  accepted = self.wait_command_ack(
1457
1470
  command=mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
1458
- timeout=timeout_ack,
1459
- source_system=target_system
1471
+ source_system=target_system,
1472
+ timeout=timeout_ack
1460
1473
  )
1461
1474
  if accepted is None: # End of mission or Timeout
1462
1475
  if self.mission_ended:
@@ -1480,22 +1493,23 @@ class MAVLinCS:
1480
1493
 
1481
1494
  def disarm(self,
1482
1495
  force: bool = False,
1483
- timeout_ack: float|None = 2,
1484
- timeout_disarming: float|None = 4,
1485
- target_system: int|None = None
1496
+ target_system: int|None = None,
1497
+ **kwargs
1486
1498
  ) -> bool:
1487
1499
  """Disarms the drone.
1488
1500
 
1489
1501
  Args:
1490
1502
  force (bool): Whether to force disarming or not.
1491
- timeout_ack (float|None): ACK wait timeout. 0 if no wait (returns False). None if no timeout.
1492
- timeout_disarming (float|None): Disarming wait timeout. 0 if no wait after ACK (returns the ACK wait result as bool). None if no timeout.
1493
1503
  target_system (int|None): Target MAVLink system ID. Defaults to the master's ID.
1504
+ **timeout_ack (float|None): ACK wait timeout. 0 if no wait (returns False). None if no timeout. Default: 2.
1505
+ **timeout_disarming (float|None): Disarming wait timeout. 0 if no wait after ACK (returns the ACK wait result as bool). None if no timeout. Default: 4.
1494
1506
 
1495
1507
  Returns:
1496
1508
  bool: Whether the drone was disarmed or not.
1497
1509
  """
1498
1510
  self.logger.info("Disarming..")
1511
+ timeout_ack = kwargs.get('timeout_ack', 2)
1512
+ timeout_disarming = kwargs.get('timeout_disarming', 4)
1499
1513
  if self.mission_ended:
1500
1514
  self.logger.critical("Mission stopped: Impossible to disarm")
1501
1515
  return False
@@ -1520,8 +1534,8 @@ class MAVLinCS:
1520
1534
  )
1521
1535
  accepted = self.wait_command_ack(
1522
1536
  command=mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
1523
- timeout=timeout_ack,
1524
- source_system=target_system
1537
+ source_system=target_system,
1538
+ timeout=timeout_ack
1525
1539
  )
1526
1540
  if accepted is None: # End of mission or Timeout
1527
1541
  if self.mission_ended:
@@ -1545,22 +1559,23 @@ class MAVLinCS:
1545
1559
 
1546
1560
  def set_mode(self,
1547
1561
  mode: str|int|tuple[int, int, int],
1548
- timeout_ack: float|None = 2,
1549
- timeout_set_mode: float|None = 4,
1550
- target_system: int|None = None
1562
+ target_system: int|None = None,
1563
+ **kwargs
1551
1564
  ) -> bool:
1552
1565
  """Changes the flight mode.
1553
1566
 
1554
1567
  Args:
1555
1568
  mode (str|int|tuple[int, int, int]): Flight mode. str: mode name. int: mode ID on ArduPilot. tuple[int, int, int]: (MAV_MODE_FLAG, Custom Main Mode, Custom Sub Mode) on PX4.
1556
- timeout_ack (float|None): ACK wait timeout. 0 if no wait (returns False). None if no timeout.
1557
- timeout_set_mode (float|None): Mode change wait timeout. 0 if no wait (returns the ACK wait result as bool). None if no timeout.
1558
1569
  target_system (int|None): Target MAVLink system ID. Defaults to the master's ID.
1570
+ **timeout_ack (float|None): ACK wait timeout. 0 if no wait (returns False). None if no timeout. Default: 2.
1571
+ **timeout_set_mode (float|None): Mode change wait timeout. 0 if no wait (returns the ACK wait result as bool). None if no timeout. Default: 4.
1559
1572
 
1560
1573
  Returns:
1561
1574
  bool: Whether the mode was changed or not.
1562
1575
  """
1563
1576
  self.logger.info("Setting Mode..")
1577
+ timeout_ack = kwargs.get('timeout_ack', 2)
1578
+ timeout_set_mode = kwargs.get('timeout_set_mode', 4)
1564
1579
  if self.mission_ended:
1565
1580
  self.logger.critical("Mission stopped: Impossible to change mode")
1566
1581
  return False
@@ -1615,8 +1630,8 @@ class MAVLinCS:
1615
1630
 
1616
1631
  accepted = self.wait_command_ack(
1617
1632
  command=mavutil.mavlink.MAV_CMD_DO_SET_MODE,
1618
- timeout=timeout_ack,
1619
- source_system=target_system
1633
+ source_system=target_system,
1634
+ timeout=timeout_ack
1620
1635
  )
1621
1636
  if accepted is None: # End of mission or Timeout
1622
1637
  if self.mission_ended:
@@ -1805,25 +1820,26 @@ class MAVLinCS:
1805
1820
  def takeoff(self,
1806
1821
  altitude: float,
1807
1822
  pitch: float = 0,
1808
- timeout_ack: float|None = 2,
1809
- timeout_takeoff: float|None = None,
1810
- precision: float = 1,
1811
- target_system: int|None = None
1823
+ target_system: int|None = None,
1824
+ **kwargs
1812
1825
  ) -> bool:
1813
1826
  """Perform a takeoff. Make sure to arm the drone beforehand.
1814
1827
 
1815
1828
  Args:
1816
1829
  altitude (float): Takeoff altitude.
1817
1830
  pitch (float): Enforced pitch.
1818
- timeout_ack (float|None): Timeout while waiting for the ack. 0 means no waiting (returns False). None means no timeout.
1819
- timeout_takeoff (float|None): Timeout while waiting for the takeoff to complete. 0 means no waiting (returns the ack result as bool). None means no timeout.
1820
- precision (float): Required altitude precision in meters to consider the takeoff successful. Defaults to 1.
1821
1831
  target_system (int|None): Target MAVLink system ID. Defaults to the master's ID.
1832
+ **precision (float): Required altitude precision in meters to consider the takeoff successful. Default: 1.
1833
+ **timeout_ack (float|None): Timeout while waiting for the ack. 0 means no waiting (returns False). None means no timeout. Default: 2.
1834
+ **timeout_takeoff (float|None): Timeout while waiting for the takeoff to complete. 0 means no waiting (returns the ack result as bool). None means no timeout. Default: None.
1822
1835
 
1823
1836
  Returns:
1824
1837
  bool: Takeoff result.
1825
1838
  """
1826
1839
  self.logger.debug("Starting takeoff..")
1840
+ precision = kwargs.get('precision', 1)
1841
+ timeout_ack = kwargs.get('timeout_ack', 2)
1842
+ timeout_takeoff = kwargs.get('timeout_takeoff', None)
1827
1843
  if self.mission_ended:
1828
1844
  self.logger.critical("Mission stopped: Unable to takeoff")
1829
1845
  return False
@@ -1843,8 +1859,6 @@ class MAVLinCS:
1843
1859
  self.logger.warning("Mode %s != GUIDED", self.mode(source_system=target_system))
1844
1860
  mod_changed = self.set_mode(
1845
1861
  mode="GUIDED",
1846
- timeout_ack=3,
1847
- timeout_set_mode=5,
1848
1862
  target_system=target_system
1849
1863
  )
1850
1864
  if not mod_changed:
@@ -1891,7 +1905,8 @@ class MAVLinCS:
1891
1905
  )
1892
1906
  accepted = self.wait_command_ack(
1893
1907
  command=mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
1894
- timeout=timeout_ack, source_system=target_system
1908
+ source_system=target_system,
1909
+ timeout=timeout_ack
1895
1910
  )
1896
1911
  if accepted is None:
1897
1912
  self.logger.critical("Takeoff failed: insufficient data")
@@ -1960,23 +1975,24 @@ class MAVLinCS:
1960
1975
  time.sleep(0.05)
1961
1976
 
1962
1977
  def land(self,
1963
- timeout_ack: float|None = 2,
1964
- timeout_set_mode: float|None = 3,
1965
- timeout_land: float|None = None,
1966
- target_system: int|None = None
1978
+ target_system: int|None = None,
1979
+ **kwargs
1967
1980
  ) -> bool:
1968
1981
  """Perform a simple landing. Currently implemented only for ArduPilot.
1969
1982
 
1970
1983
  Args:
1971
- timeout_ack (float|None): Timeout while waiting for the ack.
1972
- timeout_set_mode (float|None): Timeout while waiting for the mode change.
1973
- timeout_land (float|None): Timeout while waiting for the landing to complete.
1974
1984
  target_system (int|None): Target MAVLink system ID. Defaults to the master's ID.
1985
+ **timeout_ack (float|None): Timeout while waiting for the ack. Default: 2.
1986
+ **timeout_set_mode (float|None): Timeout while waiting for the mode change. Default: 3.
1987
+ **timeout_land (float|None): Timeout while waiting for the landing to complete. Default: None.
1975
1988
 
1976
1989
  Returns:
1977
1990
  bool: Result of the operation.
1978
1991
  """
1979
1992
  self.logger.info("Landing..")
1993
+ timeout_ack = kwargs.get('timeout_ack', 2)
1994
+ timeout_set_mode = kwargs.get('timeout_set_mode', 3)
1995
+ timeout_land = kwargs.get('timeout_land', None)
1980
1996
  if self.mission_ended:
1981
1997
  self.logger.critical("Mission stopped: Unable to land")
1982
1998
  return False
@@ -1993,9 +2009,9 @@ class MAVLinCS:
1993
2009
 
1994
2010
  if not self.set_mode(
1995
2011
  mode="LAND",
2012
+ target_system=target_system,
1996
2013
  timeout_ack=timeout_ack,
1997
- timeout_set_mode=timeout_set_mode,
1998
- target_system=target_system
2014
+ timeout_set_mode=timeout_set_mode
1999
2015
  ):
2000
2016
  self.logger.critical("Landing failed")
2001
2017
  return False
@@ -2188,19 +2204,20 @@ class MAVLinCS:
2188
2204
  return True
2189
2205
 
2190
2206
  def clear_waypoints(self,
2191
- timeout: float|None = 2,
2192
- target_system: int|None = None
2207
+ target_system: int|None = None,
2208
+ **kwargs
2193
2209
  ) -> bool:
2194
2210
  """Delete all waypoints.
2195
2211
 
2196
2212
  Args:
2197
- timeout (float|None): Timeout while waiting for the ack.
2198
2213
  target_system (int|None): Target MAVLink system ID. Defaults to the master's ID.
2214
+ **timeout (float|None): Timeout while waiting for the ack. Default: 2.
2199
2215
 
2200
2216
  Returns:
2201
2217
  bool: Result of the request.
2202
2218
  """
2203
2219
  self.logger.debug("Deleting waypoints..")
2220
+ timeout = kwargs.get('timeout', 2)
2204
2221
  if self.mission_ended:
2205
2222
  self.logger.critical("Mission stopped: Unable to clear waypoints")
2206
2223
  return False
@@ -2220,8 +2237,8 @@ class MAVLinCS:
2220
2237
 
2221
2238
  accepted = self.wait_mission_ack(
2222
2239
  mission_type=mavutil.mavlink.MAV_MISSION_TYPE_MISSION,
2223
- timeout=timeout,
2224
- source_system=target_system
2240
+ source_system=target_system,
2241
+ timeout=timeout
2225
2242
  )
2226
2243
  if accepted:
2227
2244
  self.logger.info("Waypoints cleared")
@@ -2237,19 +2254,20 @@ class MAVLinCS:
2237
2254
  def start_mission(self,
2238
2255
  first_wp_tkoff: bool,
2239
2256
  target_system: int|None = None,
2240
- timeout: float|None = 3
2257
+ **kwargs
2241
2258
  ) -> bool:
2242
2259
  """Start the mission. Currently implemented only for ArduPilot.
2243
2260
 
2244
2261
  Args:
2245
2262
  first_wp_tkoff (bool): True if the first waypoint is a takeoff waypoint, False otherwise.
2246
2263
  target_system (int|None): Target MAVLink system ID. Defaults to the master's ID.
2247
- timeout (float|None): Waiting timeout (used in several places).
2264
+ **timeout (float|None): Waiting timeout (used in several places). Default: 3.
2248
2265
 
2249
2266
  Returns:
2250
2267
  bool: Whether the mission has started.
2251
2268
  """
2252
2269
  self.logger.info("Starting mission..")
2270
+ timeout = kwargs.get('timeout', 3)
2253
2271
  if self.mission_ended:
2254
2272
  self.logger.critical("Mission stopped: Unable to start mission")
2255
2273
  return False
@@ -2268,18 +2286,18 @@ class MAVLinCS:
2268
2286
  target_system = self.master.target_system
2269
2287
  if first_wp_tkoff:
2270
2288
  armed = self.arm(
2289
+ target_system=target_system,
2271
2290
  timeout_ack=timeout,
2272
- timeout_arming=timeout,
2273
- target_system=target_system
2291
+ timeout_arming=timeout
2274
2292
  )
2275
2293
  if not armed:
2276
2294
  self.logger.critical("Start mission failed")
2277
2295
  return False
2278
2296
  mod_auto = self.set_mode(
2279
2297
  mode="AUTO",
2298
+ target_system=target_system,
2280
2299
  timeout_ack=timeout,
2281
- timeout_set_mode=timeout,
2282
- target_system=target_system
2300
+ timeout_set_mode=timeout
2283
2301
  )
2284
2302
  if not mod_auto:
2285
2303
  self.logger.critical("Start mission failed")
@@ -2287,18 +2305,18 @@ class MAVLinCS:
2287
2305
  self.logger.info("Mission started")
2288
2306
  return True
2289
2307
  disarmed = self.disarm(
2308
+ target_system=target_system,
2290
2309
  timeout_ack=timeout,
2291
- timeout_disarming=timeout,
2292
- target_system=target_system
2310
+ timeout_disarming=timeout
2293
2311
  )
2294
2312
  if not disarmed:
2295
2313
  self.logger.critical("Start mission failed")
2296
2314
  return False
2297
2315
  mod_auto = self.set_mode(
2298
2316
  mode="AUTO",
2317
+ target_system=target_system,
2299
2318
  timeout_ack=timeout,
2300
- timeout_set_mode=timeout,
2301
- target_system=target_system
2319
+ timeout_set_mode=timeout
2302
2320
  )
2303
2321
  if not mod_auto:
2304
2322
  self.logger.critical("Start mission failed")
@@ -2329,19 +2347,20 @@ class MAVLinCS:
2329
2347
  return bool(ack)
2330
2348
 
2331
2349
  def return_to_launch(self,
2332
- timeout: float|None = 2,
2333
- target_system: int|None = None
2350
+ target_system: int|None = None,
2351
+ **kwargs
2334
2352
  ) -> bool:
2335
2353
  """Send a Return-To-Launch (RTL) request.
2336
2354
 
2337
2355
  Args:
2338
- timeout (float|None): ACK waiting timeout.
2339
2356
  target_system (int|None): Target MAVLink system ID. Defaults to the master's ID.
2357
+ **timeout_ack (float|None): ACK waiting timeout. Default: 2.
2340
2358
 
2341
2359
  Returns:
2342
2360
  bool: Result of the request.
2343
2361
  """
2344
2362
  self.logger.debug("RTL..")
2363
+ timeout_ack = kwargs.get('timeout_ack', 2)
2345
2364
  if self.mission_ended:
2346
2365
  self.logger.critical("Mission stopped: Unable to RTL")
2347
2366
  return False
@@ -2365,8 +2384,8 @@ class MAVLinCS:
2365
2384
 
2366
2385
  accepted = self.wait_command_ack(
2367
2386
  command=mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH,
2368
- timeout=timeout,
2369
- source_system=target_system
2387
+ source_system=target_system,
2388
+ timeout=timeout_ack
2370
2389
  )
2371
2390
  if accepted:
2372
2391
  self.logger.info("RTL started")
@@ -2380,21 +2399,22 @@ class MAVLinCS:
2380
2399
  return bool(accepted)
2381
2400
 
2382
2401
  def reboot(self,
2383
- timeout_ack: float|None = 2,
2402
+ force=False,
2384
2403
  target_system: int|None = None,
2385
- force=False
2404
+ **kwargs
2386
2405
  ) -> bool:
2387
2406
  """Reboot the autopilot.
2388
2407
 
2389
2408
  Args:
2390
- timeout_ack (float|None): ACK waiting timeout.
2391
- target_system (int|None): Target MAVLink system ID.
2392
2409
  force (bool): Whether to perform a forced reboot (not recommended; for reference only).
2410
+ target_system (int|None): Target MAVLink system ID.
2411
+ **timeout_ack (float|None): ACK waiting timeout. Default: 2.
2393
2412
 
2394
2413
  Returns:
2395
2414
  bool: Result of the operation.
2396
2415
  """
2397
2416
  self.logger.debug("Rebooting..")
2417
+ timeout_ack = kwargs.get('timeout_ack', 2)
2398
2418
  if self.mission_ended:
2399
2419
  self.logger.critical("Mission stopped: Unable to reboot")
2400
2420
  return False
@@ -2424,8 +2444,8 @@ class MAVLinCS:
2424
2444
 
2425
2445
  accepted = self.wait_command_ack(
2426
2446
  command=mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN,
2427
- timeout=timeout_ack,
2428
- source_system=target_system
2447
+ source_system=target_system,
2448
+ timeout=timeout_ack
2429
2449
  )
2430
2450
  if accepted:
2431
2451
  self.logger.info("Reboot accepted")
@@ -3264,32 +3284,39 @@ class MAVLinCS:
3264
3284
 
3265
3285
  def open(self,
3266
3286
  address: str,
3287
+ baud: int = 57600,
3267
3288
  source_system: int = 255,
3268
3289
  source_component: int = mavutil.mavlink.MAV_COMP_ID_MISSIONPLANNER,
3269
3290
  target_system: int|None = None,
3270
- baud: int = 57600,
3271
- timeout_heartbeat: float|None = None,
3272
- pos_rate: float = 4,
3273
- dialect: str = "ardupilotmega"
3291
+ dialect: str = "ardupilotmega",
3292
+ **kwargs
3274
3293
  ):
3275
3294
  """Opens a MAVLink connection. Ensure any previous connection is closed.
3276
3295
 
3277
3296
  Args:
3278
3297
  address (str): Connection address (e.g., serial port or UDP endpoint).
3298
+ baud (int): Baud rate for serial connection.
3279
3299
  source_system (int): MAVLink system ID for the ground station.
3280
3300
  source_component (int): MAVLink component ID for the ground station.
3281
3301
  target_system (int|None): MAVLink system ID of the flight controller. If None, first detected vehicle is used.
3282
- baud (int): Baud rate for serial connection.
3283
- timeout_heartbeat (float|None): Time to wait for a HEARTBEAT before considering connection impossible. None = wait indefinitely, 0 = skip waiting.
3284
- pos_rate (float): Frequency (Hz) at which the flight controller sends position messages to the ground station.
3285
- dialect (str): MAVLink dialect, required if timeout_heartbeat==0. "ardupilotmega" for ArduPilot, "common" for PX4.
3302
+ dialect (str): MAVLink 2 dialect. Example: "ardupilotmega" for ArduPilot, "common" for PX4.
3303
+ **timeout_heartbeat (float|None): Time to wait for a HEARTBEAT before considering connection impossible. None = wait indefinitely, 0 = skip waiting. Default: None.
3304
+ **pos_rate (float): Frequency (Hz) at which the flight controller sends position messages to the ground station. Default: 4.
3286
3305
 
3287
3306
  Note:
3307
+ - Initializes MAVLink version and dialect.
3288
3308
  - Sets up callbacks for sending and receiving MAVLink messages.
3289
- - Initializes MAVLink version and dialect if timeout_heartbeat==0.
3290
3309
  - Starts background MAV thread for message handling.
3291
- - If pos_rate<=0, doesn't request position.
3310
+ - If **pos_rate<=0, doesn't request position.
3292
3311
  """
3312
+ timeout_heartbeat = kwargs.get('timeout_heartbeat', None)
3313
+ pos_rate = kwargs.get('pos_rate', 4)
3314
+ self.logger.debug("Loading MAVLink version and dialect..")
3315
+ os.environ['MAVLINK20'] = '1'
3316
+ os.environ.pop('MAVLINK09', None)
3317
+ mavutil.set_dialect(dialect=dialect)
3318
+ self.logger.debug("MAVLink version and dialect loaded")
3319
+
3293
3320
  self.logger.debug("Creating master object..")
3294
3321
  self.master: mavutil.mavfile = mavutil.mavlink_connection(
3295
3322
  device=address,
@@ -3300,11 +3327,15 @@ class MAVLinCS:
3300
3327
  )
3301
3328
 
3302
3329
  try:
3303
- self.logger.debug("Master object created")
3330
+ self.master.first_byte = False
3331
+ self.master.mav.set_callback(self._recv_msg_callback)
3332
+ self.master.mav.set_send_callback(self._send_callback)
3304
3333
  self.timesync_handler.set_master(self.master)
3334
+ self.mavthread.set_master(self.master)
3335
+
3336
+ self.logger.debug("Master object created")
3305
3337
 
3306
3338
  if timeout_heartbeat != 0:
3307
- self.master.mav.set_callback(self._recv_msg_callback)
3308
3339
  self.logger.info("Waiting for HEARTBEAT%s", f" from ({target_system}:1)" if target_system else "")
3309
3340
  start_time = time.time()
3310
3341
  while True:
@@ -3326,19 +3357,9 @@ class MAVLinCS:
3326
3357
  break
3327
3358
  else:
3328
3359
  self.logger.info("No timeout for HEARTBEAT specified")
3329
- self.master.first_byte = False
3330
- self.master.WIRE_PROTOCOL_VERSION = "2.0"
3331
- os.environ['MAVLINK20'] = '1'
3332
- mavutil.set_dialect(dialect=dialect)
3333
- self.master.mav = mavutil.mavlink.MAVLink(self.master, srcSystem=self.master.source_system, srcComponent=self.master.source_component)
3334
- self.master.mav.robust_parsing = self.master.robust_parsing
3335
- self.master.WIRE_PROTOCOL_VERSION = mavutil.mavlink.WIRE_PROTOCOL_VERSION
3336
- self.master.mav.set_callback(self._recv_msg_callback)
3337
3360
  self.master.target_system = 1 if target_system is None else target_system
3338
3361
  self.master.target_component = mavutil.mavlink.MAV_COMP_ID_AUTOPILOT1
3339
3362
 
3340
- self.master.mav.set_send_callback(self._send_callback)
3341
-
3342
3363
  self._sysid_to_request_home.clear()
3343
3364
  sysid_to_request_home = self.data_init.get("sysid_to_request_home", None)
3344
3365
  if isinstance(sysid_to_request_home, int):
@@ -3374,7 +3395,6 @@ class MAVLinCS:
3374
3395
  self.logger.warning("%s is not a MAV_CMD", cmd)
3375
3396
 
3376
3397
  # Start MAV thread
3377
- self.mavthread.set_master(self.master)
3378
3398
  self.mavthread.start_thread()
3379
3399
 
3380
3400
  if timeout_heartbeat != 0 and pos_rate>0:
@@ -20,6 +20,7 @@ try:
20
20
  colorama_init()
21
21
  _ENABLE_COLORS = True
22
22
  except ImportError:
23
+ print("Warning: colorama isn't installed. No logging color available")
23
24
  _ENABLE_COLORS = False
24
25
 
25
26
  _COLORS = {
@@ -28,7 +29,7 @@ _COLORS = {
28
29
  logging.WARNING: Fore.YELLOW,
29
30
  logging.ERROR: Fore.RED,
30
31
  logging.CRITICAL: Fore.RED + Style.BRIGHT,
31
- }
32
+ } if _ENABLE_COLORS else {}
32
33
  _RESET = Style.RESET_ALL if _ENABLE_COLORS else ""
33
34
 
34
35
 
@@ -1,6 +1,6 @@
1
1
  Metadata-Version: 2.4
2
2
  Name: pyMAVLinCS
3
- Version: 1.0.4
3
+ Version: 1.1.0
4
4
  Summary: Python API for autonomous drone control based on pymavlink
5
5
  Author-email: Noah Redon <redonnoah.cs@gmail.com>
6
6
  License: GPL-3.0-or-later
@@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"
4
4
 
5
5
  [project]
6
6
  name = "pyMAVLinCS"
7
- version = "1.0.4"
7
+ version = "1.1.0"
8
8
  description = "Python API for autonomous drone control based on pymavlink"
9
9
  authors = [
10
10
  {name="Noah Redon", email="redonnoah.cs@gmail.com"}
File without changes
File without changes
File without changes