Tower 實戰一：MavLink的連接與通信

1 廢話不多說，Tower的連接方式之一是通過android通過手機自帶藍牙模塊，和遙控器藍牙通信，最后在連接飛空，飛空是一塊單片機，里面也有一套程序處理邏輯，然后遙控器坐傳輸媒介

這樣做的好處就是把一些用戶操作放到android上面，比如航點規划，客服端控制飛機的起飛，降落等，最后飛空來控制無人機玩成一系列的操作

連接方式如圖

2 今天主要說一下Mavlink的連接首先在Tower的源碼里構建了一個類

public class MAVLinkClient implements MAVLinkStreams.MAVLinkOutputStream {
    public MAVLinkClient(Context context, MAVLinkStreams.MavlinkInputStream listener) {
        this.listener = listener;
    }

    private void connectMAVConnection() {
}

    public void sendMavPacket(MAVLinkPacket pack) {
}

    @Override
    public void toggleConnectionState() {
        if (isConnected()) {
            closeConnection();
        } else {
            openConnection();
        }
    }
}

這個了主要實習了 連接，切換連接，發送數據包，和注入一個listener ， 這個listener主要是接受到數據的時候回調，所以這個類主要就是實習這個幾個功能，Tower向無人機發送數據基本都是通過這個sendMavPacket方法去實現的

3 我們看看listener是個什么東西

    public interface MavlinkInputStream {
        void notifyConnected();

        void notifyDisconnected();

        void notifyReceivedData(MAVLinkMessage m);
    }

這個listener的聲明 ，他是用來處理收到消息的 ，調用notifyReceivedData 接受一個MavLinkmessage

4  接來我們看MavLinkClient的初始化，差看源碼可以看到，在Application的初始化OnCreatef 方法里面 MAVLinkClient MAVClient = new MAVLinkClient(this, this); 可以看到Application 實現了MavlinkInputStream 接口，並且實現了 notifyReceivedData 方法

    @Override
    public void notifyReceivedData(MAVLinkMessage msg) {
        mavLinkMsgHandler.receiveData(msg);
    }

 

5 這樣你就很容易看到在哪里處理 MAVLinkMessage 了，so ， MavLinkMsgHandler 就是 MAVLinkMessage 的處類了，你可以看到關於多種MAVLinkMessage 的處理，這些消息有心跳消息，事件消息等。。。

6 現在條理是不是很清楚了，那我們就來看android如果通過藍牙連接飛控的，首先你必須會藍牙連接的基礎知識，網上資料很多

7 我們看連接函數

    private void connectMAVConnection() {
        bluetoothConnection.addMavLinkConnectionListener("blue",mConnectionListener);
        Toast.makeText(parent, R.string.status_connecting, Toast.LENGTH_SHORT).show();
        if (bluetoothConnection.getConnectionStatus() == MavLinkConnection.MAVLINK_DISCONNECTED) {
            bluetoothConnection.connect();
        }
    }

