bool ReceviedPacket(byte[] rawpacket)
{
if (rawpacket[5] == (byte)MAVLink.MAVLINK_MSG_ID.TERRAIN_REQUEST)
{
MAVLink.mavlink_terrain_request_t packet =
rawpacket.ByteArrayToStructure <MAVLink.mavlink_terrain_request_t>();
if (issending)
{
return(false);
}
lastrequest = packet;
log.Info("received TERRAIN_REQUEST " + packet.lat / 1e7 + " " + packet.lon / 1e7 + " space " +
packet.grid_spacing + " " + Convert.ToString((long)packet.mask, 2));
System.Threading.ThreadPool.QueueUserWorkItem(QueueSendGrid);
}
else if (rawpacket[5] == (byte)MAVLink.MAVLINK_MSG_ID.TERRAIN_REPORT)
{
MAVLink.mavlink_terrain_report_t packet =
rawpacket.ByteArrayToStructure <MAVLink.mavlink_terrain_report_t>();
log.Info("received TERRAIN_REPORT " + packet.lat / 1e7 + " " + packet.lon / 1e7 + " " + packet.loaded + " " +
packet.pending);
}
return(false);
}
private bool ReceivedPacket(MAVLink.MAVLinkMessage rawpacket)
{
if (rawpacket.msgid == (byte)MAVLink.MAVLINK_MSG_ID.TERRAIN_REQUEST)
{
MAVLink.mavlink_terrain_request_t packet =
rawpacket.ToStructure <MAVLink.mavlink_terrain_request_t>();
if (issending)
{
return(false);
}
lastmessage = rawpacket;
lastrequest = packet;
log.Info("received TERRAIN_REQUEST " + packet.lat / 1e7 + " " + packet.lon / 1e7 + " space " +
packet.grid_spacing + " " + Convert.ToString((long)packet.mask, 2));
// reset state to block
mre.Reset();
System.Threading.ThreadPool.QueueUserWorkItem(QueueSendGrid);
// wait for thread to start
mre.WaitOne();
}
else if (rawpacket.msgid == (byte)MAVLink.MAVLINK_MSG_ID.TERRAIN_REPORT)
{
MAVLink.mavlink_terrain_report_t packet =
rawpacket.ToStructure <MAVLink.mavlink_terrain_report_t>();
log.Info("received TERRAIN_REPORT " + packet.lat / 1e7 + " " + packet.lon / 1e7 + " " + packet.loaded + " " +
packet.pending);
}
return(false);
}
bool ReceviedPacket(MAVLink.MAVLinkMessage rawpacket)
{
if (rawpacket.msgid == (byte) MAVLink.MAVLINK_MSG_ID.TERRAIN_REQUEST)
{
MAVLink.mavlink_terrain_request_t packet =
rawpacket.ToStructure<MAVLink.mavlink_terrain_request_t>();
if (issending)
return false;
lastmessage = rawpacket;
lastrequest = packet;
log.Info("received TERRAIN_REQUEST " + packet.lat/1e7 + " " + packet.lon/1e7 + " space " +
packet.grid_spacing + " " + Convert.ToString((long) packet.mask, 2));
System.Threading.ThreadPool.QueueUserWorkItem(QueueSendGrid);
}
else if (rawpacket.msgid == (byte) MAVLink.MAVLINK_MSG_ID.TERRAIN_REPORT)
{
MAVLink.mavlink_terrain_report_t packet =
rawpacket.ToStructure<MAVLink.mavlink_terrain_report_t>();
log.Info("received TERRAIN_REPORT " + packet.lat/1e7 + " " + packet.lon/1e7 + " " + packet.loaded + " " +
packet.pending);
}
return false;
}
bool ReceviedPacket(byte[] rawpacket)
{
if (rawpacket[5] == (byte)MAVLink.MAVLINK_MSG_ID.TERRAIN_REQUEST)
{
MAVLink.mavlink_terrain_request_t packet = rawpacket.ByteArrayToStructure<MAVLink.mavlink_terrain_request_t>();
lastrequest = packet;
log.Info("received TERRAIN_REQUEST " + packet.lat / 1e7 + " " + packet.lon / 1e7 + " space " + packet.grid_spacing + " " + Convert.ToString((long)packet.mask, 2));
// 8 across - 7 down
// cycle though the bitmask to check what we need to send (8*7)
for (byte i = 0; i < 56; i++)
{
// check to see if the ap requested this box.
if ((lastrequest.mask & ((ulong)1 << i)) > 0)
{
// get the requested lat and lon
double lat = lastrequest.lat / 1e7;
double lon = lastrequest.lon / 1e7;
// get the distance between grids
int bitgridspacing = lastrequest.grid_spacing * 4;
// get the new point, based on our current bit.
var newplla = new PointLatLngAlt(lat, lon).location_offset(bitgridspacing * (i % 8),bitgridspacing * (int)Math.Floor(i / 8.0));
// send a 4*4 grid, based on the lat lon of the bitmask
SendGrid(newplla.Lat, newplla.Lng, lastrequest.grid_spacing, i);
}
}
}
else if (rawpacket[5] == (byte)MAVLink.MAVLINK_MSG_ID.TERRAIN_REPORT)
{
MAVLink.mavlink_terrain_report_t packet = rawpacket.ByteArrayToStructure<MAVLink.mavlink_terrain_report_t>();
log.Info("received TERRAIN_REPORT " + packet.lat / 1e7 + " " + packet.lon / 1e7 + " " + packet.loaded + " " + packet.pending);
}
return false;
}
