private void timer1_Tick(object sender, EventArgs e)
{
double heading;
double SCALE = 2000;
lru0.update();
btRoll.Text = lru0.dataParam[(int)PARAM.ROLL].ToString("F2");
btPitch.Text = lru0.dataParam[(int)PARAM.PITCH].ToString("F2");
BtVolts.Text = lru0.dataParam[(int)PARAM.VOLTS].ToString("F1");
btMagX.Text = lru0.dataParam[(int)PARAM.MAGX].ToString("F1");
btMagY.Text = lru0.dataParam[(int)PARAM.MAGY].ToString("F1");
btMagZ.Text = lru0.dataParam[(int)PARAM.MAGZ].ToString("F1");
btIas.Text = lru0.dataParam[(int)PARAM.IAS].ToString("F1");
double alt = lru0.dataParam[(int)PARAM.BCA] * 10;
btBca.Text = alt.ToString("F1");
btOat.Text = lru0.dataParam[(int)PARAM.OATRAW].ToString("F1");
btTas.Text = lru0.dataParam[(int)PARAM.TAS].ToString("F1");
btAoa.Text = lru0.dataParam[(int)PARAM.AOA].ToString("F1");
btMagXMin.BackColor = Color.Transparent;
btMagYMin.BackColor = Color.Transparent;
btMagZMin.BackColor = Color.Transparent;
btMagXMax.BackColor = Color.Transparent;
btMagYMax.BackColor = Color.Transparent;
btMagZMax.BackColor = Color.Transparent;
btMagXNorm.BackColor = Color.Transparent;
btMagYNorm.BackColor = Color.Transparent;
btMagZNorm.BackColor = Color.Transparent;
if ((lru0.dataParam[(int)PARAM.MAGX] != 0) &&
(lru0.dataParam[(int)PARAM.MAGY] != 0) &&
(lru0.dataParam[(int)PARAM.MAGZ] != 0))
{
// Mag Min
if (lru0.dataParam[(int)PARAM.MAGX] < double.Parse(btMagXMin.Text))
{
btMagXMin.Text = (lru0.dataParam[(int)PARAM.MAGX]).ToString("F1");
btMagXMin.BackColor = Color.DarkGreen;
}
if (lru0.dataParam[(int)PARAM.MAGY] < double.Parse(btMagYMin.Text))
{
btMagYMin.Text = (lru0.dataParam[(int)PARAM.MAGY]).ToString("F1");
btMagYMin.BackColor = Color.DarkGreen;
}
if (lru0.dataParam[(int)PARAM.MAGZ] < double.Parse(btMagZMin.Text))
{
btMagZMin.Text = (lru0.dataParam[(int)PARAM.MAGZ]).ToString("F1");
btMagZMin.BackColor = Color.DarkGreen;
}
// Mag Max
if (lru0.dataParam[(int)PARAM.MAGX] > double.Parse(btMagXMax.Text))
{
btMagXMax.Text = (lru0.dataParam[(int)PARAM.MAGX]).ToString("F1");
btMagXMax.BackColor = Color.DarkGreen;
}
if (lru0.dataParam[(int)PARAM.MAGY] > double.Parse(btMagYMax.Text))
{
btMagYMax.Text = (lru0.dataParam[(int)PARAM.MAGY]).ToString("F1");
btMagYMax.BackColor = Color.DarkGreen;
}
if (lru0.dataParam[(int)PARAM.MAGZ] > double.Parse(btMagZMax.Text))
{
btMagZMax.Text = (lru0.dataParam[(int)PARAM.MAGZ]).ToString("F1");
btMagZMax.BackColor = Color.DarkGreen;
}
btMagXDelta.Text = Math.Abs(double.Parse(btMagXMax.Text) - double.Parse(btMagXMin.Text)).ToString("F1");
btMagYDelta.Text = Math.Abs(double.Parse(btMagYMax.Text) - double.Parse(btMagYMin.Text)).ToString("F1");
btMagZDelta.Text = Math.Abs(double.Parse(btMagZMax.Text) - double.Parse(btMagZMin.Text)).ToString("F1");
btMagXOffset.Text = (-double.Parse(btMagXMin.Text) - (double.Parse(btMagXDelta.Text) / 2)).ToString("F2");
btMagYOffset.Text = (-double.Parse(btMagYMin.Text) - (double.Parse(btMagYDelta.Text) / 2)).ToString("F2");
btMagZOffset.Text = (-double.Parse(btMagZMin.Text) - (double.Parse(btMagZDelta.Text) / 2)).ToString("F2");
btMagXCent.Text = (lru0.dataParam[(int)PARAM.MAGX] + double.Parse(btMagXOffset.Text)).ToString("F3");
btMagYCent.Text = (lru0.dataParam[(int)PARAM.MAGY] + double.Parse(btMagYOffset.Text)).ToString("F3");
btMagZCent.Text = (lru0.dataParam[(int)PARAM.MAGZ] + double.Parse(btMagZOffset.Text)).ToString("F3");
btMagXNorm.Text = (double.Parse(btMagXCent.Text) / (double.Parse(btMagXDelta.Text) / 2)).ToString("F3");
btMagYNorm.Text = (double.Parse(btMagYCent.Text) / (double.Parse(btMagYDelta.Text) / 2)).ToString("F3");
btMagZNorm.Text = (double.Parse(btMagZCent.Text) / (double.Parse(btMagZDelta.Text) / 2)).ToString("F3");
if (Math.Abs(double.Parse(btMagXNorm.Text)) > 0.98)
{
btMagXNorm.BackColor = Color.DarkGreen;
}
if (Math.Abs(double.Parse(btMagYNorm.Text)) > 0.98)
{
btMagYNorm.BackColor = Color.DarkGreen;
}
if (Math.Abs(double.Parse(btMagZNorm.Text)) > 0.98)
{
btMagZNorm.BackColor = Color.DarkGreen;
}
// Computed Heading
heading = Math.Atan2(double.Parse(btMagYNorm.Text), double.Parse(btMagXNorm.Text));
if (heading < 0)
{
heading += 2 * 3.14;
}
heading *= 180 / 3.14;
if ((heading > 0) && (heading < 360))
{
heading = 360.0 - heading;
currentHeading = accumHeading / 10;
accumHeading += heading - currentHeading;
}
btHeading.Text = currentHeading.ToString("F2");
//heading = Math.Atan2(AhrsCom.magY / SCALE, AhrsCom.magX / SCALE);
//if (heading < 0)
//{
// heading += 2 * 3.14;
//}
//heading *= 180 / 3.14;
//btHeading.Text = heading.ToString("F1");
}
this.Invalidate();
}
private void mainFormTimer_Tick(object sender, EventArgs e)
{
lru.update();
}
private void timer1_Tick(object sender, EventArgs e)
{
lru0.update();
// Setup outputs
if ((lru0.unitConnected) && firstConnect)
{
lru0.setParameterReq((int)PARAM.STATUS, RATE.RATE_1HZ);
firstConnect = false;
}
//btGyroZRaw.Text = lru0.dataParam[(int)PARAM.BTEMP].ToString();
//btVolts.Text = lru0.dataParam[(int)PARAM.VOLTS].ToString();
switch (dataSource)
{
case (int)SOURCE.GYRO_RAW0:
// Gyro display
lbSource.Text = "Gyro 0 Raw";
Chart.addNewPoint(lru0.dataParam[(int)PARAM.GYROXR0], lru0.dataParam[(int)PARAM.GYROYR0], lru0.dataParam[(int)PARAM.GYROZR0]);
btParmX.Text = lru0.dataParam[(int)PARAM.GYROXR0].ToString("0.00000");
btParmY.Text = lru0.dataParam[(int)PARAM.GYROYR0].ToString("0.00000");
btParmZ.Text = lru0.dataParam[(int)PARAM.GYROZR0].ToString("0.00000");
btParmA.Text = lru0.dataParam[(int)PARAM.FIFOCOUNT].ToString("000");
break;
case (int)SOURCE.GYRO_RAW1:
// Gyro display
lbSource.Text = "Gyro 1 Raw";
Chart.addNewPoint(lru0.dataParam[(int)PARAM.GYROXR1], lru0.dataParam[(int)PARAM.GYROYR1], lru0.dataParam[(int)PARAM.GYROZR1]);
btParmX.Text = lru0.dataParam[(int)PARAM.GYROXR1].ToString("0.00000");
btParmY.Text = lru0.dataParam[(int)PARAM.GYROYR1].ToString("0.00000");
btParmZ.Text = lru0.dataParam[(int)PARAM.GYROZR1].ToString("0.00000");
btParmA.Text = lru0.dataParam[(int)PARAM.FIFOCOUNT].ToString("000");
break;
case (int)SOURCE.GYRO_CAL:
// Gyro display
lbSource.Text = "Cal Gyro";
Chart.addNewPoint(lru0.dataParam[(int)PARAM.GYROXC], lru0.dataParam[(int)PARAM.GYROYC], lru0.dataParam[(int)PARAM.GYROZC]);
btParmX.Text = lru0.dataParam[(int)PARAM.GYROXC].ToString("0.00000");
btParmY.Text = lru0.dataParam[(int)PARAM.GYROYC].ToString("0.00000");
btParmZ.Text = lru0.dataParam[(int)PARAM.GYROZC].ToString("0.00000");
btParmA.Text = ("-");
break;
case (int)SOURCE.GYRO_SUM:
// Gyro sum display
//Chart.addNewPoint(lru0.dataParam[(int)PARAM.GYROXSUM], lru0.dataParam[(int)PARAM.GYROYSUM], lru0.dataParam[(int)PARAM.GYROZSUM]);
//btParmX.Text = lru0.dataParam[(int)PARAM.GYROXSUM].ToString("000.00");
//btParmY.Text = lru0.dataParam[(int)PARAM.GYROYSUM].ToString("000.00");
//btParmZ.Text = lru0.dataParam[(int)PARAM.GYROZSUM].ToString("000.00");
lbSource.Text = "Gyro Bias";
Chart.addNewPoint(lru0.dataParam[(int)PARAM.OI0], lru0.dataParam[(int)PARAM.OI1], lru0.dataParam[(int)PARAM.OI2]);
btParmX.Text = lru0.dataParam[(int)PARAM.OI0].ToString("0.0000");
btParmY.Text = lru0.dataParam[(int)PARAM.OI1].ToString("0.0000");
btParmZ.Text = lru0.dataParam[(int)PARAM.OI2].ToString("0.0000");
btParmA.Text = ("-");
break;
case (int)SOURCE.ACCEL_RAW0:
// Accel display
lbSource.Text = "Accel 0 Raw";
Chart.addNewPoint(lru0.dataParam[(int)PARAM.ACCELXR0], lru0.dataParam[(int)PARAM.ACCELYR0], lru0.dataParam[(int)PARAM.ACCELZR0]);
btParmX.Text = lru0.dataParam[(int)PARAM.ACCELXR0].ToString("0.00000");
btParmY.Text = lru0.dataParam[(int)PARAM.ACCELYR0].ToString("0.00000");
btParmZ.Text = lru0.dataParam[(int)PARAM.ACCELZR0].ToString("0.00000");
btParmA.Text = ("-");
break;
case (int)SOURCE.ACCEL_RAW1:
// Accel display
lbSource.Text = "Accel 1 Raw";
Chart.addNewPoint(lru0.dataParam[(int)PARAM.ACCELXR1], lru0.dataParam[(int)PARAM.ACCELYR1], lru0.dataParam[(int)PARAM.ACCELZR1]);
btParmX.Text = lru0.dataParam[(int)PARAM.ACCELXR1].ToString("0.00000");
btParmY.Text = lru0.dataParam[(int)PARAM.ACCELYR1].ToString("0.00000");
btParmZ.Text = lru0.dataParam[(int)PARAM.ACCELZR1].ToString("0.00000");
btParmA.Text = ("-");
break;
case (int)SOURCE.ACCEL_CAL:
// Accel display
lbSource.Text = "Accel Cal";
Chart.addNewPoint(lru0.dataParam[(int)PARAM.ACCELXC], lru0.dataParam[(int)PARAM.ACCELYC], lru0.dataParam[(int)PARAM.ACCELZC]);
btParmX.Text = lru0.dataParam[(int)PARAM.ACCELXC].ToString("0.00000");
btParmY.Text = lru0.dataParam[(int)PARAM.ACCELYC].ToString("0.00000");
btParmZ.Text = lru0.dataParam[(int)PARAM.ACCELZC].ToString("0.00000");
btParmA.Text = ("-");
break;
case (int)SOURCE.RPY:
// Roll Pitch Yaw
lbSource.Text = "Roll Pitch Yaw";
Chart.addNewPoint(lru0.dataParam[(int)PARAM.ROLL], lru0.dataParam[(int)PARAM.PITCH], lru0.dataParam[(int)PARAM.YAW]);
btParmX.Text = lru0.dataParam[(int)PARAM.ROLL].ToString("000.000");
btParmY.Text = lru0.dataParam[(int)PARAM.PITCH].ToString("000.000");
btParmZ.Text = lru0.dataParam[(int)PARAM.YAW].ToString("000.000");
btParmA.Text = lru0.dataParam[(int)PARAM.YAWT].ToString("000.000");
break;
case (int)SOURCE.WEIGHT:
// Weighting
lbSource.Text = "Weight";
Chart.addNewPoint(lru0.dataParam[(int)PARAM.RPYWEIGHT], lru0.dataParam[(int)PARAM.BIASWEIGHT], lru0.dataParam[(int)PARAM.AHRSRPYGATE]);
btParmX.Text = lru0.dataParam[(int)PARAM.RPYWEIGHT].ToString("00.0");
btParmY.Text = lru0.dataParam[(int)PARAM.BIASWEIGHT].ToString("00.0");
btParmZ.Text = lru0.dataParam[(int)PARAM.AHRSRPYGATE].ToString("0.000");
btParmA.Text = ("-");
break;
case (int)SOURCE.STATS1:
// Status Group 1
Chart.addNewPoint(0, lru0.dataParam[(int)PARAM.BATVOLTS], 0);
lbSource.Text = "Stats Group 1";
btParmX.Text = lru0.dataParam[(int)PARAM.UPDATERATE].ToString("00.0");
btParmY.Text = lru0.dataParam[(int)PARAM.BATVOLTS].ToString("00.0");
btParmA.Text = ("-");
break;
case (int)SOURCE.ADC:
// Air Data
Chart.addNewPoint(0, lru0.dataParam[(int)PARAM.IAS], lru0.dataParam[(int)PARAM.VS]);
lbSource.Text = "Air Data";
btParmX.Text = (lru0.dataParam[(int)PARAM.BCA] * 10).ToString("00,000.0"); // Scaled
btParmY.Text = lru0.dataParam[(int)PARAM.IAS].ToString("000.0");
btParmZ.Text = lru0.dataParam[(int)PARAM.AOA].ToString("000.0");
btParmA.Text = ("-");
break;
case (int)SOURCE.HEATERS:
// Heaters
lbSource.Text = "Heater Temps";
Chart.addNewPoint(lru0.dataParam[(int)PARAM.TEMPP], lru0.dataParam[(int)PARAM.TEMPS], 0);
btParmX.Text = lru0.dataParam[(int)PARAM.TEMPP].ToString("00.0");
btParmY.Text = lru0.dataParam[(int)PARAM.TEMPS].ToString("00.0");
btParmZ.Text = lru0.dataParam[(int)PARAM.OATRAW].ToString("0000");
btParmA.Text = ("-");
break;
case (int)SOURCE.AIRRAW:
// Heaters
lbSource.Text = "Air Data Raw";
//Chart.addNewPoint(lru0.dataParam[(int)PARAM.TEMPP], lru0.dataParam[(int)PARAM.TEMPS], 0);
btParmX.Text = lru0.dataParam[(int)PARAM.SENP].ToString("0000.0");
btParmY.Text = lru0.dataParam[(int)PARAM.SENS].ToString("0000.0");
btParmZ.Text = lru0.dataParam[(int)PARAM.OATRAW].ToString("0000");
btParmA.Text = ("-");
break;
case (int)SOURCE.RATES:
// Heaters
lbSource.Text = "AHRS Rates";
Chart.addNewPoint(lru0.dataParam[(int)PARAM.ROLLRATE], lru0.dataParam[(int)PARAM.PITCHRATE], lru0.dataParam[(int)PARAM.YAWRATE]);
btParmX.Text = lru0.dataParam[(int)PARAM.ROLLRATE].ToString("000.000");
btParmY.Text = lru0.dataParam[(int)PARAM.PITCHRATE].ToString("000.000");
btParmZ.Text = lru0.dataParam[(int)PARAM.YAWRATE].ToString("000.000");
btParmA.Text = ("-");
break;
}
// Status
int status = (int)lru0.dataParam[(int)PARAM.STATUS];
// Pitot Stable
if ((status & 0x01) == 0x01)
{
btPitotHeatStable.BackColor = Color.ForestGreen;
}
else
{
btPitotHeatStable.BackColor = Color.DarkGray;
}
// Static Stable
if ((status & 0x02) == 0x02)
{
btStaticHeatStable.BackColor = Color.ForestGreen;
}
else
{
btStaticHeatStable.BackColor = Color.DarkGray;
}
// AHRS Stable
if ((status & 0x04) == 0x04)
{
btAhrsStable.BackColor = Color.ForestGreen;
}
else
{
btAhrsStable.BackColor = Color.DarkGray;
}
// AHRS Align Complete
if ((status & 0x08) == 0x08)
{
btAhrsAlignComplete.BackColor = Color.ForestGreen;
}
else
{
btAhrsAlignComplete.BackColor = Color.DarkGray;
}
//// Source Display
//if (dataSource == (int)SOURCE.GYRO_RAW)
// lbSource.Text = "G RAW";
//if (dataSource == (int)SOURCE.RPY)
// lbSource.Text = "RPY";
// Chart min max labels
lbMaxChartVal.Text = Chart.MAX_RATE.ToString();
lbMinChartVal.Text = Chart.MIN_RATE.ToString();
lbTrgTemp.Text = Chart.TRG_RATE.ToString();
lbTrgTemp.Top = (int)Chart.rateToYLoc(Chart.TRG_RATE) + 20;
this.Invalidate();
if (freeGyroMode == true)
{
timeInFreeMode += 0.10;
lbTimeInFreeMode.Text = timeInFreeMode.ToString("000.0") + " Sec";
}
// Positioning Fixture Random Motion
if (randomEnabled)
{
if (!PosFixture.getInMotion())
{
Random r = new Random();
int r1 = r.Next(-tbRandomScaleY.Value, +tbRandomScaleY.Value);
int r2 = r.Next(-tbRandomScaleR.Value, +tbRandomScaleR.Value);
int r3 = r.Next(-tbRandomScaleP.Value, +tbRandomScaleP.Value);
if (cbRandomXEnabled.Checked != true)
{
r1 = 0;
}
if (cbRandomYEnabled.Checked != true)
{
r2 = 0;
}
if (cbRandomZEnabled.Checked != true)
{
r3 = 0;
}
PosFixture.setPosition("X" + r1.ToString() + " Y" + r2.ToString() + " Z" + r3.ToString());
}
}
else
{
//PosFixture.setPosition("X0 Y0 Z0");
}
// DG Motion Test
if (dgMotionEnabled)
{
if (!PosFixture.getInMotion())
{
//if (!dgSlowInit)
//{
// // Slow
// PosFixture.sendCommand("$xvm=35"); // xMax rate (mm/min)
// PosFixture.sendCommand("$yvm=35"); // yMax rate (mm/min)
// PosFixture.sendCommand("$zvm=35"); // zMax rate (mm/min)
// PosFixture.sendCommand("$xjm=.1"); // xMax jerk (mm/min)
// PosFixture.sendCommand("$yjm=.1"); // yMax jerk (mm/min)
// PosFixture.sendCommand("$zjm=.1"); // zMax jerk (mm/min)
// System.Threading.Thread.Sleep(2000);
// dgSlowInit = true;
//}
// Toggle between left and right
if (dgCycleCount < 6)
{
// Cycle 1
if (dgRightSide)
{
PosFixture.setPosition("X7.5 Y7.5 Z7.5");
dgRightSide = false;
}
else
{
PosFixture.setPosition("X-7.5 Y-7.5 Z-7.5");
dgRightSide = true;
//dgCycleCount++;
}
}
else
{
// Cycle 2
if (dgRightSide)
{
PosFixture.setPosition("X7.5 Y-7.5 Z-7.5");
dgRightSide = false;
}
else
{
PosFixture.setPosition("X-7.5 Y7.5 Z-7.5");
dgRightSide = true;
dgCycleCount++;
}
}
if (dgCycleCount > 12)
{
dgCycleCount = 0;
}
System.Threading.Thread.Sleep(100);
}
}
PosFixture.update();
//// Trend
//lbOmegaITrend0.Text = lru0.dataParam[(int)PARAM.OIT0].ToString("0000.00");
//lbOmegaITrend1.Text = lru0.dataParam[(int)PARAM.OIT1].ToString("0000.00");
//lbOmegaITrend2.Text = lru0.dataParam[(int)PARAM.OIT2].ToString("0000.00");
}