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)
|