missionplanner编译修改方法

作者：繁依Fanyi0 | 2024-04-10 05:27:20

踩

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;
1
2

其中的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>();
        }
1
2
3
4
5
6
7
8
9
10
11

MainV2.cs

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

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

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;
            }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304

上述代码中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的帧头。

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/繁依Fanyi0/article/detail/396865