} // Assigning GpsSampleReceived
void dataSource_ReceivedLineIMU(LineIMU shitIn)
{
lock (imuLocker)
{
imuSampleReceived = shitIn;
}
imuBool = true;
} // Assigning imuSampleReceived
// DataRecieved Event Handler "Method"
void physicalSerialPort_DataReceived(object sender, SerialDataReceivedEventArgs e)
{
byte[] buffer = new byte[1024];
int lineStop = 0;
physicalSerialPort.Read(buffer, 0, physicalSerialPort.BytesToRead < buffer.Length ? physicalSerialPort.BytesToRead : buffer.Length);
try
{
bigBuffer += new String(Encoding.UTF8.GetChars(buffer));
}
catch (Exception)
{
bigBuffer = "";
return;
}
do
{
lineStop = bigBuffer.IndexOf('\n');
if (lineStop != -1)
{
int lineStart = bigBuffer.LastIndexOf('!', 0, lineStop);
if (lineStart != -1)
{
//LineIMU line = new LineIMU(bigBuffer.Substring(lineStart + 1, lineStop - lineStart - 2));
if (this.ReceivedLineIMU != null)
{
// this.ReceivedLineIMU(line);
}
}
bigBuffer = bigBuffer.Substring(lineStop + 1);
}
else if (bigBuffer.Length >= LineIMU.LineMaxChars() * 2)
{
bigBuffer = "";
}
} while (lineStop != -1);
}
} // 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;
}
}
