Mavlink地面站编写之五—MP源码多线程读写框架分析

转载请注明出处！阿木开源社区 玩也要玩的专业 http://www.amovauto.com/?p=743#more-743

对于MissionPlanner这种多任务的程序，我们知道要采用多线程的方式来实现，读数据，显示数据，写数据。这是就需要一些数据同步的问题。尤其是串口数据的读写的数据同步。数据读写模型，比如常见的生产者，消费者模型。这里不再多述。我们来分析下MP代码的读线程，写线程，来实现我们社区的开源地面站的显示数据和写入指令的功能。下面看看数据读取线程的代码，这个数据线程读取的代码在MainV2.CS中的1962行左右。

while (port.BaseStream.IsOpen && port.BaseStream.BytesToRead > minbytes &&
port.giveComport == false)
{
try
{
port.readPacket();
}
catch (Exception ex)
{
log.Error(ex);
}
}
// update currentstate of sysids on the port
foreach (var MAV in port.MAVlist.GetMAVStates())
{
try
{
MAV.cs.UpdateCurrentSettings(null, false, port, MAV);
}
catch (Exception ex)
{
log.Error(ex);
}
}

其中port.readPacket()这个语句是把数据读到一个数据缓冲区，显示线程就可以从这个数据缓冲区拿出来显示了，具体的代码参考mavlinkinterface的内容。这个函数里面有些数据的上锁，解锁的语句，是为了完成数据同步。这是读取数据，下面我们看看如果我们要实现发送指令或者航点给飞控，也就是写数据，我们应该怎么操作。写数据一定是在另外一个线程里面。在FlightPlanne.cs中的1863行，我们可以看到写入航点的操作

private void BUT_write_Click(object sender, EventArgs e)
{
if ((altmode) CMB_altmode.SelectedValue == altmode.Absolute)
{
if (DialogResult.No ==
CustomMessageBox.Show("Absolute Alt is selected are you sure?", "Alt Mode", MessageBoxButtons.YesNo))
{
CMB_altmode.SelectedValue = (int) altmode.Relative;
}
}

// check for invalid grid data
for (int a = 0; a < Commands.Rows.Count - 0; a++)
{
for (int b = 0; b < Commands.ColumnCount - 0; b++) { double answer; if (b >= 1 && b <= 7)
{
if (!double.TryParse(Commands[b, a].Value.ToString(), out answer))
{
CustomMessageBox.Show("There are errors in your mission");
return;
}
}

if (TXT_altwarn.Text == "")
TXT_altwarn.Text = (0).ToString();

if (Commands.Rows[a].Cells[Command.Index].Value.ToString().Contains("UNKNOWN"))
continue;

byte cmd =
(byte)
(int)
Enum.Parse(typeof (MAVLink.MAV_CMD),
Commands.Rows[a].Cells[Command.Index].Value.ToString(), false);

if (cmd < (byte) MAVLink.MAV_CMD.LAST &&
double.Parse(Commands[Alt.Index, a].Value.ToString()) < double.Parse(TXT_altwarn.Text))
{
if (cmd != (byte) MAVLink.MAV_CMD.TAKEOFF &&
cmd != (byte) MAVLink.MAV_CMD.LAND &&
cmd != (byte) MAVLink.MAV_CMD.RETURN_TO_LAUNCH)
{
CustomMessageBox.Show("Low alt on WP#" + (a + 1) +
"\nPlease reduce the alt warning, or increase the altitude");
return;
}
}
}
}

ProgressReporterDialogue frmProgressReporter = new ProgressReporterDialogue
{
StartPosition = FormStartPosition.CenterScreen,
Text = "Sending WP's"
};

frmProgressReporter.DoWork += saveWPs;
frmProgressReporter.UpdateProgressAndStatus(-1, "Sending WP's");

ThemeManager.ApplyThemeTo(frmProgressReporter);

frmProgressReporter.RunBackgroundOperationAsync();

frmProgressReporter.Dispose();

MainMap.Focus();
}

上面的函数是一些验证航点逻辑准确性的操作，frmProgressReporter.DoWork += saveWPs;这是实现了一个委托。而saveWPs这个函数是数据写入函数.port.setWPTotal（写入一共的航点数量）,port.setWP（写入航点）, port.setParam（设置参数）, port.setWPACK(给飞控应答)。





这里我们可以看到是遵循MAVLINK的读写协议的，首先地面站写入航点数量(port.setWPTotal)，地面站收到航点请求指令，地面站向飞控写入航点(port.setWP)，最后port.setWPACK完成应答.源码太长自行打开源码阅读参考。这是完整的写入数据的流程。其中我们还有注意一点就是 MainV2.comPort.giveComport = true;这句，在 saveWPs函数中，这是这个写入线程占用COM端口的标志，通过这句话，我发现MP的数据读写通信是半双工的。因为在数据的代码中我们会发现
while (port.BaseStream.IsOpen && port.BaseStream.BytesToRead > minbytes &&port.giveComport == false)这句话中的port.giveComport == false表示，没有数据写入，也就是COM没有占用，读数据才会有效，否则跳出数据读循环。也就是说MP的源码通port.giveComport 来判断是不可以读取数据。所以如果port.giveComport == true表示当前有数据在写入，就不会去读取数据。由此看出MP地面站的代码数据读写是半双工的。我也是在写社区地面站的时候发现这一点的，否则我采用mavlinkinterface的时候写入航点一直有问题，就出在写入数据的时候，不能读取数据。本人浅解欢迎批评指正！