87 lines
2.9 KiB
Python
87 lines
2.9 KiB
Python
|
import time
|
||
|
|
import math
|
||
|
|
import sys
|
||
|
|
|
||
|
|
import PX4MavCtrlV4 as PX4MavCtrl
|
||
|
|
import UE4CtrlAPI
|
||
|
|
ue = UE4CtrlAPI.UE4CtrlAPI()
|
||
|
|
|
||
|
|
# Create four MAVLink API instances, the CopterSim UDP ports are 20100+2*i, where i=1,2,3,4
|
||
|
|
# Although no CopterSim is required in NoPX4Mode, the port 20100+2*i is required to identify the CopterID
|
||
|
|
mav = PX4MavCtrl.PX4MavCtrler(1)
|
||
|
|
mav1 = PX4MavCtrl.PX4MavCtrler(2)
|
||
|
|
mav2 = PX4MavCtrl.PX4MavCtrler(3)
|
||
|
|
mav3 = PX4MavCtrl.PX4MavCtrler(4)
|
||
|
|
|
||
|
|
|
||
|
|
# sendUE4Cmd: RflySim3D API to modify scene display style
|
||
|
|
# Format: ue.sendUE4Cmd(cmd,windowID=-1), where cmd is a command string, windowID is the received window number (assuming multiple RflySim3D windows are opened at the same time), windowID =-1 means sent to all windows
|
||
|
|
# Augument: RflyChangeMapbyName command means to switch the map (scene), the following string is the map name, here will switch all open windows to the grass map
|
||
|
|
ue.sendUE4Cmd('RflyChangeMapbyName Grasslands')
|
||
|
|
time.sleep(2)
|
||
|
|
|
||
|
|
# Start MAVLink listening loop from CopterSim/Pixhawk
|
||
|
|
# initPointMassModel(intAlt=0,intState=[0,0,0]):
|
||
|
|
# intAlt is the ground high which should be acquired from the UE4 map
|
||
|
|
# intState=PosX, PosY, PosZ, which can obtain intAlt at CopterSim UI
|
||
|
|
mav.initPointMassModel(-8.086,[0,0,0])
|
||
|
|
mav1.initPointMassModel(-8.29,[2,0,0])
|
||
|
|
mav2.initPointMassModel(-7.835,[0,2,0])
|
||
|
|
mav3.initPointMassModel(-7.788,[2,2,0])
|
||
|
|
time.sleep(2)
|
||
|
|
|
||
|
|
# Display the position, velocity, angle, angular rate from Pixhawk/CopterSIm
|
||
|
|
print((mav.uavPosNED,mav.truePosNED,mav.uavVelNED,mav.uavAngEular,mav.uavAngRate,mav.uavGlobalPos))
|
||
|
|
print((mav1.uavPosNED,mav1.truePosNED,mav1.uavVelNED,mav1.uavAngEular,mav1.uavAngRate,mav1.uavGlobalPos))
|
||
|
|
print((mav2.uavPosNED,mav2.truePosNED,mav2.uavVelNED,mav2.uavAngEular,mav2.uavAngRate,mav2.uavPosGPS,mav2.uavGlobalPos))
|
||
|
|
print((mav3.uavPosNED,mav3.truePosNED,mav3.uavVelNED,mav3.uavAngEular,mav3.uavAngRate,mav3.uavPosGPS,mav3.uavGlobalPos))
|
||
|
|
|
||
|
|
# 设定最大速度，这个比较重要！，根据任务来匹配机型速度
|
||
|
|
mav.SendCopterSpeed(10)
|
||
|
|
mav1.SendCopterSpeed(10)
|
||
|
|
mav2.SendCopterSpeed(10)
|
||
|
|
mav3.SendCopterSpeed(10)
|
||
|
|
time.sleep(2)
|
||
|
|
|
||
|
|
# send desired poisition control command, fly to target postion 0,0,-1.7m with yaw 0 degree
|
||
|
|
mav.SendPosNED(0, 0, -1.7, 0)
|
||
|
|
mav1.SendPosNED(2, 2, -1.7, 0)
|
||
|
|
mav2.SendPosNED(-2, -2, -1.7, 0)
|
||
|
|
mav3.SendPosNED(0, 0, -3, 0)
|
||
|
|
|
||
|
|
|
||
|
|
print("Send target Pos")
|
||
|
|
time.sleep(0.5)
|
||
|
|
# Send arm command
|
||
|
|
mav.SendMavArm(True)
|
||
|
|
time.sleep(2)
|
||
|
|
mav1.SendMavArm(True)
|
||
|
|
time.sleep(2)
|
||
|
|
mav2.SendMavArm(True)
|
||
|
|
time.sleep(2)
|
||
|
|
mav3.SendMavArm(True)
|
||
|
|
print("Send Arm Command")
|
||
|
|
|
||
|
|
time.sleep(5)
|
||
|
|
|
||
|
|
# send desired NED velocity signals, 0.2m/s download
|
||
|
|
mav.SendVelNED(0, 0, 1, 0)
|
||
|
|
mav1.SendVelNED(0, 0, 1, 0)
|
||
|
|
mav2.SendVelNED(0, 0, 1, 0)
|
||
|
|
mav3.SendVelNED(0, 0, 1, 0)
|
||
|
|
print("Send Velocity Speed")
|
||
|
|
|
||
|
|
time.sleep(5)
|
||
|
|
|
||
|
|
|
||
|
|
# exit from MAVLink listening loop
|
||
|
|
print("End pointMass model")
|
||
|
|
mav.EndPointMassModel()
|
||
|
|
mav1.EndPointMassModel()
|
||
|
|
mav2.EndPointMassModel()
|
||
|
|
mav3.EndPointMassModel()
|
||
|
|
time.sleep(1)
|
||
|
|
# while True:
|
||
|
|
# print(mav.uavPosNED)
|
||
|
|
# time.sleep(2)
|