83 lines
2.1 KiB
Python
83 lines
2.1 KiB
Python
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)
|