Tower 实战一：MavLink的连接与通信

Posted 2020-08-20 肖恩大神_

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);
    }