fix：新增例程

基础智能/e2.4 机质点模型集群实验/NoPX4SITL4Swarm.py

@@ -0,0 +1,86 @@
+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)