/// <summary>
/// Class containing the latitude and longitude sent through the protocol.
/// </summary>
/// <param name="Lat"></param>
/// <param name="Long"></param>
public LocationPacketReceivedEventArgs(Latitude Lat, Longitude Long)
{
this.Lat = Lat;
this.Long = Long;
}
/// <summary>
/// Send the helicopter to a gps cooridinate, and specify the action on arrival
/// </summary>
/// <param name="Lat">A filled Latitude struct</param>
/// <param name="Long">a filled longitude struct</param>
/// <param name="action">0x48 to hover on arrival and 0x53 to circle at arrival</param>
/// <param name="Altitude">Altitude to perform action at (metres)</param>
public void Goto(Latitude Lat, Longitude Long, byte action, UInt16 Altitude)
{
/*
1 0xA5 header
0x5A
0x0F number of bytes in data
0x46 Command type “Flight Ops”
0x47 Command “Go To”
6 0xDD Degrees (0-180) Latitude
0xMM Minutes (0-59) Latitude
0xSSSS Seconds(0-59) Latitude. Use upper six bits for integer portion of seconds, use remaining 10 bits for decimal portion of seconds. There are approximately 31 meters in one second of latitude (and slightly less in 1 minute of longitude at our distance from the equator). 31m / 2^10 = 3 cm accuracy, which is way more accuracy than the GPS will determine.
0xNS 0x4E = North
0x53 = South
11 0xDD Degrees (0-180) Longitude
0xMM Minutes (0-59) Longitude
0xSSSS Seconds(0-59) Longitude. See above for more details.
0xEW 0x45 = East
0x57 = West
16 0xAA Action on Arrival:
IF 0x48 Hover at given altitude ELSE IF 0x53 Circle at given altitude
0xAAAA Given altitude
19 0xXX checksum High
0xXX checksum Low
0xCC
22 0x33 footer
*/
if (ExpectedResponse.ResponseExpected == false)
{
OutGoingPacket = new byte[22];
OutGoingPacket[0] = 0xA5;
OutGoingPacket[1] = 0x5A;
OutGoingPacket[2] = 0x0F;//number of bytes
OutGoingPacket[3] = 0x46;//Command type “Flight Ops”
OutGoingPacket[4] = 0x47;
OutGoingPacket[5] = Lat.Degrees;
OutGoingPacket[6] = Lat.Minutes;
//highest 2 bits of the first seconds packet were used for bits 9 and 10 of the secondsL value
OutGoingPacket[7] = (byte)((Lat.FractionalMinutes & 0xFF00) >> 8);
OutGoingPacket[8] = (byte)(Lat.FractionalMinutes & 0xFF );
if (Lat.North)
{
OutGoingPacket[9] = 0x4E;
}
else
{
OutGoingPacket[9] = 0x53;
}
OutGoingPacket[10] = Long.Degrees;
OutGoingPacket[11] = Long.Minutes;
//highest 2 bits of the first seconds packet were used for bits 9 and 10 of the secondsL value
OutGoingPacket[12] = (byte)((Long.FractionalMinutes & 0xFF00)>>8);
OutGoingPacket[13] = (byte)(Long.FractionalMinutes & 0xFF);
if (Long.East)
{
OutGoingPacket[14] = 0x45;
}
else
{
OutGoingPacket[14] = 0x57;
}
if (action == 0x48)
{
OutGoingPacket[15] = 0x48;
}
else if (action == 0x53)
{
OutGoingPacket[15] = 0x53;
}
else
{
throw new Exception("CommProtocol.Goto(): invalid action specified. Must be either 0x48 or 0x53");
}
OutGoingPacket[16] = (byte)((Altitude & 0xFF00) >> 8);//altitude high byte
OutGoingPacket[17] = (byte)(Altitude & 0x00FF);//altitude low byte
//calculate checksum
short checksum = 0;
for (int i = 2; i < 18; i++)//calculate checksum
{
checksum += OutGoingPacket[i];
}
OutGoingPacket[18] = (byte)((checksum & 0xFF00) >> 8);//checksum high
OutGoingPacket[19] = (byte)(checksum & 0x00FF);//checksum low
OutGoingPacket[20] = 0xCC;
OutGoingPacket[21] = 0x33;//footer
SendPacket("Goto", ExpectedResponses.type.FullPacketResponse);
}
}
private void ParseLocationPacket()
{
/*
Location Packet:
1 0xA5 SOT1
0x5A SOT2
3 0x0B bytes
0x74 Report type “Telemetry”
5 0x4C Report “Location”
0xDD Degrees (0-180) Latitude
0xMM Minutes (0-59) Latitude
0xSSSS Seconds (0-59) Latitude. Use first six bits for integer portion of seconds, use remaining 10 bits for decimal portion of seconds. There are approximately 31 meters in one second of latitude (and slightly less in 1 minute of longitude at our distance from the equator). 31m / 2^10 = 3 cm accuracy, which is way more accuracy than the GPS will determine.
10 0xNS 0x4E = North or 0x53 = South
0xDD Degrees (0-180) Longitude
0xMM Minutes (0-59) Longitude
0xSSSS Seconds (0-59) Longitude. See above for more details.
15 0xEW 0x45 = East or 0x57 = West
0xXX Checksum 1
0xXX Checksum 2
0xCC EOT1
19 0x33 EOT2
*/
if (IncomingDataBuffer.Length == 19 && (int)IncomingDataBuffer[17] == 0xCC && (int)IncomingDataBuffer[18] == 0x33)
{
//calculate checksum
UInt16 sum = 0;
for (int i = 2; i < 15; i++)
{
sum += (ushort)IncomingDataBuffer[i];
}
byte chk1 = (byte)((sum & 0xFF00) >> 8);
byte chk2 = (byte)(sum & 0x00FF);
if (chk1 != (int)IncomingDataBuffer[15] || chk2 != (int)IncomingDataBuffer[16])
{
Invoke(BadPacketReceived, new object[] {
this,
new BadPacketReceivedEventArgs(IncomingDataBuffer,
string.Format("Invalid Location packet: invalid checksum received: {0:x4}, expected: {1:x4}",
(Convert.ToUInt16((int)IncomingDataBuffer[15]) << 8) + (int)IncomingDataBuffer[16], sum))
}
);
ClearBuffer();
}
else
{
//checksum ok
Latitude Lat = new Latitude();
Lat.Degrees = (byte)IncomingDataBuffer[5];
Lat.Minutes = (byte)IncomingDataBuffer[6];
Lat.FractionalMinutes = (ushort)(((ushort)IncomingDataBuffer[7] << 8) + (ushort)IncomingDataBuffer[8]);
if ((byte)IncomingDataBuffer[9] == 0x4E)
{
Lat.North = true;
}
else if ((byte)IncomingDataBuffer[9] == 0x4E)
{
Lat.North = false;
}
else
{
Invoke(BadPacketReceived, new object[] { this, new BadPacketReceivedEventArgs(IncomingDataBuffer, "Invalid packet: The north south byte of the Latitude in a received location packet is invalid.") });
ClearBuffer();
return;
}
Longitude Long = new Longitude();
Long.Degrees = (byte)IncomingDataBuffer[10];
Long.Minutes = (byte)IncomingDataBuffer[11];
Long.FractionalMinutes = (ushort)(((ushort)IncomingDataBuffer[12] << 8) + (ushort)IncomingDataBuffer[13]);
if ((byte)IncomingDataBuffer[14] == 0x45)
{
Long.East = true;
}
else if ((byte)IncomingDataBuffer[14] == 0x57)
{
Long.East = false;
}
else
{
Invoke(BadPacketReceived, new object[] {this,
new BadPacketReceivedEventArgs(IncomingDataBuffer,
"Invalid packet: The east west byte of the Longitude in a received location packet is invalid.")
});
ClearBuffer();
return;
}
//invoke the event
ClearBuffer();
Invoke(LocationPacketReceived, new object[] { this, new LocationPacketReceivedEventArgs(Lat, Long) });
}
}
else if (IncomingDataBuffer.Length >= 19)
{
Invoke(BadPacketReceived, new object[] {
this, new BadPacketReceivedEventArgs(IncomingDataBuffer, "Bad location packet received")
});
ClearBuffer();
}
}
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;
}
}
}
}
void ComProt_LocationPacketReceived(object sender, CommProtocol.LocationPacketReceivedEventArgs e)
{
CommPacketRXLight.On = true;
PacketReceived = true;
if (e.Long.Degrees != 0 && e.Lat.Degrees != 0)
{
Lon = e.Long;
Lat = e.Lat;
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.GotoLoc(Latitude, Longitude);
}
}
private void cp_LocationPacketReceived(object sender, CommProtocol.LocationPacketReceivedEventArgs e)
{
ConsecutiveBadPackets = 0;
Lat = e.Lat;
Lon = e.Long;
Double Longitude = -(Lon.Degrees + Lon.Minutes / 60.0 + (double)(Lon.FractionalMinutes) / 60000.0);
Double Latitude = Lat.Degrees + Lat.Minutes / 60.0 + (double)(Lat.FractionalMinutes) / 60000.0;
textBox1.AppendText("Location packet received: Location" +
" Lat degrees: " + Lat.Degrees +
" Lat minutes: " + Lat.Minutes +
" Lat minutes remainder: " + Lat.FractionalMinutes +
" North: " + Lat.North +
" floating Latitude: " + Latitude +
" Long degrees: " + Lon.Degrees +
" Long minutes: " + Lon.Minutes +
" Long minutes remainder: " + Lon.FractionalMinutes +
" East: " + Lon.East +
" floating Longitude: " + Longitude + "\r\n");
if (!ManualMode)
{
InsertRowToReceived_packetsTable("Location",
" Lat degrees: " + Lat.Degrees +
" Lat minutes: " + Lat.Minutes +
" Lat minutes remainder: " + Lat.FractionalMinutes +
" North: " + Lat.North +
" Long degrees: " + Lon.Degrees +
" Long minutes: " + Lon.Minutes +
" Long minutes remainder: " + Lon.FractionalMinutes +
" East: " + Lon.East);
}
}
private void btnGoto_Click(object sender, EventArgs e)
{
Latitude lat = new Latitude();
lat.Degrees= 0;
lat.Minutes = 0;
lat.North = true;
lat.FractionalMinutes = 0;
Longitude lon = new Longitude();
lon.Degrees = 0;
lon.Minutes = 0;
lon.East = true;
lon.FractionalMinutes = 0;
cp.Goto(lat,lon,0x48,50);
}
