void dataSource_ReceivedLineGPS(LineGPS shitIn)
{ // Event Handler Catching
lock (gpsLocker)
{
gpsSampleReceived = shitIn;
}
gpsBool = true;
} // Assigning GpsSampleReceived
// DataRecieved Event Handler "Method"
void physicalSerialPort_DataReceived(object sender, SerialDataReceivedEventArgs e)
{
int bytesPutIn = physicalSerialPort.Read(bigBuffer, tailIndex, bigBuffer.Length - tailIndex);
tailIndex += bytesPutIn;
byte[] header = { 0xA0, 0xA1, 0x00, 0x3B, 0xA8 };
int headerFound = 0;
for (int i = 0; i < tailIndex - 4; i++)
{
if (bigBuffer[i] == header[0] && bigBuffer[i + 4] == header[4])
{ // See if we're lucky and have a whole header, if so, declare full header and go to next step.
if (i != 0) // keeps you from copying if you're lucky enough to have the header start at the start of bigBuffer
{
Array.Copy(bigBuffer, i, bigBuffer, 0, tailIndex);
tailIndex = tailIndex - i;
}
headerFound = 1;
}
else if (bigBuffer[i] == header[0] && i > 0)
{ // If not lucky, search for start of header and copy to the begining, then break to go get more bytesRead
Array.Copy(bigBuffer, i, bigBuffer, 0, tailIndex);
tailIndex = tailIndex - i;
break;
}
if (headerFound == 1 && tailIndex > 58)
{ // If you have a full header, and enough data to expect a full line, assign the line
Array.Copy(bigBuffer, 4, completeLine, 0, 59); // 4 used so that message will start with A8, the Message ID
Array.Copy(bigBuffer, 59, bigBuffer, 0, tailIndex);
tailIndex = tailIndex - 59;
LineGPS line = new LineGPS(completeLine);
if (this.ReceivedLineGPS != null)
{
this.ReceivedLineGPS(line);
}
}
else if (headerFound == 1)
{ // if you have a full header, but not enough data for a full line, stop the loop and go get more data.
break;
}
}
}
} // Assigning imuSampleReceived
public void Compute() // Compute runs on clocked cycle, called from Mane.Run()
{
// If statement avoids a race condition of Compute() being called before the event handlers have assigned gps and imu data to use
if (gpsBool == false || imuBool == false)
{
gpsLed.BlinkyBlink();
}
else
{
lock (gpsLocker)
{
gpsSampleToUse = gpsSampleReceived;
}
lock (imuLocker)
{
imuSampleToUse = imuSampleReceived;
}
// insert very clever kalman filter here....
// insert future vector math here....
if (gpsSampleToUse.GPSTimeInWeek_csec > gpsTimeLast) // gpsTimeLast1
{
AttNav positionMe = new AttNav(gpsSampleToUse.ECEF_X_cm, gpsSampleToUse.ECEF_Y_cm, gpsSampleToUse.ECEF_Z_cm, imuSampleToUse.Pitch_mrad, imuSampleToUse.Yaw_mrad, imuSampleToUse.Roll_mrad, gpsSampleToUse.Latitude_e7, gpsSampleToUse.Longitude_e7, gpsSampleToUse.MSLAlt_cm, gpsSampleToUse.PositionDOP_e2, gpsSampleToUse.GPSTimeInWeek_csec);
if (this.AttNavCreated != null) // Pitches AttNav to anyone who might subscribe (Radio and FiringSolution)
{
this.AttNavCreated(positionMe);
}
}
else
{ // Creates an AttNav with GPS Time, and positionDOP zero'ed out if gps time has not updated...indicates stale attnav.
AttNav positionMe = new AttNav(gpsSampleToUse.ECEF_X_cm, gpsSampleToUse.ECEF_Y_cm, gpsSampleToUse.ECEF_Z_cm, imuSampleToUse.Pitch_mrad, imuSampleToUse.Yaw_mrad, imuSampleToUse.Roll_mrad, gpsSampleToUse.Latitude_e7, gpsSampleToUse.Longitude_e7, gpsSampleToUse.MSLAlt_cm, 0, 0);
if (this.AttNavCreated != null) // pitches AttNav to anyone who might subscribe (Radio)
{
this.AttNavCreated(positionMe);
}
}
gpsTimeLast = gpsSampleToUse.GPSTimeInWeek_csec;
}
}
// DataRecieved Event Handler "Method"
void physicalSerialPort_DataReceived(object sender, SerialDataReceivedEventArgs e)
{
int bytesPutIn = physicalSerialPort.Read(bigBuffer, tailIndex, bigBuffer.Length - tailIndex);
tailIndex += bytesPutIn;
// Parse baby, parse
if (gpsHeaderFound == false && imuHeaderFound == false)
{
for (int i = 0; i < tailIndex; i++)
{
if (i <= tailIndex - gpsHeader.Length)
{
if (bigBuffer[i] == gpsHeader[0] && bigBuffer[i + gpsHeader.Length - 1] == gpsHeader[gpsHeader.Length - 1])
{
tailIndex = tailIndex - i;
Array.Copy(bigBuffer, i, bigBuffer, 0, tailIndex);
gpsHeaderFound = true;
break;
}
if (bigBuffer[i] == imuHeader[0] && bigBuffer[i + imuHeader.Length - 1] == imuHeader[imuHeader.Length - 1])
{
tailIndex = tailIndex - i;
Array.Copy(bigBuffer, i, bigBuffer, 0, tailIndex);
imuHeaderFound = true;
break;
}
}
else
{
if (i > 0)
{//if no headers found, copy what's left back to the start of bigbuffer, and wait for more data...
tailIndex = tailIndex - i;
Array.Copy(bigBuffer, i, bigBuffer, 0, tailIndex);
break;
}
}
}
}
if (gpsHeaderFound == true && tailIndex > 65) // Note: tailIndex is Litterally, the number of bytes we have in bigBuffer...which means the largest index possible is 64... but by waiting for 1 more byte to come in (because of the >), we make the indexing easier to look at in the below lines as well.
{
if (bigBuffer[64] == gpsFooter[0] && bigBuffer[65] == gpsFooter[1])
{
Array.Copy(bigBuffer, 4, completeGPSLine, 0, 59); //this is an extra aray.copy, that should be replaced by just throwing the line from bigbuffer[4:59], but it's late, and i'm unsure of the syntax
LineGPS line = new LineGPS(completeGPSLine);
if (this.ReceivedLineGPS != null) //if someone has subscribed to this event, throw it.
{
this.ReceivedLineGPS(line);
}
}
tailIndex = tailIndex - 65;
Array.Copy(bigBuffer, 65, bigBuffer, 0, tailIndex);
gpsHeaderFound = false;
}
if (imuHeaderFound == true && tailIndex > 15)
{
if ((bigBuffer[14] == imuFooter[0] && bigBuffer[15] == imuFooter[1])) //POSSIBLE OFF BY 1 ERROR, CHECK THE 15 INDEX ABOVE
{
Array.Copy(bigBuffer, 2, completeIMULine, 0, 14);
LineIMU line = new LineIMU(completeIMULine);
if (this.ReceivedLineIMU != null)
{
this.ReceivedLineIMU(line);
}
}
tailIndex = tailIndex - 15;
Array.Copy(bigBuffer, 15, bigBuffer, 0, tailIndex);
imuHeaderFound = false;
}
}
