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