Skip to content

Commit ac2776c

Browse files
evan-palmerevan-palmer
authored
Resolve Merge Errors and Acknowledgement Bugs (#38)
* Fixed issue with readme * Integrate HRL Support (#8) * Added missing type hints * Refactored to cleanup message sending and added support to send msg until ack * Added support for new HRL messages * Updated devices to use tuple key and ensure msg target in network * Added requirement to clone repo in readme * Resolved grammatical error * Updated readme to reflect require_ack change * Updated python requirement in readme and updated setup version * Resolved errors in structuring named value int message * Cleaned up readme formatting * Created dependencies section in readme * Update Message Commands and Handlers (#15) * Add HRL Support (#10) * Fixed issue with readme * Integrate HRL Support (#8) * Added missing type hints * Refactored to cleanup message sending and added support to send msg until ack * Added support for new HRL messages * Updated devices to use tuple key and ensure msg target in network * Added requirement to clone repo in readme * Resolved grammatical error * Updated readme to reflect require_ack change * Updated python requirement in readme and updated setup version * Resolved errors in structuring named value int message * Cleaned up readme formatting * Created dependencies section in readme * Removed ros commands, fixed gps, renamed outgoingmsg to agentmsg, fixed ack issue * fixed setup version * Cleaned up debug messages and fixed readme example * Add Param Setting and Reading (#19) * Resolved typo in readme * Re-update send command methods to use timeout instead of infinite loop * Refactored various send message functions into single function * Implemented starter waypoint and mission support * Added mission to agent object * Updated agent to have name field * Rename timeout period in agent * Modified agentmsg to be parent class and updated send method to used full msg * Refactored send message to be more usable and added new commands * Added waypoint and takeoff messages * Added simple takeoff and takeoff command support * Add catches to prevent invalid altitude settings * Code cleanup * Added missing method type hints * Fixed mission init and msgmap init * Updated common messages to use msg timeout * Fixed variable naming in msgs * Updated senders to check ack instead of in retry function * Add support to set parameters * Added support to set parameters and started process of enabling reading parameters * Cleaned up agent and modified parameters stored to be circular buffer * Added interface to read parameters * Removed unused files * Added assertions to message construction to ensure proper msg type is provided * Added debug statements to parameter read method * Resolved bugs in message sending and message map * Wrapped up implementation of initial parameter setting and reading feature * Removed unused comments * Refactored send message implementation to create new thread on call * Refactored parameter implementation to use independent threads * Removed log option and made just debug * Cleaned up parameter read implementation * Update README to Reflect Usage Changes (#31) * Updated readme example to use latest version of pymavswarm * Updated readme example to use latest version of pymavswarm * Resolve param retry error * Fixed incorrect mavlink message sent on takeoff command (#35) * Resolve Takeoff and Acknowledgement Bugs (#37) * Resolved typo in readme * Re-update send command methods to use timeout instead of infinite loop * Refactored various send message functions into single function * Implemented starter waypoint and mission support * Added mission to agent object * Updated agent to have name field * Rename timeout period in agent * Modified agentmsg to be parent class and updated send method to used full msg * Refactored send message to be more usable and added new commands * Added waypoint and takeoff messages * Added simple takeoff and takeoff command support * Add catches to prevent invalid altitude settings * Code cleanup * Added missing method type hints * Fixed mission init and msgmap init * Updated common messages to use msg timeout * Fixed variable naming in msgs * Updated senders to check ack instead of in retry function * Add support to set parameters * Added support to set parameters and started process of enabling reading parameters * Cleaned up agent and modified parameters stored to be circular buffer * Added interface to read parameters * Removed unused files * Added assertions to message construction to ensure proper msg type is provided * Added debug statements to parameter read method * Resolved bugs in message sending and message map * Wrapped up implementation of initial parameter setting and reading feature * Removed unused comments * Refactored send message implementation to create new thread on call * Refactored parameter implementation to use independent threads * Removed log option and made just debug * Cleaned up parameter read implementation * Resolved bugs in message acknowledgement and takeoff
1 parent 4c9922d commit ac2776c

File tree

1 file changed

+81
-65
lines changed

1 file changed

+81
-65
lines changed

‎pymavswarm/Connection.py

Lines changed: 81 additions & 65 deletions
Original file line numberDiff line numberDiff line change
@@ -422,6 +422,84 @@ def sender(self, msg: SystemCommandMsg, fn_id: int=0) -> None:
422422
return ack
423423

424424

425+
@self.send_message(['shutdown'])
426+
def sender(self, msg: SystemCommandMsg, fn_id: int=0) -> None:
427+
"""
428+
Shutdown an agent
429+
"""
430+
self.master.mav.command_long_send(msg.target_system, msg.target_comp,
431+
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
432+
0,
433+
2, 0, 0, 0, 0, 0, 0)
434+
ack = False
435+
436+
if self.__ack_msg('COMMAND_ACK', timeout=msg.ack_timeout)[0]:
437+
ack = True
438+
else:
439+
if msg.retry:
440+
if self.__retry_msg_send(msg, self.message_senders[msg.get_type()][fn_id]):
441+
ack = True
442+
443+
if ack:
444+
self.logger.info(f'Successfully acknowledged reception of the disarm command sent to Agent ({msg.target_system}, {msg.target_comp})')
445+
else:
446+
self.logger.error(f'Failed to acknowledge reception of the disarm command sent to Agent ({msg.target_system}, {msg.target_comp})')
447+
448+
return ack
449+
450+
451+
@self.send_message(['kill'])
452+
def sender(self, msg: SystemCommandMsg, fn_id: int=0) -> None:
453+
"""
454+
Force disarm an agent
455+
"""
456+
self.master.mav.command_long_send(msg.target_system, msg.target_comp,
457+
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
458+
0,
459+
0, 21196, 0, 0, 0, 0, 0)
460+
ack = False
461+
462+
if self.__ack_msg('COMMAND_ACK', timeout=msg.ack_timeout)[0]:
463+
ack = True
464+
else:
465+
if msg.retry:
466+
if self.__retry_msg_send(msg, self.message_senders[msg.get_type()][fn_id]):
467+
ack = True
468+
469+
if ack:
470+
self.logger.info(f'Successfully acknowledged reception of the kill command sent to Agent ({msg.target_system}, {msg.target_comp})')
471+
else:
472+
self.logger.error(f'Failed to acknowledge reception of the kill command sent to Agent ({msg.target_system}, {msg.target_comp})')
473+
474+
return ack
475+
476+
477+
@self.send_message(['reboot'])
478+
def sender(self, msg: SystemCommandMsg, fn_id: int=0) -> None:
479+
"""
480+
Reboot an agent
481+
"""
482+
self.master.mav.command_long_send(msg.target_system, msg.target_comp,
483+
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
484+
0,
485+
1, 0, 0, 0, 0, 0, 0)
486+
ack = False
487+
488+
if self.__ack_msg('COMMAND_ACK', timeout=msg.ack_timeout)[0]:
489+
ack = True
490+
else:
491+
if msg.retry:
492+
if self.__retry_msg_send(msg, self.message_senders[msg.get_type()][fn_id]):
493+
ack = True
494+
495+
if ack:
496+
self.logger.info(f'Successfully acknowledged reception of the reboot command sent to Agent ({msg.target_system}, {msg.target_comp})')
497+
else:
498+
self.logger.error(f'Failed to acknowledge reception of the reboot command sent to Agent ({msg.target_system}, {msg.target_comp})')
499+
500+
return ack
501+
502+
425503
@self.send_message(['shutdown'])
426504
def sender(self, msg: SystemCommandMsg, fn_id: int=0) -> None:
427505
"""
@@ -447,10 +525,6 @@ def sender(self, msg: SystemCommandMsg, fn_id: int=0) -> None:
447525

448526
return ack
449527

450-
451-
"""
452-
Pre-flight calibration commands
453-
"""
454528

455529
@self.send_message(['accelcal'])
456530
def sender(self, msg: PreflightCalibrationMsg, fn_id: int=0) -> None:
@@ -1396,72 +1470,14 @@ def __retry_msg_send(self, msg, fn) -> bool:
13961470
# Don't let the message come back here and create an infinite loop
13971471
msg.retry = False
13981472

1399-
while time.time() - start_time >= msg.msg_timeout:
1473+
while time.time() - start_time <= msg.msg_timeout:
14001474
# Reattempt the message send
1401-
if fn(self, msg):
1475+
if fn(msg):
14021476
ack = True
14031477
break
14041478

14051479
return ack
14061480

1407-
1408-
def __send_arming_msg(self, msg, sys_id, comp_id, require_ack=False) -> None:
1409-
"""
1410-
Helper method used to send an arming command (arm or disarm)
1411-
"""
1412-
1413-
if require_ack:
1414-
ack = False
1415-
1416-
while not ack:
1417-
self.master.mav.command_long_send(sys_id, comp_id,
1418-
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0,
1419-
msg, 0, 0, 0, 0, 0, 0)
1420-
1421-
if self.__ack_sys_cmd(timeout=self.cmd_timeout):
1422-
ack = True
1423-
self.logger.debug(f'The system has acknowledged reception of the arming command: {msg}')
1424-
else:
1425-
self.logger.exception('The system was unable to confirm reception of the arming command: {msg}. Re-attempting message send.')
1426-
else:
1427-
self.master.mav.command_long_send(sys_id, comp_id,
1428-
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0,
1429-
msg, 0, 0, 0, 0, 0, 0)
1430-
1431-
if self.__ack_sys_cmd(timeout=self.cmd_timeout):
1432-
self.logger.debug('The system has acknowledged reception of the arming command: {msg}')
1433-
else:
1434-
self.logger.exception('The system was unable to confirm reception of the arming command: {msg}')
1435-
1436-
return
1437-
1438-
1439-
def __send_preflight_calibration_msg(self, msg, sys_id, comp_id, require_ack=False) -> None:
1440-
"""
1441-
Helper method used to send a pre-flight calibration message
1442-
"""
1443-
if require_ack:
1444-
ack = False
1445-
1446-
while not ack:
1447-
self.master.mav.command_long_send(sys_id, comp_id,
1448-
mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, 0,
1449-
0, 0, 0, 0, msg, 0, 0)
1450-
1451-
if self.__ack_sys_cmd(timeout=self.cmd_timeout):
1452-
ack = True
1453-
self.logger.debug(f'The system has acknowledged reception of the pre-flight calibration command: {msg}')
1454-
else:
1455-
self.logger.exception('The system was unable to confirm reception of the pre-flight calibration command: {msg}. Re-attempting message send.')
1456-
else:
1457-
self.master.mav.command_long_send(sys_id, comp_id,
1458-
mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, 0,
1459-
0, 0, 0, 0, msg, 0, 0)
1460-
1461-
if self.__ack_sys_cmd(timeout=self.cmd_timeout):
1462-
self.logger.debug('The system has acknowledged reception of the pre-flight calibration command: {msg}')
1463-
else:
1464-
self.logger.exception('The system was unable to confirm reception of the pre-flight calibration command: {msg}')
14651481

14661482
def __ack_msg(self, msg_type: str, timeout=1.0) -> Tuple[bool, Any]:
14671483
"""
@@ -1659,7 +1675,7 @@ def __read_param(self, param: Parameter) -> bool:
16591675
self.devices[(param.sys_id, param.comp_id)].last_params_read.append(read_param)
16601676
else:
16611677
if param.retry:
1662-
if self.__retry_msg_send(param, self.__set_param):
1678+
if self.__retry_msg_send(param, self.__read_param):
16631679
ack = True
16641680

16651681
if ack:

0 commit comments

Comments
 (0)