} // 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();
}
public void LedTester()
{
// System Ready Blinks
statusLed.On();
Thread.Sleep(200);
imuLed.On();
Thread.Sleep(200);
gpsLed.On();
Thread.Sleep(200);
deadLed.On();
Thread.Sleep(200);
killLed.On();
Thread.Sleep(2000);
statusLed.Off();
imuLed.Off();
gpsLed.Off();
deadLed.Off();
killLed.Off();
statusLed.BlinkyBlink();
Thread.Sleep(3000);
imuLed.BlinkyBlink();
Thread.Sleep(3000);
gpsLed.BlinkyBlink();
Thread.Sleep(3000);
deadLed.BlinkyBlink();
Thread.Sleep(3000);
killLed.BlinkyBlink();
Thread.Sleep(6000);
statusLed.Off();
imuLed.Off();
gpsLed.Off();
deadLed.Off();
killLed.Off();
statusLed.Blink(250);
Thread.Sleep(1000);
imuLed.Blink(250);
Thread.Sleep(1000);
gpsLed.Blink(250);
Thread.Sleep(1000);
deadLed.Blink(250);
Thread.Sleep(1000);
killLed.Blink(250);
Thread.Sleep(1000);
statusLed.Off();
imuLed.Off();
gpsLed.Off();
deadLed.Off();
killLed.Off();
string lineToPrint = "\n\rLed System Check Run Complete\n\r\n\r";
razorVenus.TerminalPrintOut(lineToPrint);
}
} // 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;
}
}
