96 lines
2.4 KiB
Python
96 lines
2.4 KiB
Python
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)
|