bool CheckPlaneState(PlaneState ps) { if (float.IsNaN(ps.Lat) || float.IsNaN(ps.Lon) || float.IsNaN(ps.homeLat) || float.IsNaN(ps.homeLon)) { return(false); } if (ps.Lat > 90.0f || ps.Lat < -90.0f) { return(false); } if (ps.Lon > 180.0f || ps.Lon < -180.0f) { return(false); } if (ps.homeLat > 90.0f || ps.homeLat < -90.0f) { return(false); } if (ps.homeLon > 180.0f || ps.homeLon < -180.0f) { return(false); } if (ps.Lat < 1 && ps.Lat > -1 && ps.Lon < 1 && ps.Lon > -1) { return(false); } return(true); }
protected bool ParsePacket(byte[] packet) { tramas_rx++; if (packet != null && packet[0] == DATALEN) { byte crc = CRC.CCITT8(packet, packet[0] - 1); byte t = packet[packet[0] - 1]; if (crc == t && crc != 255 && crc != 0) { tramas_ok++; int i = 1; if (planeState == null) { planeState = new PlaneState(); } planeState.Lon = USBXpress.USBXpress.tofloat(packet, ref i); planeState.Lat = USBXpress.USBXpress.tofloat(packet, ref i); planeState.Alt = USBXpress.USBXpress.toint16(packet, ref i); planeState.Rumbo = USBXpress.USBXpress.tochar(packet, ref i) * 360.0f / 256.0f; planeState.Knots = USBXpress.USBXpress.tobyte(packet, ref i); planeState.vertSpeed = USBXpress.USBXpress.tochar(packet, ref i); planeState.WptIndex = USBXpress.USBXpress.tochar(packet, ref i); planeState.homeLon = USBXpress.USBXpress.tofloat(packet, ref i); planeState.homeLat = USBXpress.USBXpress.tofloat(packet, ref i); planeState.v1 = USBXpress.USBXpress.tobyte(packet, ref i) / 10.0f; planeState.v2 = USBXpress.USBXpress.tobyte(packet, ref i) / 10.0f; planeState.pitch = USBXpress.USBXpress.tochar(packet, ref i) * 180 / 127.0f; planeState.roll = USBXpress.USBXpress.tochar(packet, ref i) * 180 / 127.0f; planeState.RSSI = USBXpress.USBXpress.tobyte(packet, ref i); if (CheckPlaneState(planeState)) { planeState.lastrx = true; Log(); return(true); } else { planeState.lastrx = false; planeState = null; return(false); } } } if (planeState != null) { planeState.lastrx = false; } return(false); }
protected bool ParsePacket(byte[] packet) { tramas_rx++; if (packet != null&&packet[0]==DATALEN) { byte crc = CRC.CCITT8(packet, packet[0] - 1); byte t = packet[packet[0] - 1]; if (crc == t && crc !=255 && crc !=0) { tramas_ok++; int i = 1; if(planeState==null) planeState=new PlaneState(); planeState.Lon = USBXpress.USBXpress.tofloat(packet, ref i); planeState.Lat = USBXpress.USBXpress.tofloat(packet, ref i); planeState.Alt = USBXpress.USBXpress.toint16(packet, ref i); planeState.Rumbo = USBXpress.USBXpress.tochar(packet, ref i) * 360.0f / 256.0f; planeState.Knots = USBXpress.USBXpress.tobyte(packet, ref i); planeState.vertSpeed = USBXpress.USBXpress.tochar(packet, ref i); planeState.WptIndex = USBXpress.USBXpress.tochar(packet, ref i); planeState.homeLon = USBXpress.USBXpress.tofloat(packet, ref i); planeState.homeLat = USBXpress.USBXpress.tofloat(packet, ref i); planeState.v1 = USBXpress.USBXpress.tobyte(packet, ref i) / 10.0f; planeState.v2 = USBXpress.USBXpress.tobyte(packet, ref i) / 10.0f; planeState.pitch = USBXpress.USBXpress.tochar(packet, ref i) * 180 / 127.0f; planeState.roll = USBXpress.USBXpress.tochar(packet, ref i) * 180 / 127.0f; planeState.RSSI = USBXpress.USBXpress.tobyte(packet, ref i); if(CheckPlaneState(planeState)) { planeState.lastrx = true; Log(); return true; } else { planeState.lastrx = false; planeState = null; return false; } } } if (planeState != null) planeState.lastrx = false; return false; }
bool CheckPlaneState(PlaneState ps) { if (float.IsNaN(ps.Lat) || float.IsNaN(ps.Lon) || float.IsNaN(ps.homeLat) || float.IsNaN(ps.homeLon)) return false; if (ps.Lat > 90.0f || ps.Lat < -90.0f) return false; if (ps.Lon > 180.0f || ps.Lon < -180.0f) return false; if (ps.homeLat > 90.0f || ps.homeLat < -90.0f) return false; if (ps.homeLon > 180.0f || ps.homeLon < -180.0f) return false; if (ps.Lat < 1 && ps.Lat > -1 && ps.Lon < 1 && ps.Lon > -1) return false; return true; }
bool CheckPlaneState(PlaneState ps) { if (float.IsNaN(ps.Lat) || float.IsNaN(ps.Lon) || float.IsNaN(ps.homeLat) || float.IsNaN(ps.homeLon)) return false; if (ps.Lat > 90.0f || ps.Lat < -90.0f) return false; if (ps.Lon > 180.0f || ps.Lon < -180.0f) return false; if (ps.homeLat > 90.0f || ps.homeLat < -90.0f) return false; if (ps.homeLon > 180.0f || ps.homeLon < -180.0f) return false; if (ps.Lat < 1 && ps.Lat > -1 && ps.Lon < 1 && ps.Lon > -1) return false; if (lastWpt == null) lastWpt = new WayPoint("", ps.Lon, ps.Lat); WayPoint here = new WayPoint("", ps.Lon, ps.Lat); if ((check_counter < 20) && (lastWpt.getDistance(here) > 1)) { check_counter++; return false; } else { check_counter = 0; lastWpt = here; } return true; }
public void Send(PlaneState planeState, WayPoint home) { if (planeState != null) { this.planeState = planeState; this.home = home; this.planeStateUpdated = true; } }