} // Assigning positionEnemyReceived
// Insert trigger event handler, that changes a trigger variable true or false on both edges of trigger.
void positionComputer_AttNavCreated(AttNav shitIn)
{
lock (meLocker)
{
positionMeReceived = shitIn;
} // Now this event handler, which is running on clocked cycle, has an uptodate positionMe
forDebugPrint.TerminalPrintOut("\n\rDOP_e2: " + positionMeReceived.PositionDOP_e2.ToString() + " Time: " + positionMeReceived.GPSTimeInWeek_csec.ToString() + " Y: " + positionMeReceived.Yaw_deg.ToString() + " P: " + positionMeReceived.Pitch_deg.ToString());
#region gpsLed Blink(rate DOP dependent
if (positionMeReceived.PositionDOP_e2 == 0)
{
gpsLed.BlinkyBlink();
}
else
{
if (positionMeReceived.PositionDOP_e2 > 0 && positionMeReceived.PositionDOP_e2 <= 6000) // positionDOP, Ideal = 1000, good = 2000-5000, moderate = 6000-8000is, poor = up by 20,000
{
gpsLed.Off();
}
else
{
if (positionMeReceived.PositionDOP_e2 > 6000 && positionMeReceived.PositionDOP_e2 <= 2000)
{
gpsLed.Blink((30000 + ((20000000 - positionMeReceived.PositionDOP_e2 * 1000000) * 212)) / 1000); // This algorithm creates a scale from 20,000 to 6,000 that maps those end points to 30 to 3000, such that a dop of 20,000 => 30msec blink, and a dop of 6000 => 3000msec blink. Everything is a huge number because it was all mulitplied by 1000 for integer operations. 0.212 is the ratio of 14000 to 2970 (which is the deltas on each end of the scales begin mapped to each other)
}
else
{
gpsLed.On();
}
}
}
#endregion
checkKillShot();
}
void xbee_ReceivedAttNavEnemy(AttNav shitIn)
{
lock (enemyLocker)
{
positionEnemyReceived = shitIn;
}
forDebugPrint.TerminalPrintOut("\r\n\t\t\t\t\t\t\t\t\t\t\t\tDOP_e2: " + positionEnemyReceived.PositionDOP_e2 + " Time: " + positionEnemyReceived.GPSTimeInWeek_csec);
} // Assigning positionEnemyReceived
// Constructor
public Radio()
{
physicalSerialPort = new SerialPort("COM2", rate, Parity.None, 8, StopBits.One);
physicalSerialPort.DataReceived += new SerialDataReceivedEventHandler(physicalSerialPort_DataReceived);
physicalSerialPort.Open();
sizeAttNav = AttNav.Serialize(new AttNav()).Length; // Get size of a serialized AttNav
completeAttNav = new byte[sizeAttNav];
sizeDLC = DeadLedControl.Serialize(new DeadLedControl()).Length;
completeDLC = new byte[sizeDLC];
}
} // end Parse
// AttNavCreated Event Handler to send positionMe to the other plane
void positionComputer_AttNavCreated(AttNav positionMe)
{
byte[] buffer = AttNav.Serialize(positionMe);
UInt16 checkSum = CheckSumCalc(buffer);
// Convert the uInt16 checksum to a byte[]
byte[] checkSumBuffer = new byte[2];
checkSumBuffer[0] = (byte)(checkSum >> 8);
checkSumBuffer[1] = (byte)(checkSum); // Casting (byte)(checkSum) just took the least significant (second half of the 16 bits) byte and put it in byte[1]
physicalSerialPort.Write(attNavHeader, 0, attNavHeader.Length);
physicalSerialPort.Write(buffer, 0, buffer.Length);
physicalSerialPort.Write(checkSumBuffer, 0, checkSumBuffer.Length);
physicalSerialPort.Write(attNavFooter, 0, attNavFooter.Length);
}
public static AttNav Deserialize(byte[] line)
{
int a = (line[0] << 24) | (line[1] << 16) | (line[2] << 8) | (line[3] << 0);
int b = (line[4] << 24) | (line[5] << 16) | (line[6] << 8) | (line[7] << 0);
int c = (line[8] << 24) | (line[9] << 16) | (line[10] << 8) | (line[11] << 0);
int d = (line[12] << 24) | (line[13] << 16) | (line[14] << 8) | (line[15] << 0);
int e = (line[16] << 24) | (line[17] << 16) | (line[18] << 8) | (line[19] << 0);
int f = (line[20] << 24) | (line[21] << 16) | (line[22] << 8) | (line[23] << 0);
int g = (line[24] << 24) | (line[25] << 16) | (line[26] << 8) | (line[27] << 0);
int h = (line[28] << 24) | (line[29] << 16) | (line[30] << 8) | (line[31] << 0);
int i = (line[32] << 24) | (line[33] << 16) | (line[34] << 8) | (line[35] << 0);
int j = (line[36] << 24) | (line[37] << 16) | (line[38] << 8) | (line[39] << 0);
int k = (line[40] << 24) | (line[41] << 16) | (line[42] << 8) | (line[43] << 0);
AttNav deserialized = new AttNav(a, b, c, d, e, f, g, h, i, j, k);
return(deserialized);
}
public static byte[] Serialize(AttNav attNav)
{
byte[] buffer = new byte[44]; // Size must be increased if AttNav is changed.
Serialize(buffer, 0, attNav.x_cm);
Serialize(buffer, 4, attNav.y_cm);
Serialize(buffer, 8, attNav.z_cm);
Serialize(buffer, 12, attNav.pitch_mrad);
Serialize(buffer, 16, attNav.yaw_mrad);
Serialize(buffer, 20, attNav.roll_mrad);
Serialize(buffer, 24, attNav.latitude_e7);
Serialize(buffer, 28, attNav.longitude_e7);
Serialize(buffer, 32, attNav.mslAlt_cm);
Serialize(buffer, 36, attNav.positionDOP_e2);
Serialize(buffer, 40, attNav.gpsTimeInWeek_csec);
return(buffer);
}
public void Testing()
{
AttNav tAttNav = new AttNav(-234847615, -379916876, 453814132, 1600, 3100, 780, 456486502, -121722502, 150000, 1, 10000);
DeadLedControl tDLC = new DeadLedControl(1);
byte[] tDlCSerialized = DeadLedControl.Serialize(tDLC);
byte[] tAttNavSerialized = AttNav.Serialize(tAttNav);
byte[] fakeRadioStream1 = new byte[51];
fakeRadioStream1[0] = 0x00; // This is a dummy byte because InsertTestBuffer uses tailindex, which is often 1 because the datareceived event triggers on startup.
fakeRadioStream1[1] = 0xB0;
fakeRadioStream1[2] = 0xB1;
Array.Copy(tAttNavSerialized, 0, fakeRadioStream1, 3, tAttNavSerialized.Length);
UInt16 checkSum = Radio.CheckSumCalc(tAttNavSerialized);
fakeRadioStream1[47] = (byte)(checkSum >> 8);
fakeRadioStream1[48] = (byte)(checkSum);
fakeRadioStream1[49] = 0x0D;
fakeRadioStream1[50] = 0x0A;
byte[] fakeRadioStream2 = new byte[16];
fakeRadioStream2[0] = 0x44;
fakeRadioStream2[1] = 0x4C;
fakeRadioStream2[2] = 0x43;
Array.Copy(tDlCSerialized, 0, fakeRadioStream2, 3, tDlCSerialized.Length);
UInt16 checkSumtDLC = Radio.CheckSumCalc(tDlCSerialized);
fakeRadioStream2[7] = (byte)(checkSumtDLC >> 8);
fakeRadioStream2[8] = (byte)(checkSumtDLC);
fakeRadioStream2[9] = 0x0D;
fakeRadioStream2[10] = 0x0A;
fakeRadioStream2[11] = 0xAA; // Pretty sure these next bytes (11-15) are just noise, intended to make sure the parsing engine can handle noise showing up with real infomation.
fakeRadioStream2[12] = 0xAB;
fakeRadioStream2[13] = 0xAC;
fakeRadioStream2[14] = 0x0F;
fakeRadioStream2[15] = 0x01;
xbee.InsertTestBuffer(fakeRadioStream1);
// Thread.Sleep(10); // shitty sleep
xbee.InsertTestBuffer(fakeRadioStream2);
}
} // 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;
}
}
public void checkKillShot()
{
// Must copy the most recent positionMeReceived and positionEnemyReceived into the values that will be used.
lock (enemyLocker)
{
positionEnemy = positionEnemyReceived;
}
lock (meLocker)
{
positionMe = positionMeReceived;
// LTN: We should add a check here that checks to see if the positionMe has moved at all, and if it doesn't for a certain number of updates, throw a "stale data" flag
// LTN: Yes, and because there's only 1 instance of FiringSolution, the memory of previous position Me's can be stored in FiringSolution, overwriting every 5 or so.
// LTN&JPD: Maybe not, that sounds like a lot of work, when DOP should give us similar information.
}
// Guarding against bothering with the math if we have no positionEnemy
if (positionEnemy == null)
{
//statusLed.BlinkyBlink(); //commented out to chase down heartbeat stalling with Main.Testing() commented out 29May12
return;
}
if (positionEnemy.GPSTimeInWeek_csec == 0)
{
//statusLed.BlinkyBlink(); //commented out to chase down heartbeat stalling with Main.Testing() commented out 29May12
}
dlcForLogging = 0; //i'm lazy and don't want to figure out how to log the deadledcontrol class
double latitudeMe_rad = (((double)positionMe.Latitude_e7) / 10000000) * exMath.PI / 180;
double longitudeMe_rad = (((double)positionMe.Longitude_e7) / 10000000) * exMath.PI / 180;
int delta_ECEF_X_cm = positionEnemy.X_cm - positionMe.X_cm;
int delta_ECEF_Y_cm = positionEnemy.Y_cm - positionMe.Y_cm;
int delta_ECEF_Z_cm = positionEnemy.Z_cm - positionMe.Z_cm;
//see pic of matrix math: http://upload.wikimedia.org/wikipedia/en/math/6/c/5/6c5e10c1708acc1663d618c2f3fecc98.png
double east_m = -exMath.Sin(longitudeMe_rad) * delta_ECEF_X_cm / 100 + exMath.Cos(longitudeMe_rad) * delta_ECEF_Y_cm / 100;
double north_m = -exMath.Cos(longitudeMe_rad) * exMath.Cos(latitudeMe_rad) * delta_ECEF_X_cm / 100 - exMath.Sin(latitudeMe_rad) * exMath.Sin(longitudeMe_rad) * delta_ECEF_Y_cm / 100 + exMath.Cos(latitudeMe_rad) * delta_ECEF_Z_cm / 100;
double up_m = exMath.Cos(latitudeMe_rad) * exMath.Cos(longitudeMe_rad) * delta_ECEF_X_cm / 100 + exMath.Cos(latitudeMe_rad) * exMath.Sin(longitudeMe_rad) * delta_ECEF_Y_cm / 100 + exMath.Sin(latitudeMe_rad) * delta_ECEF_Z_cm / 100;
//distance_m = sqrt(dx^2 + dy^2 + dz^2)
double distance_m = System.Math.Pow(System.Math.Pow(east_m, 2) + System.Math.Pow(north_m, 2) + System.Math.Pow(up_m, 2), 0.5);
double toShootAzimuthOffset_rad; // offset converts traditional polar math to compass heading. See jeff's notebook drawings 3 Feb.
north_m = north_m + 1.1; //for debug only: add a smidge to north_m so you're not dividing by zero
// Determines how much offset to use:
if (east_m > 0 && north_m > 0) //pointing in quadrant I
{
toShootAzimuthOffset_rad = 0;
}
else if (north_m < 0) //pointing in quadrant II or III
{
toShootAzimuthOffset_rad = exMath.PI;
}
else if (east_m < 0 && north_m > 0)//pointing in quadrant IV
{
toShootAzimuthOffset_rad = 2 * exMath.PI;
}
else
{
toShootAzimuthOffset_rad = 999;
}
//double toShootAzimuth_rad = exMath.Atan(east_m / north_m);
toShootAzimuth_rad = exMath.Atan(east_m / north_m) + toShootAzimuthOffset_rad;
double toShootAzimuth_deg = toShootAzimuth_rad * 180 / 3.1415926; // To Do: Delete this if we aren't using it for printout anymore. It's a waste of computation. LTN 2012_12_02
forDebugPrint.TerminalPrintOut("\n\r\t\t\t\t\t\tTSA: " + toShootAzimuth_deg.ToString("F04") + "\trng_m: " + distance_m.ToString("F01"));
double denominator = System.Math.Pow((System.Math.Pow(east_m, 2) + System.Math.Pow(north_m, 2)), 0.5);
double toShootElevationAngle_rad = exMath.Atan(up_m / denominator);
//this is a tolernace to be used when comparing caclulated angles for killshot.
double deadbandTolerance_mrad = 175; // 50mrad = ~2.5deg //349mrad = ~20deg
//we want to add magCal to yaw in order to keep the magnetic quantities together; the other option is to add it to toShootAzimuth (which is a GPS based angle)
yawWithMagCal_mrad = positionMe.Yaw_mrad + yawOffsetFromMagCal_mrad;
// warning! Mod in Octave is not the same as % in c#.
if (yawWithMagCal_mrad < 0)
{
yawWithMagCal_mrad = yawWithMagCal_mrad + (2 * exMath.PI * 1000);
}
else
{
yawWithMagCal_mrad = yawWithMagCal_mrad % (2 * exMath.PI * 1000);
}
// Metal Detector Magic!!!!! (like finding a gold coin on the beach)
double maxOfAzimuths = exMath.Max(toShootAzimuth_rad * 1000, yawWithMagCal_mrad);
double minOfAzimuths = exMath.Min(toShootAzimuth_rad * 1000, yawWithMagCal_mrad);
double azimuthDelta_mrad = exMath.Min((2 * exMath.PI * 1000 - maxOfAzimuths) + minOfAzimuths, maxOfAzimuths - minOfAzimuths); // always pos by nature.
double elevationDelta_mrad = toShootElevationAngle_rad * 1000 - positionMe.Pitch_mrad;
double magnitudeOfDeltas_mrad = System.Math.Pow(azimuthDelta_mrad * azimuthDelta_mrad + elevationDelta_mrad * elevationDelta_mrad, 0.5);
#region Compare magnitudeOfDeltas to deadBandTolerance and DLC Throw
if (magnitudeOfDeltas_mrad < deadbandTolerance_mrad)
{
killLed.On();
forDebugPrint.TerminalPrintOut("\n\r$\n\r$\n\r$\n\r$\n\r$I be KILLIN a M**********r NOW\n\r$\n\r$\n\r$\n\r$");
DeadLedControl dlcToEnemy = new DeadLedControl(1);
dlcForLogging = 1;
if (this.DLCCreated != null)
{
this.DLCCreated(dlcToEnemy);
}
}
else
{
killLed.Blink((int)magnitudeOfDeltas_mrad);
DeadLedControl dlcToEnemy = new DeadLedControl(2);
if (this.DLCCreated != null)
{
this.DLCCreated(dlcToEnemy);
}
}
#endregion
loggingCounter++;
//TODO: add a guard aginst trying to log with no SD card installed
if (loggingCounter > 10) //ten was just for simplicity. main-loop (mane-loop?) is currently 10Hz, so this was to get 1Hz data.
{
loggingCounter = 0;
//logger.Initialize();
logger.Log(positionMe.GPSTimeInWeek_csec.ToString() + "\t" + positionMe.Latitude_e7.ToString() + "\t" + positionMe.Longitude_e7.ToString() + "\t" + positionMe.MSLAlt_cm.ToString() + "\t" + positionMe.PositionDOP_e2 + "\t" + toShootAzimuth_rad.ToString("F04") + "\t" + positionMe.Yaw_mrad.ToString() + "\t" + yawWithMagCal_mrad.ToString() + "\t" + toShootElevationAngle_rad.ToString("F04") + "\t" + positionMe.Pitch_mrad.ToString() + "\t" + distance_m.ToString("F04") + "\t" + positionEnemy.Latitude_e7.ToString() + "\t" + positionEnemy.Longitude_e7.ToString() + "\t" + positionEnemy.MSLAlt_cm.ToString() + "\t" + positionEnemy.PositionDOP_e2.ToString() + "\t" + dlcForLogging.ToString() + "\t" + yawOffsetFromMagCal_mrad.ToString());
//logger.Close();
logger.Flush();
}
}
private void parse()
{
// The next two lines should be put in the initializer. We don't need to calculate msgLength every time we parse, just once per instance. (LTN and JPD 5/18/2012)
int msgLengthAttNav = attNavHeader.Length + sizeAttNav + 2 + attNavFooter.Length; //header + attnav + checksum + footer
int msgLengthDLC = dlcHeader.Length + sizeDLC + 2 + dlcFooter.Length; // header + dlc + checksum + footer
//1) Identify a header, 2) enough data for a full msg of that header type 3) footer 4) checksum = 0;
if (attNavHeaderFound == false && dlcHeaderFound == false)
{
for (int i = 0; i < tailIndex; i++)
{
if (i <= tailIndex - dlcHeader.Length) // Just have the check for dlcHeaderLength, because it's longer, so if there are enough bytes for a dlc header, there are enough for an attnav not to overrun the index
{
if (bigBuffer[i] == attNavHeader[0] && bigBuffer[i + attNavHeader.Length - 1] == attNavHeader[attNavHeader.Length - 1])
{
tailIndex = tailIndex - i;
Array.Copy(bigBuffer, i, bigBuffer, 0, tailIndex);
attNavHeaderFound = true;
break;
}
if (bigBuffer[i] == dlcHeader[0] && bigBuffer[i + dlcHeader.Length - 1] == dlcHeader[dlcHeader.Length - 1])
{
tailIndex = tailIndex - i;
Array.Copy(bigBuffer, i, bigBuffer, 0, tailIndex);
dlcHeaderFound = 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;
}
}
}
}
// Have AttNavHeader and Plenty of Data? --> Create Attnav, or throw it away if can't find a footer.
if (attNavHeaderFound == true && tailIndex >= msgLengthAttNav)
{
if (bigBuffer[attNavHeader.Length + sizeAttNav + 2] == attNavFooter[0] &&
bigBuffer[attNavHeader.Length + sizeAttNav + 3] == attNavFooter[1])
{
//if the checksum returns zero
byte[] forCheckSumCalc = new byte[sizeAttNav + 2];
Array.Copy(bigBuffer, attNavHeader.Length, forCheckSumCalc, 0, forCheckSumCalc.Length);
if (CheckSumCalc(forCheckSumCalc) == 0)
{
Array.Copy(forCheckSumCalc, 0, completeAttNav, 0, sizeAttNav);
AttNav positionEnemy = AttNav.Deserialize(completeAttNav);
if (this.ReceivedAttNavEnemy != null)
{
this.ReceivedAttNavEnemy(positionEnemy); // Throwing the Event
}
}
}
tailIndex = tailIndex - msgLengthAttNav;
Array.Copy(bigBuffer, msgLengthAttNav, bigBuffer, 0, tailIndex);
attNavHeaderFound = false;
}
// Have dlcHeader and plenty of data? --> Create a DLC, or throw it away if can't find a footer.
if (dlcHeaderFound == true && tailIndex >= msgLengthDLC)
{
if (bigBuffer[dlcHeader.Length + sizeDLC + 2] == dlcFooter[0] &&
bigBuffer[dlcHeader.Length + sizeDLC + 3] == dlcFooter[1])
{
//if checksum returns zero
byte[] forCheckSumCalc = new byte[sizeDLC + 2];
Array.Copy(bigBuffer, dlcHeader.Length, forCheckSumCalc, 0, forCheckSumCalc.Length);
if (CheckSumCalc(forCheckSumCalc) == 0)
{
Array.Copy(forCheckSumCalc, 0, completeDLC, 0, sizeDLC);
DeadLedControl dlcToMe = DeadLedControl.Deserialize(completeDLC);
if (this.ReceivedImDeadDLC != null)
{
this.ReceivedImDeadDLC(dlcToMe);
}
// Control the LED here, as the dlc comes in. In future, can move this to firing solution if desired so that it runs on clockcycle.
if (dlcToMe.ControllingInt == 1)
{
//deadLed.On(); Update 02Sept2012. Aaawh, geeez. we're throwing a null exception here on deployment now. keyword: sanity // We could make the DLC represent how far off a shot is, and then blink accordingly. But for now, we'll just treat it like a bool - LTN 4/1/12
}
else
{
//deadLed.Off(); for some ungodly reason the code keeps breaking on a null reference here were i try to deploy. i've been working on magCal and sevenTimesKeyed--haven't mucked about with DLCs anywhere. haven't a clue why this is breaking. commented out to save my sanity and continue debugging magCal. JPD 28AUG2012
}
}
}
tailIndex = tailIndex - msgLengthDLC;
Array.Copy(bigBuffer, msgLengthDLC, bigBuffer, 0, tailIndex); // Every now and then I get a system....outOfRange exception on this line, because tailindex has somehow become -5. This makes me think maybe we need to lock bigbuffer somehowwhen we're doing Array.copy functions. All of this testing is with fakeradiostream and insertTestBuffer, so maybe that's the problem, and we won't have it with the physicalSerialPort Data Receieved event handler. To be tested.
dlcHeaderFound = false;
}
} // end Parse
