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)