fix：新增例程

基础智能/e5.2机碰撞实验/CollisionDemo.py

@@ -0,0 +1,95 @@
+import time
+import math
+import sys
+
+import PX4MavCtrlV4 as PX4MavCtrl
+import UE4CtrlAPI
+ue = UE4CtrlAPI.UE4CtrlAPI()
+
+#Create a new MAVLink communication instance, UDP sending port (CopterSim’s receving port) is 20100
+mav = PX4MavCtrl.PX4MavCtrler(1)
+mav2 = PX4MavCtrl.PX4MavCtrler(2)
+
+ue.sendUE4Cmd('RflyChangeViewKeyCmd S',0) #开启标号显示
+time.sleep(0.5)
+ue.sendUE4Cmd('RflyChangeViewKeyCmd T',0) #开启标号显示
+time.sleep(0.5)
+ue.sendUE4Cmd('RflyChangeViewKeyCmd P',0)  #开启碰撞引擎
+time.sleep(5)
+
+
+#Turn on MAVLink to monitor CopterSim data and update it in real time. 
+mav.InitMavLoop()
+mav2.InitMavLoop()
+time.sleep(0.5)
+
+
+#Display Position information received from CopterSim
+print(mav.uavPosNED)
+
+
+#Turn on Offboard mode
+mav.initOffboard()
+mav2.initOffboard()
+# Send the desired position signal, fly to the target point 0,0, -1.7 position, the yaw angle is 0
+
+
+mav.SendPosNED(0, 0, -1.7, 0) 
+mav2.SendPosNED(2, -2, -1.7, 0) 
+
+print("Send target Pos")
+#Send arm command to arm the drone
+mav.SendMavArm(True) 
+mav2.SendMavArm(True) 
+print("Send Arm Command")
+
+time.sleep(15)
+
+# Send the desired speed signal, 0.2m/s downwards, the z-axis downward is positive
+mav.SendVelNED(1, 0, 0, 0) 
+mav2.SendVelNED(-1, 0, 0, 0) 
+print("Send Velocity Speed")
+
+hasV1Crashed=False
+hasV2Crashed=False
+startTime = time.time()
+lastTime = time.time()
+timeInterval = 1/30.0 # time interval of the timer
+while True:
+    # The above code will be executed 30Hz (0.033333s)    
+    lastTime = lastTime + timeInterval
+    sleepTime = lastTime - time.time()
+    if sleepTime > 0:
+        time.sleep(sleepTime)
+    else:
+        lastTime = time.time()
+    
+    #使用mav.isVehicleCrash来检测飞机是否碰撞
+    if (not hasV1Crashed) and mav.isVehicleCrash:
+        print('Vehicle #1 Crashed with vehicle #',mav.isVehicleCrashID)
+        hasV1Crashed=True
+    
+    #使用mav2.isVehicleCrash来检测飞机是否碰撞
+    if (not hasV2Crashed) and mav2.isVehicleCrash:
+        print('Vehicle #2 Crashed with vehicle #',mav2.isVehicleCrashID)
+        hasV2Crashed=True
+        
+    if hasV2Crashed and hasV1Crashed:
+        time.sleep(4)
+        break
+
+
+#Exit Offboard control mode
+print("Send offboard stop")
+mav.endOffboard()
+mav2.endOffboard()
+time.sleep(1)
+
+#Exit MAVLink data receiving mode
+print("Send Mavlink stop")
+mav.stopRun()
+mav2.stopRun()
+time.sleep(1)
+#while True:
+#    print(mav.uavPosNED)
+#    time.sleep(2)