84 lines
2.4 KiB
Python
84 lines
2.4 KiB
Python
|
import time
|
|||
|
|
import math
|
|||
|
|
import sys
|
|||
|
|
import numpy as np
|
|||
|
|
import PX4MavCtrlV4 as PX4MavCtrl
|
|||
|
|
VehilceNum = 3
|
|||
|
|
CarNum = 3
|
|||
|
|
|
|||
|
|
MavList=[]
|
|||
|
|
flagI = np.zeros(VehilceNum, dtype=bool)
|
|||
|
|
# Create MAV instance
|
|||
|
|
for i in range(VehilceNum + CarNum):
|
|||
|
|
MavList=MavList+[PX4MavCtrl.PX4MavCtrler(1+i)]
|
|||
|
|
|
|||
|
|
time.sleep(2)
|
|||
|
|
# Start MAV loop with UDP mode: Udp_Simple
|
|||
|
|
for i in range(VehilceNum+CarNum):
|
|||
|
|
MavList[i].InitMavLoop(1)
|
|||
|
|
|
|||
|
|
# Enter Offboard mode to start vehicle control
|
|||
|
|
time.sleep(2)
|
|||
|
|
for i in range(VehilceNum+CarNum):
|
|||
|
|
MavList[i].initOffboard()
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
|
|||
|
|
time.sleep(2)
|
|||
|
|
for i in range(VehilceNum+CarNum):
|
|||
|
|
MavList[i].SendMavArm(True)
|
|||
|
|
# Get the takeoff position of each vehicle to the UE4 Map
|
|||
|
|
# this can be adopted to obtain the global position of a vehicle in swarm simulation
|
|||
|
|
time.sleep(5)
|
|||
|
|
Error2UE4Map=[]
|
|||
|
|
for i in range(VehilceNum):
|
|||
|
|
mav=MavList[i]
|
|||
|
|
Error2UE4Map = Error2UE4Map+[-np.array([mav.uavGlobalPos[0]-mav.uavPosNED[0],mav.uavGlobalPos[1]-mav.uavPosNED[1],mav.uavGlobalPos[2]-mav.uavPosNED[2]])]
|
|||
|
|
|
|||
|
|
# fly to 10m high above its takeoff position
|
|||
|
|
for i in range(VehilceNum):
|
|||
|
|
MavList[i].SendPosNED(0, 0, -10, 0)
|
|||
|
|
time.sleep(10)
|
|||
|
|
|
|||
|
|
|
|||
|
|
lastTime = time.time()
|
|||
|
|
startTime = time.time()
|
|||
|
|
timeInterval = 1/30.0 #here is 0.0333s (30Hz)
|
|||
|
|
|
|||
|
|
while True:
|
|||
|
|
lastTime = lastTime + timeInterval
|
|||
|
|
sleepTime = lastTime - time.time()
|
|||
|
|
if sleepTime > 0:
|
|||
|
|
time.sleep(sleepTime) # sleep until the desired clock
|
|||
|
|
else:
|
|||
|
|
lastTime = time.time()
|
|||
|
|
# The following code will be executed 30Hz (0.0333s)
|
|||
|
|
|
|||
|
|
t=time.time()-startTime
|
|||
|
|
for j in range(VehilceNum):
|
|||
|
|
|
|||
|
|
mav=MavList[j]
|
|||
|
|
|
|||
|
|
if abs(mav.uavPosNED[2] - (-10)) <2 and (flagI[j] == False):
|
|||
|
|
flagI[j] = True
|
|||
|
|
print(str(j)+"号飞机已到达指定高度")
|
|||
|
|
|
|||
|
|
if flagI[j] == True:
|
|||
|
|
mav.SendPosNED(MavList[j+3].uavPosNED[0],MavList[j+3].uavPosNED[1],mav.uavPosNED[2],0)
|
|||
|
|
|
|||
|
|
for i in range(VehilceNum,VehilceNum+CarNum):
|
|||
|
|
if flagI[i-3] == True:
|
|||
|
|
# print(f"编号 {i-3}：北 {MavList[i-3].uavPosNED[0]:.2f} m，东 {MavList[i-3].uavPosNED[1]:.2f} m，地 {MavList[i-3].uavPosNED[2]:.2f} m")
|
|||
|
|
MavList[i].SendPosNEDNoYaw(MavList[i].uavPosNED[0]+2.5,MavList[i].uavPosNED[1],0)
|
|||
|
|
|
|||
|
|
|
|||
|
|
if t>200: # end simulation when 200s
|
|||
|
|
break
|
|||
|
|
|
|||
|
|
|
|||
|
|
for i in range(VehilceNum):
|
|||
|
|
MavList[i].endOffboard()
|
|||
|
|
|
|||
|
|
time.sleep(1)
|
|||
|
|
for i in range(VehilceNum):
|
|||
|
|
MavList[i].stopRun()
|