public Locationwp getHomePosition()
{
doCommand(MAV_CMD.GET_HOME_POSITION, 0, 0, 0, 0, 0, 0, 0, false);
giveComport = true;
MAVLinkMessage buffer;
DateTime start = DateTime.Now;
int retrys = 3;
while (true)
{
if (!(start.AddMilliseconds(700) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("getHomePosition Retry " + retrys + " - giv com " + giveComport);
doCommand(MAV_CMD.GET_HOME_POSITION, 0, 0, 0, 0, 0, 0, 0, false);
giveComport = true;
start = DateTime.Now;
retrys--;
continue;
}
giveComport = false;
//return (byte)int.Parse(param["WP_TOTAL"].ToString());
throw new TimeoutException("Timeout on read - getHomePosition");
}
buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer.msgid == (byte)MAVLINK_MSG_ID.HOME_POSITION)
{
var home = buffer.ToStructure<mavlink_home_position_t>();
var loc = new Locationwp().Set(home.latitude / 1.0e7, home.longitude / 1.0e7, home.altitude / 1000.0, (byte)MAV_CMD.WAYPOINT);
giveComport = false;
return loc; // should be ushort, but apm has limited wp count < byte
}
}
else
{
log.Info(DateTime.Now + " PC getHomePosition ");
}
}
}