diff --git a/gcs/src/components/missions/missionStatistics.jsx b/gcs/src/components/missions/missionStatistics.jsx index 95ecd10bb..f77be7fd1 100644 --- a/gcs/src/components/missions/missionStatistics.jsx +++ b/gcs/src/components/missions/missionStatistics.jsx @@ -213,7 +213,7 @@ export default function MissionStatistics({ missionItems }) { value={maxSlopeGradient.maxGradient} tooltip={ maxSlopeGradient.points && - `Between ${maxSlopeGradient.points[0].seq} and ${maxSlopeGradient.points[1].seq}` + `Between ${maxSlopeGradient.points[0]?.seq} and ${maxSlopeGradient.points[1]?.seq}` } units="%" /> diff --git a/radio/app/controllers/missionController.py b/radio/app/controllers/missionController.py index 592f9f7dd..756393883 100644 --- a/radio/app/controllers/missionController.py +++ b/radio/app/controllers/missionController.py @@ -605,6 +605,8 @@ def uploadMission( "success": False, "message": "Could not upload mission, received mission acknowledgement error", } + elif response.msgname == "MISSION_ACK" and response.type == 0: + continue # Continue to next iteration if we received a MISSION_ACK elif response.mission_type == mission_type: self.drone.logger.debug( f"Sending mission item {response.seq} out of {new_loader.count()-1}" diff --git a/radio/tests/mission_test_files/default_fence.txt b/radio/tests/mission_test_files/default_fence.txt new file mode 100644 index 000000000..1453a7ff9 --- /dev/null +++ b/radio/tests/mission_test_files/default_fence.txt @@ -0,0 +1,15 @@ +QGC WPL 110 +0 1 0 16 0 0 0 0 52.7806539 -0.7083070 136.350000 1 +1 0 3 5001 6.00000000 0.00000000 0.00000000 0.00000000 52.78146190 -0.71057081 0.000000 1 +2 0 3 5001 6.00000000 0.00000000 0.00000000 0.00000000 52.78105306 -0.71029186 1.000000 1 +3 0 3 5001 6.00000000 0.00000000 0.00000000 0.00000000 52.78003421 -0.71083903 2.000000 1 +4 0 3 5001 6.00000000 0.00000000 0.00000000 0.00000000 52.78185126 -0.70354342 3.000000 1 +5 0 3 5001 6.00000000 0.00000000 0.00000000 0.00000000 52.78305826 -0.70601106 4.000000 1 +6 0 3 5001 6.00000000 0.00000000 0.00000000 0.00000000 52.78254562 -0.70815682 5.000000 1 +7 0 3 5004 25.00000000 0.00000000 0.00000000 0.00000000 52.77965780 -0.70921630 0.000000 1 +8 0 3 5003 50.00000000 0.00000000 0.00000000 0.00000000 52.78019000 -0.70440170 0.000000 1 +9 0 3 5002 5.00000000 0.00000000 0.00000000 0.00000000 52.78282465 -0.70610762 0.000000 1 +10 0 3 5002 5.00000000 0.00000000 0.00000000 0.00000000 52.78187073 -0.70415497 1.000000 1 +11 0 3 5002 5.00000000 0.00000000 0.00000000 0.00000000 52.78144892 -0.70581794 2.000000 1 +12 0 3 5002 5.00000000 0.00000000 0.00000000 0.00000000 52.78218871 -0.70580721 3.000000 1 +13 0 3 5002 5.00000000 0.00000000 0.00000000 0.00000000 52.78263647 -0.70689082 4.000000 1 diff --git a/radio/tests/mission_test_files/default_mission.txt b/radio/tests/mission_test_files/default_mission.txt new file mode 100644 index 000000000..8eb0fdd45 --- /dev/null +++ b/radio/tests/mission_test_files/default_mission.txt @@ -0,0 +1,9 @@ +QGC WPL 110 +0 1 0 16 0 0 0 0 52.7806539 -0.7083070 136.350000 1 +1 0 3 22 0.00000000 0.00000000 0.00000000 0.00000000 0.00000000 0.00000000 30.000000 1 +2 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 52.78031970 -0.70979300 30.000000 1 +3 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 52.78122830 -0.70989490 30.000000 1 +4 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 52.78169880 -0.70795300 30.000000 1 +5 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 52.78144240 -0.70571600 30.000000 1 +6 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 52.78079670 -0.70659580 30.000000 1 +7 0 3 21 0.00000000 0.00000000 0.00000000 0.00000000 52.78043980 -0.70819970 0.000000 1 diff --git a/radio/tests/mission_test_files/default_rally.txt b/radio/tests/mission_test_files/default_rally.txt new file mode 100644 index 000000000..fd7023fa2 --- /dev/null +++ b/radio/tests/mission_test_files/default_rally.txt @@ -0,0 +1,4 @@ +QGC WPL 110 +0 1 0 16 0 0 0 0 52.7806539 -0.7083070 136.350000 1 +1 0 3 5100 0.00000000 0.00000000 0.00000000 0.00000000 52.78100110 -0.70948720 10.000000 1 +2 0 3 5100 0.00000000 0.00000000 0.00000000 0.00000000 52.78211080 -0.70567850 10.000000 1 diff --git a/radio/tests/mission_test_files/test_getCurrentMission_correctFence_result.json b/radio/tests/mission_test_files/test_getCurrentMission_correctFence_result.json new file mode 100644 index 000000000..519cba6d4 --- /dev/null +++ b/radio/tests/mission_test_files/test_getCurrentMission_correctFence_result.json @@ -0,0 +1,240 @@ +{ + "success": true, + "mission_type": "fence", + "items": [ + { + "autocontinue": 0, + "command": 5001, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 6.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 0, + "target_component": 0, + "target_system": 255, + "x": 527814618, + "y": -7105708, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5001, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 6.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 1, + "target_component": 0, + "target_system": 255, + "x": 527810530, + "y": -7102918, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5001, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 6.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 2, + "target_component": 0, + "target_system": 255, + "x": 527800342, + "y": -7108390, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5001, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 6.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 3, + "target_component": 0, + "target_system": 255, + "x": 527818512, + "y": -7035434, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5001, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 6.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 4, + "target_component": 0, + "target_system": 255, + "x": 527830582, + "y": -7060110, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5001, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 6.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 5, + "target_component": 0, + "target_system": 255, + "x": 527825456, + "y": -7081568, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5004, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 25.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 6, + "target_component": 0, + "target_system": 255, + "x": 527796578, + "y": -7092163, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5003, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 50.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 7, + "target_component": 0, + "target_system": 255, + "x": 527801900, + "y": -7044017, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5002, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 5.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 8, + "target_component": 0, + "target_system": 255, + "x": 527828246, + "y": -7061076, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5002, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 5.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 9, + "target_component": 0, + "target_system": 255, + "x": 527818707, + "y": -7041549, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5002, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 5.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 10, + "target_component": 0, + "target_system": 255, + "x": 527814489, + "y": -7058179, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5002, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 5.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 11, + "target_component": 0, + "target_system": 255, + "x": 527821887, + "y": -7058072, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5002, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 5.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 12, + "target_component": 0, + "target_system": 255, + "x": 527826364, + "y": -7068908, + "z": 0.0 + } + ] +} diff --git a/radio/tests/mission_test_files/test_getCurrentMission_correctMission_result.json b/radio/tests/mission_test_files/test_getCurrentMission_correctMission_result.json new file mode 100644 index 000000000..0e860cbab --- /dev/null +++ b/radio/tests/mission_test_files/test_getCurrentMission_correctMission_result.json @@ -0,0 +1,150 @@ +{ + "success": true, + "mission_type": "mission", + "items": [ + { + "autocontinue": 1, + "command": 16, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 0, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 0, + "target_component": 0, + "target_system": 255, + "x": 527805690, + "y": -7079236, + "z": 0.09999999403953552 + }, + { + "mavpackettype": "MISSION_ITEM_INT", + "target_system": 255, + "target_component": 0, + "seq": 1, + "frame": 3, + "command": 22, + "current": 0, + "autocontinue": 1, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "x": 0, + "y": 0, + "z": 30.0, + "mission_type": 0 + }, + { + "mavpackettype": "MISSION_ITEM_INT", + "target_system": 255, + "target_component": 0, + "seq": 2, + "frame": 3, + "command": 16, + "current": 0, + "autocontinue": 1, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "x": 527803197, + "y": -7097930, + "z": 30.0, + "mission_type": 0 + }, + { + "mavpackettype": "MISSION_ITEM_INT", + "target_system": 255, + "target_component": 0, + "seq": 3, + "frame": 3, + "command": 16, + "current": 0, + "autocontinue": 1, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "x": 527812283, + "y": -7098949, + "z": 30.0, + "mission_type": 0 + }, + { + "mavpackettype": "MISSION_ITEM_INT", + "target_system": 255, + "target_component": 0, + "seq": 4, + "frame": 3, + "command": 16, + "current": 0, + "autocontinue": 1, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "x": 527816988, + "y": -7079530, + "z": 30.0, + "mission_type": 0 + }, + { + "mavpackettype": "MISSION_ITEM_INT", + "target_system": 255, + "target_component": 0, + "seq": 5, + "frame": 3, + "command": 16, + "current": 0, + "autocontinue": 1, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "x": 527814424, + "y": -7057160, + "z": 30.0, + "mission_type": 0 + }, + { + "mavpackettype": "MISSION_ITEM_INT", + "target_system": 255, + "target_component": 0, + "seq": 6, + "frame": 3, + "command": 16, + "current": 0, + "autocontinue": 1, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "x": 527807967, + "y": -7065958, + "z": 30.0, + "mission_type": 0 + }, + { + "mavpackettype": "MISSION_ITEM_INT", + "target_system": 255, + "target_component": 0, + "seq": 7, + "frame": 3, + "command": 21, + "current": 0, + "autocontinue": 1, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 1.0, + "x": 527804398, + "y": -7081997, + "z": 0.0, + "mission_type": 0 + } + ] +} diff --git a/radio/tests/mission_test_files/test_getCurrentMission_correctRally_result.json b/radio/tests/mission_test_files/test_getCurrentMission_correctRally_result.json new file mode 100644 index 000000000..0fcfa133a --- /dev/null +++ b/radio/tests/mission_test_files/test_getCurrentMission_correctRally_result.json @@ -0,0 +1,42 @@ +{ + "success": true, + "mission_type": "rally", + "items": [ + { + "autocontinue": 0, + "command": 5100, + "current": 0, + "frame": 3, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 2, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 0, + "target_component": 0, + "target_system": 255, + "x": 527810011, + "y": -7094872, + "z": 10.0 + }, + { + "autocontinue": 0, + "command": 5100, + "current": 0, + "frame": 3, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 2, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 1, + "target_component": 0, + "target_system": 255, + "x": 527821108, + "y": -7056785, + "z": 10.0 + } + ] +} diff --git a/radio/tests/mission_test_files/test_getCurrentMission_correctState_result.json b/radio/tests/mission_test_files/test_getCurrentMission_correctState_result.json new file mode 100644 index 000000000..9395ccca4 --- /dev/null +++ b/radio/tests/mission_test_files/test_getCurrentMission_correctState_result.json @@ -0,0 +1,422 @@ +{ + "mission_items": [ + { + "autocontinue": 1, + "command": 16, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 0, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 0, + "target_component": 0, + "target_system": 255, + "x": 527805690, + "y": -7079236, + "z": 0.09999999403953552 + }, + { + "mavpackettype": "MISSION_ITEM_INT", + "target_system": 255, + "target_component": 0, + "seq": 1, + "frame": 3, + "command": 22, + "current": 0, + "autocontinue": 1, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "x": 0, + "y": 0, + "z": 30.0, + "mission_type": 0 + }, + { + "mavpackettype": "MISSION_ITEM_INT", + "target_system": 255, + "target_component": 0, + "seq": 2, + "frame": 3, + "command": 16, + "current": 0, + "autocontinue": 1, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "x": 527803197, + "y": -7097930, + "z": 30.0, + "mission_type": 0 + }, + { + "mavpackettype": "MISSION_ITEM_INT", + "target_system": 255, + "target_component": 0, + "seq": 3, + "frame": 3, + "command": 16, + "current": 0, + "autocontinue": 1, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "x": 527812283, + "y": -7098949, + "z": 30.0, + "mission_type": 0 + }, + { + "mavpackettype": "MISSION_ITEM_INT", + "target_system": 255, + "target_component": 0, + "seq": 4, + "frame": 3, + "command": 16, + "current": 0, + "autocontinue": 1, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "x": 527816988, + "y": -7079530, + "z": 30.0, + "mission_type": 0 + }, + { + "mavpackettype": "MISSION_ITEM_INT", + "target_system": 255, + "target_component": 0, + "seq": 5, + "frame": 3, + "command": 16, + "current": 0, + "autocontinue": 1, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "x": 527814424, + "y": -7057160, + "z": 30.0, + "mission_type": 0 + }, + { + "mavpackettype": "MISSION_ITEM_INT", + "target_system": 255, + "target_component": 0, + "seq": 6, + "frame": 3, + "command": 16, + "current": 0, + "autocontinue": 1, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "x": 527807967, + "y": -7065958, + "z": 30.0, + "mission_type": 0 + }, + { + "mavpackettype": "MISSION_ITEM_INT", + "target_system": 255, + "target_component": 0, + "seq": 7, + "frame": 3, + "command": 21, + "current": 0, + "autocontinue": 1, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 1.0, + "x": 527804398, + "y": -7081997, + "z": 0.0, + "mission_type": 0 + } + ], + "fence_items": [ + { + "autocontinue": 0, + "command": 5001, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 6.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 0, + "target_component": 0, + "target_system": 255, + "x": 527814618, + "y": -7105708, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5001, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 6.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 1, + "target_component": 0, + "target_system": 255, + "x": 527810530, + "y": -7102918, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5001, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 6.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 2, + "target_component": 0, + "target_system": 255, + "x": 527800342, + "y": -7108390, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5001, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 6.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 3, + "target_component": 0, + "target_system": 255, + "x": 527818512, + "y": -7035434, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5001, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 6.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 4, + "target_component": 0, + "target_system": 255, + "x": 527830582, + "y": -7060110, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5001, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 6.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 5, + "target_component": 0, + "target_system": 255, + "x": 527825456, + "y": -7081568, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5004, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 25.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 6, + "target_component": 0, + "target_system": 255, + "x": 527796578, + "y": -7092163, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5003, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 50.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 7, + "target_component": 0, + "target_system": 255, + "x": 527801900, + "y": -7044017, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5002, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 5.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 8, + "target_component": 0, + "target_system": 255, + "x": 527828246, + "y": -7061076, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5002, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 5.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 9, + "target_component": 0, + "target_system": 255, + "x": 527818707, + "y": -7041549, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5002, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 5.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 10, + "target_component": 0, + "target_system": 255, + "x": 527814489, + "y": -7058179, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5002, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 5.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 11, + "target_component": 0, + "target_system": 255, + "x": 527821887, + "y": -7058072, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5002, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 5.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 12, + "target_component": 0, + "target_system": 255, + "x": 527826364, + "y": -7068908, + "z": 0.0 + } + ], + "rally_items": [ + { + "autocontinue": 0, + "command": 5100, + "current": 0, + "frame": 3, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 2, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 0, + "target_component": 0, + "target_system": 255, + "x": 527810011, + "y": -7094872, + "z": 10.0 + }, + { + "autocontinue": 0, + "command": 5100, + "current": 0, + "frame": 3, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 2, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 1, + "target_component": 0, + "target_system": 255, + "x": 527821108, + "y": -7056785, + "z": 10.0 + } + ] +} diff --git a/radio/tests/mission_test_files/test_writeCurrentMission_uploadFenceSuccess_data.json b/radio/tests/mission_test_files/test_writeCurrentMission_uploadFenceSuccess_data.json new file mode 100644 index 000000000..460a0305f --- /dev/null +++ b/radio/tests/mission_test_files/test_writeCurrentMission_uploadFenceSuccess_data.json @@ -0,0 +1,239 @@ +{ + "type": "fence", + "items": [ + { + "autocontinue": 0, + "command": 5001, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 6.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 0, + "target_component": 0, + "target_system": 255, + "x": 527814618, + "y": -7105708, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5001, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 6.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 1, + "target_component": 0, + "target_system": 255, + "x": 527810530, + "y": -7102918, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5001, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 6.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 2, + "target_component": 0, + "target_system": 255, + "x": 527800342, + "y": -7108390, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5001, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 6.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 3, + "target_component": 0, + "target_system": 255, + "x": 527818512, + "y": -7035434, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5001, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 6.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 4, + "target_component": 0, + "target_system": 255, + "x": 527830582, + "y": -7060110, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5001, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 6.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 5, + "target_component": 0, + "target_system": 255, + "x": 527825456, + "y": -7081568, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5004, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 25.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 6, + "target_component": 0, + "target_system": 255, + "x": 527796578, + "y": -7092163, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5003, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 50.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 7, + "target_component": 0, + "target_system": 255, + "x": 527801900, + "y": -7044017, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5002, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 5.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 8, + "target_component": 0, + "target_system": 255, + "x": 527828246, + "y": -7061076, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5002, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 5.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 9, + "target_component": 0, + "target_system": 255, + "x": 527818707, + "y": -7041549, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5002, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 5.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 10, + "target_component": 0, + "target_system": 255, + "x": 527814489, + "y": -7058179, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5002, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 5.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 11, + "target_component": 0, + "target_system": 255, + "x": 527821887, + "y": -7058072, + "z": 0.0 + }, + { + "autocontinue": 0, + "command": 5002, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 1, + "param1": 5.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 12, + "target_component": 0, + "target_system": 255, + "x": 527826364, + "y": -7068908, + "z": 0.0 + } + ] +} diff --git a/radio/tests/mission_test_files/test_writeCurrentMission_uploadMissionSuccess_data.json b/radio/tests/mission_test_files/test_writeCurrentMission_uploadMissionSuccess_data.json new file mode 100644 index 000000000..1070d0f12 --- /dev/null +++ b/radio/tests/mission_test_files/test_writeCurrentMission_uploadMissionSuccess_data.json @@ -0,0 +1,149 @@ +{ + "type": "mission", + "items": [ + { + "autocontinue": 1, + "command": 16, + "current": 0, + "frame": 0, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 0, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 0, + "target_component": 0, + "target_system": 255, + "x": 527805690, + "y": -7079236, + "z": 0.09999999403953552 + }, + { + "mavpackettype": "MISSION_ITEM_INT", + "target_system": 255, + "target_component": 0, + "seq": 1, + "frame": 3, + "command": 22, + "current": 0, + "autocontinue": 1, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "x": 0, + "y": 0, + "z": 30.0, + "mission_type": 0 + }, + { + "mavpackettype": "MISSION_ITEM_INT", + "target_system": 255, + "target_component": 0, + "seq": 2, + "frame": 3, + "command": 16, + "current": 0, + "autocontinue": 1, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "x": 527803200, + "y": -7097929, + "z": 30.0, + "mission_type": 0 + }, + { + "mavpackettype": "MISSION_ITEM_INT", + "target_system": 255, + "target_component": 0, + "seq": 3, + "frame": 3, + "command": 16, + "current": 0, + "autocontinue": 1, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "x": 527812256, + "y": -7098949, + "z": 30.0, + "mission_type": 0 + }, + { + "mavpackettype": "MISSION_ITEM_INT", + "target_system": 255, + "target_component": 0, + "seq": 4, + "frame": 3, + "command": 16, + "current": 0, + "autocontinue": 1, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "x": 527816992, + "y": -7079530, + "z": 30.0, + "mission_type": 0 + }, + { + "mavpackettype": "MISSION_ITEM_INT", + "target_system": 255, + "target_component": 0, + "seq": 5, + "frame": 3, + "command": 16, + "current": 0, + "autocontinue": 1, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "x": 527814400, + "y": -7057160, + "z": 30.0, + "mission_type": 0 + }, + { + "mavpackettype": "MISSION_ITEM_INT", + "target_system": 255, + "target_component": 0, + "seq": 6, + "frame": 3, + "command": 16, + "current": 0, + "autocontinue": 1, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "x": 527807968, + "y": -7065958, + "z": 30.0, + "mission_type": 0 + }, + { + "mavpackettype": "MISSION_ITEM_INT", + "target_system": 255, + "target_component": 0, + "seq": 7, + "frame": 3, + "command": 21, + "current": 0, + "autocontinue": 1, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 1.0, + "x": 527804416, + "y": -7081997, + "z": 0.0, + "mission_type": 0 + } + ] +} diff --git a/radio/tests/mission_test_files/test_writeCurrentMission_uploadRallySuccess_data.json b/radio/tests/mission_test_files/test_writeCurrentMission_uploadRallySuccess_data.json new file mode 100644 index 000000000..53f2143b8 --- /dev/null +++ b/radio/tests/mission_test_files/test_writeCurrentMission_uploadRallySuccess_data.json @@ -0,0 +1,41 @@ +{ + "type": "rally", + "items": [ + { + "autocontinue": 0, + "command": 5100, + "current": 0, + "frame": 3, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 2, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 0, + "target_component": 0, + "target_system": 255, + "x": 527810011, + "y": -7094872, + "z": 10.0 + }, + { + "autocontinue": 0, + "command": 5100, + "current": 0, + "frame": 3, + "mavpackettype": "MISSION_ITEM_INT", + "mission_type": 2, + "param1": 0.0, + "param2": 0.0, + "param3": 0.0, + "param4": 0.0, + "seq": 1, + "target_component": 0, + "target_system": 255, + "x": 527821108, + "y": -7056785, + "z": 10.0 + } + ] +} diff --git a/radio/tests/mission_test_files/upload_mission_helper.py b/radio/tests/mission_test_files/upload_mission_helper.py new file mode 100644 index 000000000..f81fd4f7f --- /dev/null +++ b/radio/tests/mission_test_files/upload_mission_helper.py @@ -0,0 +1,77 @@ +from pymavlink import mavutil, mavwp + + +def uploadMission(file_name, mission_type, master): + with open(file_name, "r") as f: + lines = f.readlines() + + if not lines[0].startswith("QGC WPL 110"): + raise Exception("File is not supported WP version") + + if mission_type == "mission": + loader = mavwp.MAVWPLoader() + mission_type_int = 0 + elif mission_type == "fence": + loader = mavwp.MissionItemProtocol_Fence() + mission_type_int = 1 + elif mission_type == "rally": + loader = mavwp.MissionItemProtocol_Rally() + mission_type_int = 2 + else: + raise ValueError(f"Invalid mission type: {mission_type}") + + for i, line in enumerate(lines[1:]): + linearray = line.split("\t") + if len(linearray) < 12 or line.startswith("#"): + continue # Skip lines that do not have enough data or are comments + ln_seq = int(linearray[0]) + ln_current = int(linearray[1]) + ln_frame = int(linearray[2]) + ln_command = int(linearray[3]) + ln_param1 = float(linearray[4]) + ln_param2 = float(linearray[5]) + ln_param3 = float(linearray[6]) + ln_param4 = float(linearray[7]) + ln_x = float(linearray[8]) + ln_y = float(linearray[9]) + ln_z = float(linearray[10]) + ln_autocontinue = int(float(linearray[11].strip())) + + # For fence/rally, the first line is a home location so don't add it + if mission_type != "mission" and i == 0: + continue + + wp_item = mavutil.mavlink.MAVLink_mission_item_int_message( + master.target_system, + master.target_component, + ln_seq, + ln_frame, + ln_command, + ln_current, + ln_autocontinue, + ln_param1, + ln_param2, + ln_param3, + ln_param4, + int(ln_x * 1e7), + int(ln_y * 1e7), + ln_z, + mission_type_int, + ) + loader.add(wp_item) + + master.mav.mission_count_send( + master.target_system, + master.target_component, + loader.count(), + mission_type=mission_type_int, + ) + + for i in range(loader.count()): + msg = master.recv_match(type=["MISSION_REQUEST"], blocking=True, timeout=3) + if msg is None: + raise TimeoutError("Did not receive MISSION_REQUEST message within timeout") + + master.mav.send(loader.wp(msg.seq)) + + print(f"Uploaded {mission_type} with {loader.count()} items") diff --git a/radio/tests/test_MissionController.py b/radio/tests/test_MissionController.py new file mode 100644 index 000000000..259ff53f2 --- /dev/null +++ b/radio/tests/test_MissionController.py @@ -0,0 +1,87 @@ +import pytest +from app.controllers.missionController import MissionController + + +class DummyDrone: + target_system = 1 + target_component = 1 + + +@pytest.fixture +def controller(): + return MissionController(DummyDrone()) + + +def test_checkMissionType_valid_mission(controller): + resp = controller._checkMissionType(0) + assert resp["success"] is True + + +def test_checkMissionType_valid_fence(controller): + resp = controller._checkMissionType(1) + assert resp["success"] is True + + +def test_checkMissionType_valid_rally(controller): + resp = controller._checkMissionType(2) + assert resp["success"] is True + + +def test_checkMissionType_invalid_type(controller): + resp = controller._checkMissionType(9999) + assert resp["success"] is False + + +def test_convertCoordinate_float_to_int(controller): + result = controller._convertCoordinate(52.7814618) + assert isinstance(result, (int, float)) + assert result == 527814618 + + +def test_convertCoordinate_int_to_float(controller): + result = controller._convertCoordinate(527814618) + assert isinstance(result, (int, float)) + assert result == 52.7814618 + + +def test_convertCoordinate_invalid_type_raises(controller): + with pytest.raises(ValueError) as excinfo: + controller._convertCoordinate("not_a_number") + + assert ( + str(excinfo.value) + == "Invalid coordinate type . Must be int or float." + ) + + +def test_getMissionName_mission(controller): + name = controller._getMissionName(0) + assert name == "mission" + + +def test_getMissionName_fence(controller): + name = controller._getMissionName(1) + assert name == "fence" + + +def test_getMissionName_rally(controller): + name = controller._getMissionName(2) + assert name == "rally" + + +def test_getMissionName_invalid_type_raises(controller): + with pytest.raises(ValueError) as excinfo: + controller._getMissionName(9999) + + assert str(excinfo.value) == "Invalid mission type 9999" + + +def test_getCommandName_known_command(controller): + name = controller._getCommandName(16) # Example known command + assert isinstance(name, str) + assert name == "MAV_CMD_NAV_WAYPOINT" + + +def test_getCommandName_unknown_command(controller): + name = controller._getCommandName(9999999) # Example unknown command + assert name == "Unknown command 9999999" diff --git a/radio/tests/test_mission.py b/radio/tests/test_mission.py index 35f1ff928..6ef5d78b5 100644 --- a/radio/tests/test_mission.py +++ b/radio/tests/test_mission.py @@ -1,11 +1,60 @@ +import json +import os + +import pytest from flask_socketio.test_client import SocketIOTestClient from . import falcon_test from .helpers import NoDrone +from .mission_test_files.upload_mission_helper import uploadMission + +MISSION_FILES_PATH = os.path.join( + os.path.dirname(__file__), + "mission_test_files", +) + + +@pytest.fixture() +def upload_default_mission(): + """ + Uploads the default mission, fence, and rally files to the drone before running a test. + """ + # Setup + + # Should be imported after the fixture to ensure the droneStatus is fresh + import app.droneStatus as droneStatus + + assert droneStatus.drone is not None, "Drone must be connected before running tests" + + droneStatus.drone.is_listening = False + + uploadMission( + os.path.join(MISSION_FILES_PATH, "default_mission.txt"), + "mission", + droneStatus.drone.master, + ) + uploadMission( + os.path.join(MISSION_FILES_PATH, "default_fence.txt"), + "fence", + droneStatus.drone.master, + ) + uploadMission( + os.path.join(MISSION_FILES_PATH, "default_rally.txt"), + "rally", + droneStatus.drone.master, + ) + + droneStatus.drone.is_listening = True + + yield # this is where the testing happens + + # Teardown @falcon_test(pass_drone_status=True) -def test_getCurrentMission_wrongState(socketio_client: SocketIOTestClient, droneStatus): +def test_getCurrentMissionAll_wrongState( + socketio_client: SocketIOTestClient, droneStatus +): droneStatus.state = "params" socketio_client.emit("get_current_mission_all") socketio_result = socketio_client.get_received()[0] @@ -16,8 +65,9 @@ def test_getCurrentMission_wrongState(socketio_client: SocketIOTestClient, drone } +@pytest.mark.usefixtures("upload_default_mission") @falcon_test(pass_drone_status=True) -def test_getCurrentMission_correctState( +def test_getCurrentMissionAll_correctState( socketio_client: SocketIOTestClient, droneStatus ): droneStatus.state = "dashboard" @@ -26,161 +76,19 @@ def test_getCurrentMission_correctState( assert socketio_result["name"] == "current_mission_all" # Correct name emitted - # pytest.skip(reason="Sending mission to simulator is currently bugged and fails sometimes") - assert socketio_result["args"][0] == { - "mission_items": [ - { - "autocontinue": 1, - "command": 16, - "current": 0, - "frame": 0, - "mavpackettype": "MISSION_ITEM_INT", - "mission_type": 0, - "param1": 0.0, - "param2": 0.0, - "param3": 0.0, - "param4": 0.0, - "seq": 0, - "target_component": 0, - "target_system": 255, - "x": 527805690, - "y": -7079236, - "z": 0.09999999403953552, - }, - { - "mavpackettype": "MISSION_ITEM_INT", - "target_system": 255, - "target_component": 0, - "seq": 1, - "frame": 3, - "command": 22, - "current": 0, - "autocontinue": 1, - "param1": 0.0, - "param2": 0.0, - "param3": 0.0, - "param4": 0.0, - "x": 0, - "y": 0, - "z": 30.0, - "mission_type": 0, - }, - { - "mavpackettype": "MISSION_ITEM_INT", - "target_system": 255, - "target_component": 0, - "seq": 2, - "frame": 3, - "command": 16, - "current": 0, - "autocontinue": 1, - "param1": 0.0, - "param2": 0.0, - "param3": 0.0, - "param4": 0.0, - "x": 527803200, - "y": -7097929, - "z": 30.0, - "mission_type": 0, - }, - { - "mavpackettype": "MISSION_ITEM_INT", - "target_system": 255, - "target_component": 0, - "seq": 3, - "frame": 3, - "command": 16, - "current": 0, - "autocontinue": 1, - "param1": 0.0, - "param2": 0.0, - "param3": 0.0, - "param4": 0.0, - "x": 527812256, - "y": -7098949, - "z": 30.0, - "mission_type": 0, - }, - { - "mavpackettype": "MISSION_ITEM_INT", - "target_system": 255, - "target_component": 0, - "seq": 4, - "frame": 3, - "command": 16, - "current": 0, - "autocontinue": 1, - "param1": 0.0, - "param2": 0.0, - "param3": 0.0, - "param4": 0.0, - "x": 527816992, - "y": -7079530, - "z": 30.0, - "mission_type": 0, - }, - { - "mavpackettype": "MISSION_ITEM_INT", - "target_system": 255, - "target_component": 0, - "seq": 5, - "frame": 3, - "command": 16, - "current": 0, - "autocontinue": 1, - "param1": 0.0, - "param2": 0.0, - "param3": 0.0, - "param4": 0.0, - "x": 527814400, - "y": -7057160, - "z": 30.0, - "mission_type": 0, - }, - { - "mavpackettype": "MISSION_ITEM_INT", - "target_system": 255, - "target_component": 0, - "seq": 6, - "frame": 3, - "command": 16, - "current": 0, - "autocontinue": 1, - "param1": 0.0, - "param2": 0.0, - "param3": 0.0, - "param4": 0.0, - "x": 527807968, - "y": -7065958, - "z": 30.0, - "mission_type": 0, - }, - { - "mavpackettype": "MISSION_ITEM_INT", - "target_system": 255, - "target_component": 0, - "seq": 7, - "frame": 3, - "command": 21, - "current": 0, - "autocontinue": 1, - "param1": 0.0, - "param2": 0.0, - "param3": 0.0, - "param4": 1.0, - "x": 527804416, - "y": -7081997, - "z": 0.0, - "mission_type": 0, - }, - ], - "fence_items": [], - "rally_items": [], - } + with open( + os.path.join( + MISSION_FILES_PATH, "test_getCurrentMission_correctState_result.json" + ), + "r", + ) as f: + result_data = json.load(f) + + assert socketio_result["args"][0] == result_data @falcon_test(pass_drone_status=True) -def test_getCurrentMission_noDroneConnection( +def test_getCurrentMissionAll_noDroneConnection( socketio_client: SocketIOTestClient, droneStatus ): droneStatus.state = "dashboard" @@ -195,6 +103,84 @@ def test_getCurrentMission_noDroneConnection( } +@falcon_test(pass_drone_status=True) +def test_getCurrentMission_wrongState(socketio_client: SocketIOTestClient, droneStatus): + droneStatus.state = "params" + socketio_client.emit("get_current_mission", {"type": "mission"}) + socketio_result = socketio_client.get_received()[0] + + assert socketio_result["name"] == "params_error" # Correct name emitted + assert socketio_result["args"][0] == { + "message": "You must be on the dashboard or missions screen to get the current mission." + } + + +@pytest.mark.usefixtures("upload_default_mission") +@falcon_test(pass_drone_status=True) +def test_getCurrentMission_correctMission( + socketio_client: SocketIOTestClient, droneStatus +): + droneStatus.state = "missions" + socketio_client.emit("get_current_mission", {"type": "mission"}) + socketio_result = socketio_client.get_received()[-1] + + assert socketio_result["name"] == "current_mission" + + with open( + os.path.join( + MISSION_FILES_PATH, "test_getCurrentMission_correctMission_result.json" + ), + "r", + ) as f: + result_data = json.load(f) + + assert socketio_result["args"][0] == result_data + + +@pytest.mark.usefixtures("upload_default_mission") +@falcon_test(pass_drone_status=True) +def test_getCurrentMission_correctFence( + socketio_client: SocketIOTestClient, droneStatus +): + droneStatus.state = "missions" + socketio_client.emit("get_current_mission", {"type": "fence"}) + socketio_result = socketio_client.get_received()[-1] + + assert socketio_result["name"] == "current_mission" + + with open( + os.path.join( + MISSION_FILES_PATH, "test_getCurrentMission_correctFence_result.json" + ), + "r", + ) as f: + result_data = json.load(f) + + assert socketio_result["args"][0] == result_data + + +@pytest.mark.usefixtures("upload_default_mission") +@falcon_test(pass_drone_status=True) +def test_getCurrentMission_correctRally( + socketio_client: SocketIOTestClient, droneStatus +): + droneStatus.state = "missions" + socketio_client.emit("get_current_mission", {"type": "rally"}) + socketio_result = socketio_client.get_received()[-1] + + assert socketio_result["name"] == "current_mission" + + with open( + os.path.join( + MISSION_FILES_PATH, "test_getCurrentMission_correctRally_result.json" + ), + "r", + ) as f: + result_data = json.load(f) + + assert socketio_result["args"][0] == result_data + + @falcon_test(pass_drone_status=True) def test_writeCurrentMission_wrongState( socketio_client: SocketIOTestClient, droneStatus @@ -210,160 +196,65 @@ def test_writeCurrentMission_wrongState( @falcon_test(pass_drone_status=True) -def test_writeCurrentMission_correctState( +def test_writeCurrentMission_uploadMissionSuccess( socketio_client: SocketIOTestClient, droneStatus ): droneStatus.state = "missions" - data = { - "type": "mission", - "items": [ - { - "autocontinue": 1, - "command": 16, - "current": 0, - "frame": 0, - "mavpackettype": "MISSION_ITEM_INT", - "mission_type": 0, - "param1": 0.0, - "param2": 0.0, - "param3": 0.0, - "param4": 0.0, - "seq": 0, - "target_component": 0, - "target_system": 255, - "x": 527805690, - "y": -7079236, - "z": 0.09999999403953552, - }, - { - "mavpackettype": "MISSION_ITEM_INT", - "target_system": 255, - "target_component": 0, - "seq": 1, - "frame": 3, - "command": 22, - "current": 0, - "autocontinue": 1, - "param1": 0.0, - "param2": 0.0, - "param3": 0.0, - "param4": 0.0, - "x": 0, - "y": 0, - "z": 30.0, - "mission_type": 0, - }, - { - "mavpackettype": "MISSION_ITEM_INT", - "target_system": 255, - "target_component": 0, - "seq": 2, - "frame": 3, - "command": 16, - "current": 0, - "autocontinue": 1, - "param1": 0.0, - "param2": 0.0, - "param3": 0.0, - "param4": 0.0, - "x": 527803200, - "y": -7097929, - "z": 30.0, - "mission_type": 0, - }, - { - "mavpackettype": "MISSION_ITEM_INT", - "target_system": 255, - "target_component": 0, - "seq": 3, - "frame": 3, - "command": 16, - "current": 0, - "autocontinue": 1, - "param1": 0.0, - "param2": 0.0, - "param3": 0.0, - "param4": 0.0, - "x": 527812256, - "y": -7098949, - "z": 30.0, - "mission_type": 0, - }, - { - "mavpackettype": "MISSION_ITEM_INT", - "target_system": 255, - "target_component": 0, - "seq": 4, - "frame": 3, - "command": 16, - "current": 0, - "autocontinue": 1, - "param1": 0.0, - "param2": 0.0, - "param3": 0.0, - "param4": 0.0, - "x": 527816992, - "y": -7079530, - "z": 30.0, - "mission_type": 0, - }, - { - "mavpackettype": "MISSION_ITEM_INT", - "target_system": 255, - "target_component": 0, - "seq": 5, - "frame": 3, - "command": 16, - "current": 0, - "autocontinue": 1, - "param1": 0.0, - "param2": 0.0, - "param3": 0.0, - "param4": 0.0, - "x": 527814400, - "y": -7057160, - "z": 30.0, - "mission_type": 0, - }, - { - "mavpackettype": "MISSION_ITEM_INT", - "target_system": 255, - "target_component": 0, - "seq": 6, - "frame": 3, - "command": 16, - "current": 0, - "autocontinue": 1, - "param1": 0.0, - "param2": 0.0, - "param3": 0.0, - "param4": 0.0, - "x": 527807968, - "y": -7065958, - "z": 30.0, - "mission_type": 0, - }, - { - "mavpackettype": "MISSION_ITEM_INT", - "target_system": 255, - "target_component": 0, - "seq": 7, - "frame": 3, - "command": 21, - "current": 0, - "autocontinue": 1, - "param1": 0.0, - "param2": 0.0, - "param3": 0.0, - "param4": 1.0, - "x": 527804416, - "y": -7081997, - "z": 0.0, - "mission_type": 0, - }, - ], + with open( + os.path.join( + MISSION_FILES_PATH, + "test_writeCurrentMission_uploadMissionSuccess_data.json", + ), + "r", + ) as f: + data = json.load(f) + + socketio_client.emit("write_current_mission", data) + socketio_result = socketio_client.get_received()[-1] + + assert socketio_result["name"] == "write_mission_result" + assert socketio_result["args"][0] == { + "success": True, + "message": "Mission uploaded successfully", + } + + +@falcon_test(pass_drone_status=True) +def test_writeCurrentMission_uploadFenceSuccess( + socketio_client: SocketIOTestClient, droneStatus +): + droneStatus.state = "missions" + with open( + os.path.join( + MISSION_FILES_PATH, "test_writeCurrentMission_uploadFenceSuccess_data.json" + ), + "r", + ) as f: + data = json.load(f) + + socketio_client.emit("write_current_mission", data) + socketio_result = socketio_client.get_received()[-1] + + assert socketio_result["name"] == "write_mission_result" + assert socketio_result["args"][0] == { + "success": True, + "message": "Mission uploaded successfully", } + +@falcon_test(pass_drone_status=True) +def test_writeCurrentMission_uploadRallySuccess( + socketio_client: SocketIOTestClient, droneStatus +): + droneStatus.state = "missions" + with open( + os.path.join( + MISSION_FILES_PATH, "test_writeCurrentMission_uploadRallySuccess_data.json" + ), + "r", + ) as f: + data = json.load(f) + socketio_client.emit("write_current_mission", data) socketio_result = socketio_client.get_received()[-1] @@ -375,7 +266,7 @@ def test_writeCurrentMission_correctState( @falcon_test(pass_drone_status=True) -def test_writeCurrentMission_correctState_incorrectMissionType( +def test_writeCurrentMission_incorrectMissionType( socketio_client: SocketIOTestClient, droneStatus ): droneStatus.state = "missions" @@ -395,7 +286,7 @@ def test_writeCurrentMission_correctState_incorrectMissionType( @falcon_test(pass_drone_status=True) -def test_writeCurrentMission_correctState_noWaypoints( +def test_writeCurrentMission_noWaypoints( socketio_client: SocketIOTestClient, droneStatus ): droneStatus.state = "missions"