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); }