missionplanner编译修改方法

1.下载工具，下载visual studio2019 community

进入安装界面，安装界面选择以下安装内容：.Net桌面开发,使用C++的桌面开发，通用windows平台开发，使用.Net的移动开发，选择后一直到安装完成

2.下载源代码

方法为：git clone https://github.com/xiaozuozi/MissionPlanner.git

3.打开项目，生成解决方案以及运行

4.编译成功后进行代码的修改

下面只进行mavlink通讯相关的代码修改，主要修改的几个文件为

MAVLinkInterface.cs，CurrentState.cs,MAVLink.cs,MainV2.cs

• MAVLink.cs

 public const byte MAVLINK_STX = 253;
 public const byte MAVLINK_STX_MAVLINK1 = 0xFE;

其中的MAVLINK_STX是mavlink2.0的帧头，MAVLINK_STX_MAVLINK1为mavlink1.0的帧头

• CurrentState.cs
  此文件是用来修改mavlink的数据流的请求频率的
  在下面的方法中设置频率

static CurrentState()
        {
            // set default telemrates
            rateattitudebackup = 4;
            ratepositionbackup = 2;
            ratestatusbackup = 2;
            ratesensorsbackup = 2;
            ratercbackup = 2;
            //Init dictionary for storing names for customfields
            custom_field_names = new Dictionary<string, string>();
        }

• MainV2.cs

public void doConnect(MAVLinkInterface comPort, string portname, string baud, bool getparams = true)

以上方法主要是在点击连接按钮的时候，进行获取所有参数等的操作

