/**
   * Disconnect a mavlink connection. If the operation is successful, it will be reported through
   * the MavLinkConnectionListener interface.
   */
  public void disconnect() {
    if (mConnectionStatus.get() == MAVLINK_DISCONNECTED
        || (mConnectThread == null && mTaskThread == null)) {
      return;
    }

    try {
      final long disconnectTime = System.currentTimeMillis();

      mConnectionStatus.set(MAVLINK_DISCONNECTED);
      mConnectionTime.set(-1);

      if (mConnectThread != null && mConnectThread.isAlive() && !mConnectThread.isInterrupted()) {
        mConnectThread.interrupt();
      }

      if (mTaskThread != null && mTaskThread.isAlive() && !mTaskThread.isInterrupted()) {
        mTaskThread.interrupt();
      }

      closeConnection();
      reportDisconnect(disconnectTime);
    } catch (IOException e) {
      mLogger.logErr(TAG, e);
      reportComError(e.getMessage());
    }
  }
 private void queueToLog(byte[] packetData) {
   if (packetData != null) {
     if (!mPacketsToLog.offer(packetData)) {
       mLogger.logErr(TAG, "Unable to log mavlink packet. Queue is full!");
     }
   }
 }
 protected void onConnectionOpened() {
   if (mConnectionStatus.compareAndSet(MAVLINK_CONNECTING, MAVLINK_CONNECTED)) {
     mLogger.logInfo(TAG, "Starting manager thread.");
     mTaskThread = new Thread(mManagerTask, "MavLinkConnection-Manager Thread");
     mTaskThread.start();
   }
 }
 /**
  * Establish a mavlink connection. If the connection is successful, it will be reported through
  * the MavLinkConnectionListener interface.
  */
 public void connect() {
   if (mConnectionStatus.compareAndSet(MAVLINK_DISCONNECTED, MAVLINK_CONNECTING)) {
     mLogger.logInfo(TAG, "Starting connection thread.");
     mConnectThread = new Thread(mConnectingTask, "MavLinkConnection-Connecting Thread");
     mConnectThread.start();
     reportConnecting();
   }
 }
  public void removeLoggingPath(String tag) {
    if (tag == null || tag.length() == 0) return;

    Pair<String, BufferedOutputStream> logInfo = loggingOutStreams.remove(tag);
    if (logInfo != null) {
      BufferedOutputStream outStream = logInfo.second;
      if (outStream != null) {
        try {
          outStream.close();
        } catch (IOException e) {
          mLogger.logErr(TAG, "IO Exception while closing " + logInfo.first, e);
        }
      }
    }
  }
 public void sendMavPacket(MAVLinkPacket packet) {
   final byte[] packetData = packet.encodePacket();
   if (!mPacketsToSend.offer(packetData)) {
     mLogger.logErr(TAG, "Unable to send mavlink packet. Packet queue is full!");
   }
 }
 protected void onConnectionFailed(String errMsg) {
   mLogger.logInfo(TAG, "Unable to establish connection: " + errMsg);
   reportComError(errMsg);
   disconnect();
 }