// DLC received debug printout "I JUST DIED"
void xbee_ReceivedDLCMe(DeadLedControl dlcToMe)
{
if (dlcToMe.ControllingInt == 1)
{
razorVenus.TerminalPrintOut("\n\r*\n\r*\n\r*\n\r*\n\r*\n\r*\n\r*\n\r*\n\r*\n\r*I JUST DIED\n\r*I JUST DIED\n\r*I JUST DIED\n\r*I JUST DIED\n\r*I JUST DIED");
}
}
public static DeadLedControl Deserialize(byte[] line)
{
int a = (line[0] << 24) | (line[1] << 16) | (line[2] << 8) | (line[3] << 0);
DeadLedControl deserialized = new DeadLedControl(a);
return(deserialized);
}
public static byte[] Serialize(DeadLedControl dlc)
{
byte[] buffer = new byte[4]; // Size must be increased if DLC is changed.
Serialize(buffer, 0, dlc.controllingInt);
return(buffer);
}
// 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];
}
// DLCCreated Event Handler to send dlcToEnemy to other plane
void firingSolution_DLCCreated(DeadLedControl dlcToEnemy)
{
byte[] dlcBuffer = DeadLedControl.Serialize(dlcToEnemy);
UInt16 checkSum = CheckSumCalc(dlcBuffer);
// Convert the uInt16 checksum to a byte[]
byte[] checkSumBuffer = new byte[2];
checkSumBuffer[0] = (byte)(checkSum >> 8);
checkSumBuffer[1] = (byte)(checkSum);
physicalSerialPort.Write(dlcHeader, 0, dlcHeader.Length);
physicalSerialPort.Write(dlcBuffer, 0, dlcBuffer.Length);
physicalSerialPort.Write(checkSumBuffer, 0, checkSumBuffer.Length);
physicalSerialPort.Write(dlcFooter, 0, dlcFooter.Length);
}
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);
}
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