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