void SendGrid(double lat, double lon, ushort grid_spacing, byte bit)
{
log.Info("SendGrid " + lat + " " + lon + " space " + grid_spacing + " bit " + bit);
MAVLink.mavlink_terrain_data_t resp = new MAVLink.mavlink_terrain_data_t();
resp.grid_spacing = grid_spacing;
resp.lat = lastrequest.lat;
resp.lon = lastrequest.lon;
resp.gridbit = bit;
resp.data = new short[16];
for (int i = 0; i < (4 * 4); i++)
{
int x = i % 4;
int y = i / 4;
PointLatLngAlt plla = new PointLatLngAlt(lat, lon).gps_offset(x * grid_spacing, y * grid_spacing);
var alt = srtm.getAltitude(plla.Lat, plla.Lng);
// check where the alt returned came from.
if (alt.currenttype == srtm.tiletype.invalid)
{
return;
}
resp.data[i] = (short)alt.alt;
}
_interface.sendPacket(resp);
}
void SendGrid(double lat, double lon, ushort grid_spacing, byte bit)
{
log.Info("SendGrid " + lat + " " + lon + " space " + grid_spacing + " bit " + bit);
MAVLink.mavlink_terrain_data_t resp = new MAVLink.mavlink_terrain_data_t();
resp.grid_spacing = grid_spacing;
resp.lat = lastrequest.lat;
resp.lon = lastrequest.lon;
resp.gridbit = bit;
resp.data = new short[16];
for (int i = 0; i < (4 * 4); i++)
{
int x = i % 4;
int y = i / 4;
PointLatLngAlt plla = new PointLatLngAlt(lat, lon).gps_offset(x * grid_spacing, y * grid_spacing);
double alt = srtm.getAltitude(plla.Lat, plla.Lng);
if (alt == 0)
{
return;
}
resp.data[i] = (short)alt;
}
MainV2.comPort.sendPacket(resp);
}
void SendGrid(double lat, double lon, ushort grid_spacing, byte bit)
{
log.Info("SendGrid "+ lat + " " + lon + " space " + grid_spacing + " bit "+ bit);
MAVLink.mavlink_terrain_data_t resp = new MAVLink.mavlink_terrain_data_t();
resp.grid_spacing = grid_spacing;
resp.lat = lastrequest.lat;
resp.lon = lastrequest.lon;
resp.gridbit = bit;
resp.data = new short[16];
for (int i = 0; i < (4 * 4); i++)
{
int y = i % 4;
int x = i / 4;
PointLatLngAlt plla = new PointLatLngAlt(lat, lon).location_offset(x * grid_spacing, y * grid_spacing);
double alt = srtm.getAltitude(plla.Lat, plla.Lng);
resp.data[i] = (short)alt;
}
MainV2.comPort.sendPacket(resp);
}
bool ReceviedPacket(byte[] rawpacket)
{
if (rawpacket[5] == (byte)MAVLink.MAVLINK_MSG_ID.TERRAIN_DATA)
{
MAVLink.mavlink_terrain_data_t packet =
rawpacket.ByteArrayToStructure <MAVLink.mavlink_terrain_data_t>();
if (issending)
{
return(false);
}
double lLat = packet.lat / Math.Pow(10, 7);
double lLon = packet.lon / Math.Pow(10, 7);
short[] lData = packet.data;
// bls: they will all have the same location so
// just add one map marker. Perhaps labeled with
// the highest value or the range of values?
for (int i = 0; i < lData.Length; i++)
{
short lDataVal = lData[i];
if (lDataVal > 0)
{
Debug.WriteLine("data at {0} = {1}", i, lDataVal);
mFlightData.addPOIatLoc(lLat, lLon, lDataVal.ToString());
}
}
}
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);
}
void SendGrid(double lat, double lon, ushort grid_spacing, byte bit)
{
log.Info("SendGrid "+ lat + " " + lon + " space " + grid_spacing + " bit "+ bit);
MAVLink.mavlink_terrain_data_t resp = new MAVLink.mavlink_terrain_data_t();
resp.grid_spacing = grid_spacing;
resp.lat = lastrequest.lat;
resp.lon = lastrequest.lon;
resp.gridbit = bit;
resp.data = new short[16];
for (int i = 0; i < (4 * 4); i++)
{
int x = i % 4;
int y = i / 4;
PointLatLngAlt plla = new PointLatLngAlt(lat, lon).gps_offset(x * grid_spacing, y * grid_spacing);
var alt = srtm.getAltitude(plla.Lat, plla.Lng);
// check where the alt returned came from.
if (alt.currenttype == srtm.tiletype.invalid)
return;
resp.data[i] = (short)alt.alt;
}
_interface.sendPacket(resp);
}
bool ReceviedPacket(byte[] rawpacket)
{
if (rawpacket[5] == (byte)MAVLink.MAVLINK_MSG_ID.TERRAIN_DATA)
{
MAVLink.mavlink_terrain_data_t packet =
rawpacket.ByteArrayToStructure <MAVLink.mavlink_terrain_data_t>();
if (issending)
{
return(false);
}
bool lNeedToAddPoint = false;
short lMin = short.MaxValue;
short lMax = short.MinValue;
float lSum = 0;
double lLat = packet.lat / Math.Pow(10, 7);
double lLon = packet.lon / Math.Pow(10, 7);
short[] lData = packet.data;
// bls: they will all have the same location so
// just add one map marker. Perhaps labeled with
// the highest value or the range of values?
for (int i = 0; i < lData.Length; i++)
{
short lDataVal = lData[i];
lSum += lDataVal;
if (lDataVal < lMin)
{
lMin = lDataVal;
}
if (lDataVal > lMax)
{
lMax = lDataVal;
}
if (lDataVal > 100)
{
Debug.WriteLine("value {0} = {1} ", i, lDataVal);
lNeedToAddPoint = true;
}
}
if (lNeedToAddPoint)
{
float lAverage = lSum / lData.Count();
string lLabel = "";
lLabel += " Max: " + lMax.ToString();
lLabel += " Avg: " + lAverage.ToString();
lLabel += " Min: " + lMin.ToString();
mFlightData.addPOIatLoc(lLat, lLon, lLabel);
}
}
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);
}