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的帧头。
网友评论