def __init__(self, port):
self.f = fifo
self.stopFlag = False
self.mav0 = mavlink2.MAVLink(self.f)
self.udp_socket = socket.socket(socket.AF_INET,
socket.SOCK_DGRAM) # 创建套接字
self.ip = '127.0.0.1' # 服务器ip和端口
self.port = port
self.uavAngEular = [0, 0, 0]
self.uavAngRate = [0, 0, 0]
self.uavPosNED = [0, 0, 0]
self.uavVelNED = [0, 0, 0]
self.EnList = [0, 1, 0, 0, 0, 1]
self.coordinate_frame = mavlink2.MAV_FRAME_BODY_NED
self.pos = [0, 0, 0]
self.vel = [0, 0, 0]
self.acc = [0, 0, 0]
self.yaw = 0
self.yawrate = 0
self.isInOffboard = False
self.isArmed = False
self.hasSendDisableRTLRC = False
self.ch5 = -1
self.ch6 = -1
self.ch9 = -1
self.ch10 = -1
print("Thread Started!")
def handleMissionTransmission(self, m, wp):
while True:
mavlinkMSG = mavlink.MAVLink(m)
msg = m.recv_match(blocking=True)
if not msg:
return
msgID = msg.get_msgId()
if msgID == mavlink.MAVLINK_MSG_ID_MISSION_REQUEST:
print("New mission request object for: %u" % msg.seq)
missionIndex = msg.seq
mavlinkMSG.send(wp.wp(missionIndex))
if missionIndex == wp.count() - 1:
print "I am done transmitting for this aircraft"
break
elif msgID == mavlink.MAVLINK_MSG_ID_BAD_DATA:
if (mavutil.all_printable(msg.data)):
sys.stdout.write(msg.data)
sys.stdout.flush()
elif msgID == mavlink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
aircraftLatitude = msg.lat
aircraftLongitude = msg.lon
else:
print(msg)
def __init__(self, comm_port):
# create a mavlink serial instance
self.mav_connection = mavutil.mavlink_connection(comm_port, baud=57600)
self.mav_link = mavlink.MAVLink(self.mav_connection)
self.home_location = None
self.location = None
self.in_mission = False
self.heading = 0
def heartbeatTimer(m, interval):
mavlinkMSG = mavlink.MAVLink(m)
while True:
time.sleep(interval)
mavlinkMSG.heartbeat_send(mavlink.MAV_TYPE_GCS, mavlink.MAV_AUTOPILOT_GENERIC, 0, 0, mavlink.MAV_STATE_ACTIVE)
home = LocationGlobal(vehicle.home_location.lat, vehicle.home_location.lon, vehicle.home_location.alt)
mavlinkMSG.mission_item_send(0, 0, 0, 0, 0, 0, 0, vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon,vehicle.location.global_relative_frame.alt, vehicle.heading, vehicle.home_location.lat, vehicle.home_location.lon, vehicle.home_location.alt)
# sock.bind(('',socket.htons(14451)))
# master = mavutil.mavlink_connection()
# gc = sockaddr_in()
# gc.sin_family = socket.AF_INET
# gc.sin_addr = "127.0.0.1"
# gc.sin_port = socket.htons(14550)
# gcAddr = ("127.0.0.1", 14550)
# sock.connect(gcAddr)
'''
f = fifo()
mav = mavlink1.MAVLink(f)
print("Untuk menutup program, tekan CTRL+C pada keyboard.")
print()
while True:
time_since_start += 100
try:
msg = mavcom.recv_msg()
if(msg):
print("Pesan diterima dari COM. ", end="")
mavudpout.write(msg.get_msgbuf())
print(f"MSG ID: {msg.get_msgId()} ", end="")
print("| Pesan dikirim melalui UDP.")
else:
print("Menunggu pesan dari COM.")
def runCommand(self):
# create a mavlink serial instance
aircraftOne = mavutil.mavlink_connection('com4', baud=57600)
self.aircraftComms.append(aircraftOne)
self.aircraftMsg.append(mavlink.MAVLink(aircraftOne))
# aircraftTwo = mavutil.mavlink_connection('/dev/ttyUSB1', baud=57600)
# self.aircraftComms.append(aircraftTwo)
# self.aircraftMsg.append(mavlink.MAVLink(aircraftTwo))
# master = mavutil.mavlink_connection('/dev/ttyUSB0', baud=57600)
# mavlinkMSG = mavlink.MAVLink(master)
# mavlinkMSG.command_long_send()
# wait for the heartbeat msg to find the system ID
for i in range(0, len(self.aircraftComms), 1):
self.wait_heartbeat(i, self.aircraftComms[i])
print "I saw a heartbeat, now I am going to acknowledge I have seen the aircraft"
self.aircraftMsg[i].command_ack_send(0, 0)
print "The value I am trying to put in is: %s" % i
self.wait_homeLocation(i, self.aircraftComms[i])
self.sortedAircraftOrder = self.sortWesternPoints(self.aircraftHome)
for i in range(0, len(self.aircraftComms), 1):
wp = mavwp.MAVWPLoader()
seq = 1
frame = mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT
radius = 5
currentTargets = self.positionArray[i]
for j in range(0, len(currentTargets), 1):
currentPosition = currentTargets[j]
print "The current position is %s" % currentPosition
wp.add(
mavlink.MAVLink_mission_item_message(
self.aircraftComms[i].target_system,
self.aircraftComms[i].target_component, i, frame,
mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, radius, 0, 0,
currentPosition[0], currentPosition[1],
self.targetAltitude))
transmitVehicle = self.sortedAircraftOrder[i]
self.aircraftMsg[transmitVehicle].mission_clear_all_send(
self.aircraftComms[transmitVehicle].target_system,
self.aircraftComms[transmitVehicle].target_component)
print "I am done sending the clear all"
self.aircraftMsg[transmitVehicle].mission_count_send(
self.aircraftComms[transmitVehicle].target_system,
self.aircraftComms[transmitVehicle].target_component,
len(currentTargets))
print "I am done telling how many targets there are "
self.handleMissionTransmission(self.aircraftComms[transmitVehicle],
wp)
# tell the vehicle to takeoff
# Collect events until released
keyboard_listener = Listener(on_press=self.on_press)
# when done
keyboard_listener.start()
keyboard_listener.join()
def handleMissionMessage(m):
mavlinkMSG = mavlink.MAVLink(m)
global vehicle
global currentMissionIndex
global latArray
global lngArray
global altArray
global missionThread
global stopThread
'''show incoming mavlink messages'''
while True:
msg = m.recv_match(blocking=True)
if not msg:
continue
msgID = msg.get_msgId()
if msgID == mavlink.MAVLINK_MSG_ID_MISSION_ITEM:
if(msg.command == mavlink.MAV_CMD_NAV_WAYPOINT):
index = msg.seq + 1
logging.debug("I saw a new mission item at index %s",index)
latArray.insert(index, msg.x)
lngArray.insert(index, msg.y)
altArray.insert(index, msg.z)
currentMissionIndex = currentMissionIndex + 1
if currentMissionIndex < missionLength:
logging.debug("Making a request for the item at %s",currentMissionIndex)
mavlinkMSG.mission_request_send(m.target_system, m.target_component, currentMissionIndex)
else:
logging.debug("I am done requesting mission items")
# This will clear the current commands inside the vehicle for auto mode
cmds = vehicle.commands
cmds.clear()
for i in range(0,len(latArray),1):
mavlink.MAVLink_command_long_message
cmd1 = Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, latArray[i], lngArray[i], altArray[i])
cmds.add(cmd1)
cmds.upload()
elif(msg.command == mavlink.MAV_CMD_NAV_TAKEOFF):
arm_and_takeoff(20)
elif(msg.command == mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH):
vehicle.mode = VehicleMode("RTL")
elif(msg.command == mavlink.MAV_CMD_NAV_LAND):
vehicle.mode = VehicleMode("LAND")
elif(msg.command == mavlink.MAV_CMD_DO_GUIDED_MASTER):
print "I have been allowed to perform my guided mode"
if missionThread.isAlive():
print "I am already in the mission mode"
else:
vehicle.mode = VehicleMode("GUIDED")
stopThread = False
missionThread = Thread(target=performMissionFunciton)
missionThread.start()
elif msgID == mavlink.MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
logging.debug("I saw a clear all message")
vehicle.mode = VehicleMode("LOITER")
if missionThread.isAlive():
stopThread = True
missionThread.join()
latArray = []
lngArray = []
altArray = []
#probably want to halt motion and wait for further commands as well
elif msgID == mavlink.MAVLINK_MSG_ID_MISSION_COUNT:
logging.debug("I saw a mission count message")
missionLength = msg.count
currentMissionIndex = 0
mavlinkMSG.mission_request_send(m.target_system, m.target_component, currentMissionIndex)
elif msgID == mavlink.MAVLINK_MSG_ID_BAD_DATA:
if(mavutil.all_printable(msg.data)):
sys.stdout.write(msg.data)
sys.stdout.flush()
else:
print(msg)
elif msgID == mavlink.MAVLINK_MSG_ID_BAD_DATA:
if(mavutil.all_printable(msg.data)):
sys.stdout.write(msg.data)
sys.stdout.flush()
else:
print(msg)
time.sleep(10)
if __name__ == '__main__':
#vehicle = connect("udp:0.0.0.0:14550", wait_ready=False)
vehicle = connect("/dev/ttySAC0", baud=57600, wait_ready=False)
vehicle.add_attribute_listener('mode', vehicleModeCallback)
cmds = vehicle.commands
cmds.clear()
# Get Vehicle Home location - will be `None` until first set by autopilot
while not vehicle.home_location:
if not vehicle.home_location:
print " Waiting for home location ..."
time.sleep(1)
# We have a home location, so print it!
print "\n Home location: %s" % vehicle.home_location
master = mavutil.mavlink_connection(args.device, baud=args.baudrate)
mavlinkMSG = mavlink.MAVLink(master)
heartbeatThread = Thread(target = heartbeatTimer, args = (master,1))
heartbeatThread.start()
handleMissionMessage(master)