直接調用了bluetoothConnection.connect(),  MavLinkConnection bluetoothConnection是一個抽象類，也住無人機地面站里面的一個比較核心的類，

  /**
     * Listen for incoming data on the mavlink connection.
     */
    private final Runnable mConnectingTask = new Runnable() {

        @Override
        public void run() {
            Thread sendingThread = null, loggingThread = null;
            try {
                // Open the connection
                openConnection();
                mConnectionStatus.set(MAVLINK_CONNECTED);
                // Launch the 'Sending', and 'Logging' threads
                sendingThread = new Thread(mSendingTask, "MavLinkConnection-Sending Thread");
                sendingThread.start();

                final Parser parser = new Parser();
                parser.stats.mavlinkResetStats();
          
                final byte[] readBuffer = new byte[READ_BUFFER_SIZE];
                while (mConnectionStatus.get() == MAVLINK_CONNECTED&&!isAndroidfor4_4()) {
                    int bufferSize = readDataBlock(readBuffer);
                    if (readBuffer==null||readBuffer.length==0){
                        continue;
                    }
                    handleData(parser, bufferSize, readBuffer);
                }
            } catch (Exception e) {
                Log.i("===>>","disconent");
                if (loggingThread != null && loggingThread.isAlive()) {
                    loggingThread.interrupt();
                }

                if (sendingThread != null && sendingThread.isAlive()) {
                    sendingThread.interrupt();
                }
                mConnectionStatus.set(MAVLINK_DISCONNECTED);
                disconnect();
            }
        }

8 這個類有幾個核心的方法 1打開連接 openConnection（），2 sendingThread不停地發送數據 3  handleData（）處理數據，在其實現類里面主要也是完成這3個方法，

public Bluetoothfor2_0Connection(Context parentContext) {
        prefs = new DroidPlannerPrefs(parentContext);
        mBluetoothAdapter = BluetoothAdapter.getDefaultAdapter();
        if (mBluetoothAdapter == null) {
            Log.d(BLUE, "Null adapters");
        }
    }

    protected void openAndroidConnection() throws IOException {
        Log.d(BLUE, "Connect");

        // Reset the bluetooth connection
        resetConnection();

        // Retrieve the stored device
        BluetoothDevice device = null;
        final String addressName = prefs.getBluetoothDeviceAddress();

        if (addressName != null) {
            // strip name, use address part - stored as <address>;<name>
            final String part[] = addressName.split(";");
            try {
                device = mBluetoothAdapter.getRemoteDevice(part[0]);
            } catch (IllegalArgumentException ex) {
                // invalid configuration (device may have been removed)
                // NOP fall through to 'no device'
            }
        }
        // no device
        if (device == null)
            device = findSerialBluetoothBoard();

        Log.d(BLUE, "Trying to connect to device with address " + device.getAddress());
        Log.d(BLUE, "BT Create Socket Call...");
        bluetoothSocket = device.createInsecureRfcommSocketToServiceRecord(UUID
                .fromString(UUID_SPP_DEVICE));

        Log.d(BLUE, "BT Cancel Discovery Call...");
        mBluetoothAdapter.cancelDiscovery();

        Log.d(BLUE, "BT Connect Call...");
        bluetoothSocket.connect(); // Here the IOException will rise on BT
                                    // protocol/handshake error.

        Log.d(BLUE, "## BT Connected ##");
        out = bluetoothSocket.getOutputStream();
        in = bluetoothSocket.getInputStream();
    }

    @Override
    protected int readDataBlock(byte[] buffer) throws IOException {
        return in.read(buffer);

    }

    @Override
    protected void sendBuffer(byte[] buffer) throws IOException {
        if (out != null) {
            out.write(buffer);
        }
    }

9 handledata 方法就是回調之前的 listener.notifydatareceiver(),是不是之前地方了，至於發送數據

 private final Runnable mSendingTask = new Runnable() {
        @Override
        public void run() {
            int msgSeqNumber = 0;
            try {
                while (mConnectionStatus.get() == MAVLINK_CONNECTED) {
                    final MAVLinkPacket packet = mPacketsToSend.take();
                    packet.seq = msgSeqNumber;
                    byte[] buffer = packet.encodePacket();
                        if (buffer==null|| buffer.length==0){
                            continue;
                        }
                    try {
                        sendBuffer(buffer);
                        queueToLog(packet);
                    } catch (IOException e) {
                        reportComError(e.getMessage());
                        mLogger.logErr(TAG, e);
                    }

                    msgSeqNumber = (msgSeqNumber + 1) % (MAX_PACKET_SEQUENCE + 1);
                }
            } catch (InterruptedException e) {
         
    };

 

 發送數據其實也是Mavclient的 sendMavPacket方法，里面有一個阻塞隊列的東西，線程安全的雙端隊列，可能需要你看看，思路很清晰，2可能會講 MavlinkMessage

    @Override
    public void sendMavPacket(MAVLinkPacket pack) {
        if (!isConnected()) {
            return;
        }
        bluetoothConnection.sendMavPacket(pack);
    }