• MAVLinkInterface.cs
  所有的mavlink消息的解析在此文件中完成

        public async Task<MAVLinkMessage> readPacketAsync()
        {
            byte[] buffer = new byte[MAVLINK_MAX_PACKET_LEN + 25];
            int count = 0;
            int length = 0;
            int readcount = 0;
            MAVLinkMessage message = null;

            DateTime start = DateTime.Now;

            if (debug)
                Console.WriteLine(DateTime.Now.Millisecond + " SR0 " + BaseStream?.BytesToRead);

            await readlock.WaitAsync().ConfigureAwait(false);
            try
            {
                if (debug)
                    Console.WriteLine(DateTime.Now.Millisecond + " SR1 " + BaseStream?.BytesToRead);

                while ((BaseStream != null && BaseStream.IsOpen) || logreadmode)
                {
                    try
                    {
                        if (readcount > 300)
                        {
                            break;
                        }

                        readcount++;
                        if (logreadmode)
                        {
                            message = readlogPacketMavlink();
                            buffer = message.buffer;
                            if (buffer == null || buffer.Length == 0)
                                return MAVLinkMessage.Invalid;
                        }
                        else
                        {
                            if (BaseStream.ReadTimeout != 1200)
                                BaseStream.ReadTimeout = 1200; // 1200 ms between chars - the gps detection requires this.

                            // time updated for internal reference
                            MAV.cs.datetime = DateTime.Now;

                            DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout);

                            if (debug)
                                Console.WriteLine(DateTime.Now.Millisecond + " SR1a " + BaseStream?.BytesToRead);

                            while (BaseStream.IsOpen && BaseStream.BytesToRead <= 0)
                            {
                                if (DateTime.Now > to)
                                {
                                    log.InfoFormat("MAVLINK: 1 wait time out btr {0} len {1}", BaseStream?.BytesToRead,
                                        length);
                                    throw new TimeoutException("Timeout");
                                }

                                await Task.Delay(1).ConfigureAwait(false);
                                if (debug)
                                    Console.WriteLine(DateTime.Now.Millisecond + " SR0b " + BaseStream?.BytesToRead);
                            }

                            if (debug)
                                Console.WriteLine(DateTime.Now.Millisecond + " SR1a " + BaseStream?.BytesToRead);
                            if (BaseStream.IsOpen)
                            {
                                BaseStream.Read(buffer, count, 1);
                                if (rawlogfile != null && rawlogfile.CanWrite)
                                    rawlogfile.WriteByte(buffer[count]);
                            }

                            if (debug)
                                Console.WriteLine(DateTime.Now.Millisecond + " SR1b " + BaseStream?.BytesToRead);
                        }
                    }
                    catch (Exception e)
                    {
                        log.Info("MAVLink readpacket read error: " + e.ToString());
                        break;
                    }

                    // check if looks like a mavlink packet and check for exclusions and write to console
                    if (buffer[0] != 0xfe && buffer[0] != 'U' && buffer[0] != 0xfd)
                    {
                        if (buffer[0] >= 0x20 && buffer[0] <= 127 || buffer[0] == '\n' || buffer[0] == '\r')
                        {
                            // check for line termination
                            if (buffer[0] == '\r' || buffer[0] == '\n')
                            {
                                // check new line is valid
                                if (buildplaintxtline.Length > 3)
                                    plaintxtline = buildplaintxtline;

                                log.Info(plaintxtline);
                                // reset for next line
                                buildplaintxtline = "";
                            }

                            TCPConsole.Write(buffer[0]);
                            Console.Write((char)buffer[0]);
                            buildplaintxtline += (char)buffer[0];
                        }

                        _bytesReceivedSubj.OnNext(1);
                        count = 0;
                        buffer[1] = 0;
                        continue;
                    }

                    // reset count on valid packet
                    readcount = 0;

                    if (debug)
                        Console.WriteLine(DateTime.Now.Millisecond + " SR2 " + BaseStream?.BytesToRead);

                    // check for a header
                    if (buffer[0] == 0xfe || buffer[0] == 0xfd || buffer[0] == 'U')
                    {
                        var mavlinkv2 = buffer[0] == MAVLINK_STX ? true : false;

                        int headerlength = mavlinkv2 ? MAVLINK_CORE_HEADER_LEN : MAVLINK_CORE_HEADER_MAVLINK1_LEN;
                        int headerlengthstx = headerlength + 1;

                        // if we have the header, and no other chars, get the length and packet identifiers
                        if (count == 0 && !logreadmode)
                        {
                            DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout);

                            while (BaseStream.IsOpen && BaseStream.BytesToRead < headerlength)
                            {
                                if (DateTime.Now > to)
                                {
                                    log.InfoFormat("MAVLINK: 2 wait time out btr {0} len {1}", BaseStream.BytesToRead,
                                        length);
                                    throw new TimeoutException("Timeout");
                                }

                                await Task.Delay(1).ConfigureAwait(false);
                            }

                            int read = BaseStream.Read(buffer, 1, headerlength);
                            count = read;
                            if (rawlogfile != null && rawlogfile.CanWrite)
                                rawlogfile.Write(buffer, 1, read);
                        }

                        // packet length
                        if (buffer[0] == MAVLINK_STX)
                        {
                            length = buffer[1] + headerlengthstx +
                                     MAVLINK_NUM_CHECKSUM_BYTES; // data + header + checksum - magic - length
                            if ((buffer[2] & MAVLINK_IFLAG_SIGNED) > 0)
                            {
                                length += MAVLINK_SIGNATURE_BLOCK_LEN;
                            }
                        }
                        else
                        {
                            length = buffer[1] + headerlengthstx +
                                     MAVLINK_NUM_CHECKSUM_BYTES; // data + header + checksum - U - length    
                        }

                        if (count >= headerlength || logreadmode)
                        {
                            try
                            {
                                if (logreadmode)
                                {
                                }
                                else
                                {
                                    DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout);

                                    while (BaseStream.IsOpen && BaseStream.BytesToRead < (length - (headerlengthstx)))
                                    {
                                        if (DateTime.Now > to)
                                        {
                                            log.InfoFormat("MAVLINK: 3 wait time out btr {0} len {1}",
                                                BaseStream.BytesToRead, length);
                                            break;
                                        }

                                        await Task.Delay(1).ConfigureAwait(false);
                                    }

                                    if (BaseStream.IsOpen)
                                    {
                                        int read = BaseStream.Read(buffer, headerlengthstx, length - (headerlengthstx));
                                        if (read != (length - headerlengthstx))
                                            log.InfoFormat("MAVLINK: bad read {0}, {1}, {2}", headerlengthstx, length,
                                                count);
                                        if (rawlogfile != null && rawlogfile.CanWrite)
                                        {
                                            // write only what we read, temp is the whole packet, so 6-end
                                            rawlogfile.Write(buffer, headerlengthstx, read);
                                        }
                                    }
                                }

                                count = length;
                            }
                            catch
                            {
                                break;
                            }

                            break;
                        }
                    }

                    count++;
                    if (count == 299)
                        break;
                }

                if (debug)
                    Console.WriteLine(DateTime.Now.Millisecond + " SR3 " + BaseStream?.BytesToRead);
            } // end readlock
            finally
            {
                readlock.Release();
            }

            // resize the packet to the correct length
            Array.Resize<byte>(ref buffer, count);

            // add byte count
            _bytesReceivedSubj.OnNext(buffer.Length);

            // update bps statistics
            if (_bpstime.Second != DateTime.Now.Second)
            {
                long btr = 0;
                if (BaseStream != null && BaseStream.IsOpen)
                {
                    btr = BaseStream.BytesToRead;
                }
                else if (logreadmode)
                {
                    btr = logplaybackfile.BaseStream.Length - logplaybackfile.BaseStream.Position;
                }

                Console.Write(
                    "bps {0} loss {1} left {2} mem {3} mav2 {4} sign {5} mav1 {6} mav2 {7} signed {8}      \n", _bps1,
                    MAV.synclost, btr,
                    GC.GetTotalMemory(false) / 1024 / 1024.0, MAV.mavlinkv2, MAV.signing, _mavlink1count,
                    _mavlink2count, _mavlink2signed);
                _bps2 = _bps1; // prev sec
                _bps1 = 0; // current sec
                _bpstime = DateTime.Now;
                _mavlink1count = 0;
                _mavlink2count = 0;
                _mavlink2signed = 0;
            }

            _bps1 += buffer.Length;

            if (buffer.Length == 0)
                return MAVLinkMessage.Invalid;

            if (message == null)
                message = new MAVLinkMessage(buffer, DateTime.UtcNow);

            uint msgid = message.msgid;

            message_info msginfo = MAVLINK_MESSAGE_INFOS.GetMessageInfo(msgid);

            // calc crc
            var sigsize = (message.sig != null) ? MAVLINK_SIGNATURE_BLOCK_LEN : 0;
            ushort crc = MavlinkCRC.crc_calculate(buffer, message.Length - sigsize - MAVLINK_NUM_CHECKSUM_BYTES);

            // calc extra bit of crc for mavlink 1.0/2.0
            if (message.header == 0xfe || message.header == 0xfd)
            {
                crc = MavlinkCRC.crc_accumulate(msginfo.crc, crc);
            }

            // check message length size vs table (mavlink1 explicit size check | mavlink2 allow all, undersize 0 trimmed, and oversize unknown extension)
            if (!message.ismavlink2 && message.payloadlength != msginfo.minlength)
            {
                if (msginfo.length == 0) // pass for unknown packets
                {
                    log.InfoFormat("unknown packet type {0}", message.msgid);
                }
                else
                {
                    log.InfoFormat("Mavlink Bad Packet (Len Fail) len {0} pkno {1}", buffer.Length, message.msgid);
                    return MAVLinkMessage.Invalid;
                }
            }

            // check crc
            if ((message.crc16 >> 8) != (crc >> 8) ||
                (message.crc16 & 0xff) != (crc & 0xff))
            {
                if (buffer.Length > 5 && msginfo.name != null)
                    log.InfoFormat("Mavlink Bad Packet (crc fail) len {0} crc {1} vs {4} pkno {2} {3}", buffer.Length,
                        crc, message.msgid, msginfo.name.ToString(),
                        message.crc16);
                if (logreadmode)
                    log.InfoFormat("bad packet pos {0} ", logplaybackfile.BaseStream.Position);
                return MAVLinkMessage.Invalid;
            }

上述代码中BaseStream.Read(buffer, count, 1);的意思是填入第一个字节的内容，用来判断是mavlink1.0还是mavlink2.0,
int read = BaseStream.Read(buffer, 1, headerlength);是填入第1位到payload前一位的所有字节。int read = BaseStream.Read(buffer, headerlengthstx, length - (headerlengthstx));是填入payload部分和2位crc校验的值的内容。如果要对mavlink协议进行解密的话，则将BaseStream.Read(buffer, count, 1);单独解密，用来判断帧头，int read = BaseStream.Read(buffer, 2, length -2);是用来获取后面所有位的值
BaseStream.Write(packet, 0, i);则是发送指令。其中的0xfe为mavlink1.0的帧头，0xfd为mavlink2.0的帧头。