/// <summary>
/// Sends a differential gps correction factor packet
/// </summary>
/// <param name="gpsc">a filled GPSCorrection struct</param>
public void SendGPSCorrectionFactor(GPSCorrection gpsc)
{
/*
GPS Correction Factor
0x46 Command type “Flight Ops”
0x43 Command “GPS Correction Factor”
0xSSSS Latitude seconds correction (-3000 to 3000) in thousandths of seconds.
0xSSSS Longitude seconds correction (-3000 to 3000) in thousandths of seconds.
0xAAAA Altitude correction in 10ths of meters.
0xHHMMSS Time of GPS fix
*/
if (ExpectedResponse.ResponseExpected == false)
{
OutGoingPacket = new byte[18];
OutGoingPacket[0] = 0xA5;//header
OutGoingPacket[1] = 0x5A;
OutGoingPacket[2] = 0x0B;//number of bytes
OutGoingPacket[3] = 0x46;//flight ops command
OutGoingPacket[4] = 0x43;
OutGoingPacket[5] = (byte)((gpsc.LatMinutes & 0xFF00) >> 8);//upper lat seconds byte
OutGoingPacket[6] = (byte)(gpsc.LatMinutes & 0x00FF);
OutGoingPacket[7] = (byte)((gpsc.LongMinutes & 0xFF00) >> 8);//upper long seconds byte
OutGoingPacket[8] = (byte)(gpsc.LongMinutes & 0x00FF);
OutGoingPacket[9] = (byte)((gpsc.Altitude & 0xFF00) >> 8);//upper altitude byte
OutGoingPacket[10] = (byte)(gpsc.Altitude & 0x00FF);
OutGoingPacket[11] = (byte)gpsc.Time.Hour;
OutGoingPacket[12] = (byte)gpsc.Time.Minute;
OutGoingPacket[13] = (byte)gpsc.Time.Second;
ushort checksum = 0;
for (int i = 2; i < 14; i++)
{
checksum += OutGoingPacket[i];
}
OutGoingPacket[14] = (byte)((checksum & 0xFF00) >> 8);
OutGoingPacket[15] = (byte)(checksum & 0x00FF);
OutGoingPacket[16] = 0xCC;
OutGoingPacket[17] = 0x33;
SendPacket("GPS Correction Factor", ExpectedResponses.type.FullPacketResponse);
}
}
void GPSP_GPSStringReceived(object sender, GPSParser.GPSStringReceivedEventArgs e)
{
if (e.data.Long.Degrees != 0 && e.data.Lat.Degrees != 0)
{
Lon = e.data.Long;
Lat = e.data.Lat;
if (GoogleMapCtrl.BaseStationSet)
{
RequestInfoTimer.Enabled = false;
double Longitude = -(Lon.Degrees + ((Lon.Minutes + (Lon.FractionalMinutes / 10000.0)) / 60.0));
double Latitude = Lat.Degrees + ((Lat.Minutes + (Lat.FractionalMinutes / 10000.0)) / 60.0);
GoogleMapCtrl.SetMarkerAtLoc(Latitude, Longitude);
double LatCorrection = 0;//GoogleMapCtrl.BaseStation.latitude - Latitude;
double LongCorrection = 0;//GoogleMapCtrl.BaseStation.longitude - Longitude;
GPSCorrection gpsc = new GPSCorrection();
if (AltitudeSamples < 10)
{
gpsc.Altitude = 0;//zero correction until we have enough base station samples
BaseStationAltitude += e.data.Altitude;
AltitudeSamples++;
}
if (AltitudeSamples == 10)
{
gpsc.Altitude = 0;//zero correction until we have enough base station samples
AltitudeSamples = 11;
BaseStationAltitude = BaseStationAltitude / 10.0;
}
if (AltitudeSamples == 11)
{
gpsc.Altitude = Convert.ToInt16((e.data.Altitude * 10) - (BaseStationAltitude * 10));
txtAltitudeCorrection.Text = gpsc.Altitude.ToString();
}
gpsc.Time = new DateTime();
gpsc.Time.AddSeconds(e.data.GPSDateTime.Second);
gpsc.LatMinutes = (short)(LatCorrection * 60.0 * 10000.0);
gpsc.LongMinutes = (short)-(LongCorrection * 60.0 * 10000.0);
txtLatCorrection.Text = gpsc.LatMinutes.ToString();
txtLongCorrection.Text = gpsc.LongMinutes.ToString();
if (ComProt != null)
{
int timeout = 0;
while (ComProt.ExpectedResponse.ResponseExpected)
{
if (timeout > 1000)
{
break;
}
timeout++;
}
ComProt.SendGPSCorrectionFactor(gpsc);
RequestInfoTimer.Enabled = true;
}
}
}
}