def mavlink_thread_out():
# Huge try catch in case we see http://bugs.python.org/issue1856
try:
while self._alive:
try:
msg = self.out_queue.get(True, timeout=0.01)
self.master.write(msg)
except Empty:
continue
except socket.error as error:
# If connection reset (closed), stop polling.
if error.errno == ECONNABORTED:
raise APIException('Connection aborting during read')
raise
except Exception as e:
errprinter('>>> mav send error:', e)
break
except APIException as e:
errprinter('>>> ' + str(e.message))
self._alive = False
self.master.close()
self._death_error = e
except Exception as e:
# http://bugs.python.org/issue1856
if not self._alive:
pass
else:
self._alive = False
self.master.close()
self._death_error = e
# Explicitly clear out buffer so .close closes.
self.out_queue = Queue()
def mavlink_thread_in():
# Huge try catch in case we see http://bugs.python.org/issue1856
try:
while self._alive:
# Loop listeners.
for fn in self.loop_listeners:
fn(self)
# Sleep
self.master.select(0.05)
while self._accept_input:
try:
msg = self.master.recv_msg()
except socket.error as error:
# If connection reset (closed), stop polling.
if error.errno == ECONNABORTED:
raise APIException(
'Connection aborting during send')
raise
except mavutil.mavlink.MAVError as e:
# Avoid
# invalid MAVLink prefix '73'
# invalid MAVLink prefix '13'
self._logger.debug('mav recv error: %s' % str(e))
msg = None
except Exception:
# Log any other unexpected exception
self._logger.exception(
'Exception while receiving message: ',
exc_info=True)
msg = None
if not msg:
break
# Message listeners.
for fn in self.message_listeners:
try:
fn(self, msg)
except Exception:
self._logger.exception(
'Exception in message handler for %s' %
msg.get_type(),
exc_info=True)
except APIException as e:
self._logger.exception('Exception in MAVLink input loop')
self._alive = False
self.master.close()
self._death_error = e
return
except Exception as e:
# http://bugs.python.org/issue1856
if not self._alive:
pass
else:
self._alive = False
self.master.close()
self._death_error = e
def mavlink_thread_in():
# Huge try catch in case we see http://bugs.python.org/issue1856
try:
while self._alive:
# Downtime
time.sleep(0.05)
# Loop listeners.
for fn in self.loop_listeners:
self.send_heartbeat()
fn(self)
while self._accept_input:
if not self._alive:
break
try:
msg = self.master.recv_msg()
# print("msg=%s" % (str(msg)))
except socket.error as error:
# If connection reset (closed), stop polling.
if error.errno == ECONNABORTED:
raise APIException('Connection aborting during send')
raise
except Exception as e:
# TODO this should be more rigorous. How to avoid
# invalid MAVLink prefix '73'
# invalid MAVLink prefix '13'
errprinter('mav recv error:', e)
msg = None
if not msg:
break
# Message listeners.
for fn in self.message_listeners:
try:
fn(self, msg)
except Exception as e:
errprinter('>>> Exception in message handler for %s' %
msg.get_type())
errprinter('>>> ' + str(e))
except APIException as e:
errprinter('>>> APIException (' + str(e.message) + ")")
self._alive = False
self.master.close()
self._death_error = e
return
except Exception as e:
# http://bugs.python.org/issue1856
if not self._alive:
pass
else:
self._alive = False
self.master.close()
self._death_error = e
errprinter('>>> Exception (' + str(e.message) + ")")
traceback.print_exc()
def simple_goto(self, lat, lon, alt, relative=False):
if relative != True:
raise APIException('relative positions required')
self.mav.mission_item_send(self.target_system,
self.target_component,
0,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2, 0, 0,
0, 0, 0, lat, lon,
alt)