fix：新增例程

基础智能/e1.集群Python接口演示实验/MAVLinkFull4Swarm.py

@@ -0,0 +1,82 @@
+import time
+import math
+import sys
+
+import PX4MavCtrlV4 as PX4MavCtrl
+
+# Create four MAVLink API instances, the CopterSim UDP ports are 20100+2*i, where i=1,2,3,4
+mav = PX4MavCtrl.PX4MavCtrler(1)
+mav1 = PX4MavCtrl.PX4MavCtrler(2)
+mav2 = PX4MavCtrl.PX4MavCtrler(3)
+mav3 = PX4MavCtrl.PX4MavCtrler(4)
+
+# Start MAVLink listening loop from CopterSim/Pixhawk
+# MAVLink Full mode is adopted here
+mav.InitMavLoop()
+mav1.InitMavLoop()
+mav2.InitMavLoop()
+mav3.InitMavLoop()
+time.sleep(2)
+
+# Display the position, velocity, angle, angular rate from Pixhawk/CopterSIm
+print((mav.uavPosNED,mav.uavVelNED,mav.uavAngEular,mav.uavAngRate,mav.uavPosGPSHome,mav.uavPosGPS,mav.uavGlobalPos))
+print((mav1.uavPosNED,mav1.uavVelNED,mav1.uavAngEular,mav1.uavAngRate,mav1.uavPosGPSHome,mav1.uavPosGPS,mav1.uavGlobalPos))
+print((mav2.uavPosNED,mav2.uavVelNED,mav2.uavAngEular,mav2.uavAngRate,mav2.uavPosGPSHome,mav2.uavPosGPS,mav2.uavGlobalPos))
+print((mav3.uavPosNED,mav3.uavVelNED,mav3.uavAngEular,mav3.uavAngRate,mav3.uavPosGPSHome,mav3.uavPosGPS,mav3.uavGlobalPos))
+
+
+# Start Offboard mode of Pixhawk
+mav.initOffboard()
+mav1.initOffboard()
+mav2.initOffboard()
+mav3.initOffboard()
+
+
+# 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(10)
+
+# send desired NED velocity signals, 0.2m/s download
+mav.SendVelNED(0, 0, 0.2, 0) 
+mav1.SendVelNED(0, 0, 0.2, 0) 
+mav2.SendVelNED(0, 0, 0.2, 0) 
+mav3.SendVelNED(0, 0, 0.2, 0) 
+print("Send Velocity Speed")
+
+time.sleep(10)
+
+# exit from Offboard control mode of Pixhawk
+print("Send offboard stop")
+mav.endOffboard()
+mav1.endOffboard()
+mav2.endOffboard()
+mav3.endOffboard()
+time.sleep(1)
+
+# exit from MAVLink listening loop
+print("Send Mavlink stop")
+mav.stopRun()
+mav1.stopRun()
+mav2.stopRun()
+mav3.stopRun()
+time.sleep(1)
+#while True:
+#    print(mav.uavPosNED)
+#    time.sleep(2)