public static void setScaleRunningSate(bool run)
{
if (run)
{
// Move to scale cal
currentState = 0;
biasPointsRunning = false;
scalePointsRunning = true;
readPointsRunning = false;
prgPointsRunning = false;
passCount = 0;
// Button color
uiForm.btStartGyroCal.BackColor = System.Drawing.Color.Green;
}
else
{
// State
biasPointsRunning = false;
scalePointsRunning = false;
readPointsRunning = false;
prgPointsRunning = false;
// Button color
uiForm.btStartGyroCal.BackColor = System.Drawing.Color.DimGray;
// Move home
PosFixture.setPosition("X0 Y0 Z0");
}
}
// Start / Stop
public static void setAhrsAtpRunningSate(bool run)
{
if (run)
{
atpPointsRunning = true;
uiForm.btStartAhrsAtp.BackColor = System.Drawing.Color.Green;
}
else
{
atpPointsRunning = false;
//uiForm.btStartAhrsAtp.BackColor = System.Drawing.Color.DimGray;
PosFixture.setPosition("X0 Y0 Z0");
}
}
// Start / Stop
public static void setBiasRunningSate(bool run)
{
if (run)
{
// Start with gyro cal
biasPointsRunning = true;
scalePointsRunning = false;
//biasPointsRunning = false;
//scalePointsRunning = true;
readPointsRunning = false;
prgPointsRunning = false;
// Initial values
currentState = 0;
calData[(int)CAL_POINT.BIASX] = 0;
calData[(int)CAL_POINT.BIASY] = 0;
calData[(int)CAL_POINT.BIASZ] = 0;
calData[(int)CAL_POINT.XONX] = 1;
calData[(int)CAL_POINT.XONY] = 0;
calData[(int)CAL_POINT.XONZ] = 0;
calData[(int)CAL_POINT.YONX] = 0;
calData[(int)CAL_POINT.YONY] = 1;
calData[(int)CAL_POINT.YONZ] = 0;
calData[(int)CAL_POINT.ZONX] = 0;
calData[(int)CAL_POINT.ZONY] = 0;
calData[(int)CAL_POINT.ZONZ] = 1;
// Button color
uiForm.btStartGyroCal.BackColor = System.Drawing.Color.Green;
}
else
{
// State
biasPointsRunning = false;
scalePointsRunning = false;
readPointsRunning = false;
prgPointsRunning = false;
// Button color
uiForm.btStartGyroCal.BackColor = System.Drawing.Color.DimGray;
// Move home
PosFixture.setPosition("X0 Y0 Z0");
}
}
// Start / Stop
public static void setBiasRunningSate(bool run)
{
if (run)
{
// Start with bias cal
biasPointsRunning = true;
scalePointsRunning = false;
readPointsRunning = false;
prgPointsRunning = false;
uiForm.btLru0MagParmName.Text = "Bias Values";
uiForm.btLru0MagParmValue.Text = "Set Position";
uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
uiForm.lru0.setParameterReq((int)PARAM.MAGX, RATE.RATE_50HZ);
uiForm.lru0.setParameterReq((int)PARAM.MAGY, RATE.RATE_50HZ);
uiForm.lru0.setParameterReq((int)PARAM.MAGZ, RATE.RATE_50HZ);
// Initial values
currentState = 0;
loopCount = 0;
uiForm.btLru0MagXMin.Text = "9999.0";
uiForm.btLru0MagYMin.Text = "9999.0";
uiForm.btLru0MagZMin.Text = "9999.0";
uiForm.btLru0MagXMax.Text = "-9999.0";
uiForm.btLru0MagYMax.Text = "-9999.0";
uiForm.btLru0MagZMax.Text = "-9999.0";
// Button color
uiForm.btStartMagCal.BackColor = System.Drawing.Color.Green;
}
else
{
// State
biasPointsRunning = false;
scalePointsRunning = false;
readPointsRunning = false;
prgPointsRunning = false;
// Button color
uiForm.btStartMagCal.BackColor = System.Drawing.Color.DimGray;
// Move home
PosFixture.setPosition("X0 Y0 Z0");
}
}
// State machine update
public static void update()
{
string msg = "";
// Handle state
if (atpPointsRunning)
{
msg = atpStateMachine();
}
// Handle next step
if (msg == "next")
{
currentState++;
}
else if (msg == "back")
{
currentState--;
}
else if (msg == "skip")
{
currentState += 2;
}
else if (msg == "wait")
{
}
else if (msg == "end")
{
// Turn off outputs
uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
atpPointsRunning = false;
// Back to zero
PosFixture.setPosition("X0 Y0 Z0");
//// Data
//uiForm.btAhrsAtpParmName.Text = "Complete";
//uiForm.btLru0AhrsAtpParmValue.Text = "";
// Button color
//uiForm.btStartAhrsAtp.BackColor = System.Drawing.Color.DimGray;
}
}
// Start / Stop
public static void setCalRunningSate(bool run)
{
if (run)
{
// Start with accel cal
currentState = 0;
calPointsRunning = true;
readPointsRunning = false;
prgPointsRunning = false;
// Initial values
calData[(int)CAL_POINT.AXZ] = 0;
calData[(int)CAL_POINT.AXP] = 1;
calData[(int)CAL_POINT.AXN] = -1;
calData[(int)CAL_POINT.AYZ] = 0;
calData[(int)CAL_POINT.AYP] = 1;
calData[(int)CAL_POINT.AYN] = -1;
calData[(int)CAL_POINT.AZZ] = 0;
calData[(int)CAL_POINT.AZP] = 1;
calData[(int)CAL_POINT.AZN] = -1;
// Button color
uiForm.btStartAccelCal.BackColor = System.Drawing.Color.Green;
}
else
{
// State
calPointsRunning = false;
readPointsRunning = false;
prgPointsRunning = false;
// Button color
//uiForm.btStartAccelCal.BackColor = System.Drawing.Color.DimGray;
// Move home
PosFixture.setPosition("X0 Y0 Z0");
}
}
// Start / Stop
public static void setCalRunningSate(bool run)
{
if (run)
{
// Start with accel cal
currentState = 0;
calAccelRunning = true;
calGyroRunning = false;
// Initial values
axz = 0;
axp = 1;
axn = -1;
ayz = 0;
ayp = 1;
ayn = -1;
azz = 0;
azp = 1;
azn = -1;
// Button color
uiForm.btStartAccelCal.BackColor = System.Drawing.Color.Green;
}
else
{
// Shutdown state
calAccelRunning = false;
calGyroRunning = false;
// Button color
uiForm.btStartAccelCal.BackColor = System.Drawing.Color.DimGray;
// Move home
PosFixture.setPosition("X0 Y0 Z0");
}
}
// State machine update
public static void update()
{
string msg = "";
// Show data based on state
if ((currentState >= (int)CAL_STATE.SET_ACCELX0) && (currentState <= (int)CAL_STATE.GET_ACCELXN1))
{
//uiForm.btLru0AccelParmValue.Text = uiForm.lru0.dataParam[(int)PARAM.ACCELXR].ToString("00.00000");
}
if ((currentState >= (int)CAL_STATE.SET_ACCELY0) && (currentState <= (int)CAL_STATE.GET_ACCELYN1))
{
uiForm.btLru0AccelParmValue.Text = uiForm.lru0.dataParam[(int)PARAM.ACCELYR0].ToString("00.00000");
}
if ((currentState >= (int)CAL_STATE.SET_ACCELZ0) && (currentState <= (int)CAL_STATE.GET_ACCELZN1))
{
uiForm.btLru0AccelParmValue.Text = uiForm.lru0.dataParam[(int)PARAM.ACCELZR0].ToString("00.00000");
}
// Handle state
if (calPointsRunning)
{
// Accel cal points
msg = accelStateMachine();
}
else if (prgPointsRunning)
{
// Program points
msg = prgPointsStateMachine();
}
else if (readPointsRunning)
{
// Read points
msg = readPointsStateMachine();
}
// Handle next step
if (msg == "next")
{
currentState++;
}
else if (msg == "back")
{
currentState--;
}
else if (msg == "skip")
{
currentState += 2;
}
else if (msg == "wait")
{
}
else if (msg == "end")
{
// Turn off outputs
uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
calPointsRunning = false;
prgPointsRunning = false;
readPointsRunning = false;
// Back to zero
PosFixture.setPosition("X0 Y0 Z0");
// Data
uiForm.btLru0AccelParmName.Text = "Complete";
uiForm.btLru0AccelParmValue.Text = "";
// Button color
//uiForm.btStartAccelCal.BackColor = System.Drawing.Color.DimGray;
}
}
private static string atpStateMachine()
{
switch (currentState)
{
case (int)ATP_STATE.AHRS_RES:
{
PosFixture.setPosition("X0 Y0 Z0");
//uiForm.btAhrsAtpParmName.Text = "AHRS Reset";
//uiForm.btLru0AhrsAtpParmValue.Text = "AHRS Reset";
// Put into slaving mode
uiForm.lru0.setEngWriteF(ENG.FRGM, 0.0);
// Reset AHRS
uiForm.lru0.setEngWriteF(ENG.ARES, 1.0);
uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
uiForm.lru0.setParameterReq((int)PARAM.STATUS, RATE.RATE_1HZ);
uiForm.lru0.setParameterReq((int)PARAM.RPYWEIGHT, RATE.RATE_50HZ);
uiForm.lru0.setParameterReq((int)PARAM.BIASWEIGHT, RATE.RATE_50HZ);
loopCount = 0;
stableCount = 0;
Thread.Sleep(1000);
return("next");
}
case (int)ATP_STATE.AHRS_WAIT:
{
double weight = uiForm.lru0.dataParam[(int)PARAM.RPYWEIGHT];
//uiForm.btLru0AhrsAtpParmValue.Text = weight.ToString("00");
if (((int)uiForm.lru0.dataParam[(int)PARAM.STATUS] & STATUS.AHRS_STABLE) != 0)
{
if (stableCount < 100)
{
stableCount++;
return("wait");
}
else
{
return("next");
}
}
else
{
return("wait");
}
}
case (int)ATP_STATE.AHRS_PREP:
{
//uiForm.btAhrsAtpParmName.Text = "Scale Test Points";
//uiForm.btLru0AhrsAtpParmValue.Text = "Set Position";
uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
uiForm.lru0.setParameterReq((int)PARAM.STATUS, RATE.RATE_1HZ);
uiForm.lru0.setParameterReq((int)PARAM.ROLL, RATE.RATE_50HZ);
uiForm.lru0.setParameterReq((int)PARAM.PITCH, RATE.RATE_50HZ);
uiForm.lru0.setParameterReq((int)PARAM.YAW, RATE.RATE_50HZ);
loopCount = 0;
// Wait for serial data to start and get parsed
Thread.Sleep(2000);
return("next");
}
case (int)ATP_STATE.SET_POSITION:
{
if (loopCount < atpMovePoints.Length)
{
// Put into free gyro mode
uiForm.lru0.setEngWriteF(ENG.FRGM, 1.0);
// Get initial values
iRoll = uiForm.lru0.dataParam[(int)PARAM.ROLL];
iPitch = uiForm.lru0.dataParam[(int)PARAM.PITCH];
iYaw = uiForm.lru0.dataParam[(int)PARAM.YAW];
PosFixture.setPosition(atpMovePoints[loopCount]);
//uiForm.btAhrsAtpParmName.Text = atpMovePointsDesc[loopCount];
sampleCount = 0;
return("next");
}
else
{
currentState = (int)ATP_STATE.END_ATP;
return("none");
}
}
case (int)ATP_STATE.WAIT_POSITION:
{
//uiForm.btLru0AhrsAtpParmValue.Text = "R:" + uiForm.lru0.dataParam[(int)PARAM.ROLL].ToString("00.00") +
// " P:" + uiForm.lru0.dataParam[(int)PARAM.PITCH].ToString("00.00") +
// " Y:" + uiForm.lru0.dataParam[(int)PARAM.YAW].ToString("00.00");
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
// Wait for full motion to stop and get readings
if (sampleCount++ > 50)
{
return("next");
}
else
{
return("wait");
}
}
}
case (int)ATP_STATE.GET_POSITION:
{
roll = uiForm.lru0.dataParam[(int)PARAM.ROLL];
pitch = uiForm.lru0.dataParam[(int)PARAM.PITCH];
yaw = uiForm.lru0.dataParam[(int)PARAM.YAW];
double rollError = 10, pitchError = 10, yawError = 10, testValue = 0;
switch (loopCount)
{
// Roll +
case 0:
testValue = roll;
rollError = roll - iRoll - atpMoveValue[loopCount];
pitchError = pitch - iPitch;
yawError = yaw - iYaw;
if (yawError > 180)
{
yawError -= 360;
}
if (yawError < -180)
{
yawError += 360;
}
break;
// Roll -
case 1:
testValue = roll;
rollError = roll - iRoll - atpMoveValue[loopCount];
pitchError = pitch - iPitch;
yawError = yaw - iYaw;
if (yawError > 180)
{
yawError -= 360;
}
if (yawError < -180)
{
yawError += 360;
}
break;
// Pitch +
case 2:
testValue = pitch;
rollError = roll - iRoll;
pitchError = pitch - iPitch - atpMoveValue[loopCount];
yawError = yaw - iYaw;
if (yawError > 180)
{
yawError -= 360;
}
if (yawError < -180)
{
yawError += 360;
}
break;
// Pitch -
case 3:
testValue = pitch;
rollError = roll - iRoll;
pitchError = pitch - iPitch - atpMoveValue[loopCount];
yawError = yaw - iYaw;
if (yawError > 180)
{
yawError -= 360;
}
if (yawError < -180)
{
yawError += 360;
}
break;
// Yaw +
case 4:
testValue = yaw;
rollError = roll - iRoll;
pitchError = pitch - iPitch;
yawError = yaw - iYaw - atpMoveValue[loopCount];
if (yawError > 180)
{
yawError -= 360;
}
if (yawError < -180)
{
yawError += 360;
}
break;
// Yaw -
case 5:
testValue = yaw;
rollError = roll - iRoll;
pitchError = pitch - iPitch;
yawError = yaw - iYaw - atpMoveValue[loopCount];
if (yawError > 180)
{
yawError -= 360;
}
if (yawError < -180)
{
yawError += 360;
}
break;
}
//switch (loopCount)
//{
// case 0: // + Roll
// atpDataPoint[loopCount] = uiForm.lru0.dataParam[(int)PARAM.ROLL];
// break;
// case 1: // - Roll
// atpDataPoint[loopCount] = uiForm.lru0.dataParam[(int)PARAM.ROLL];
// break;
// case 2: // + Pitch
// atpDataPoint[loopCount] = uiForm.lru0.dataParam[(int)PARAM.PITCH];
// break;
// case 3: // - Pitch
// atpDataPoint[loopCount] = uiForm.lru0.dataParam[(int)PARAM.PITCH];
// break;
// case 4: // + Yaw
// atpDataPoint[loopCount] = uiForm.lru0.dataParam[(int)PARAM.YAW];
// break;
// case 5: // - Yaw
// atpDataPoint[loopCount] = uiForm.lru0.dataParam[(int)PARAM.YAW];
// break;
//}
string passFail;
if ((Math.Abs(rollError) > TEST_TOL) || (Math.Abs(pitchError) > TEST_TOL) || (Math.Abs(yawError) > TEST_TOL))
{
passFail = "FAIL";
}
else
{
passFail = "PASS";
}
addTableEntry(atpMovePointsDesc[loopCount], testValue.ToString("00.00"), rollError.ToString("0.00"), pitchError.ToString("0.00"), yawError.ToString("0.00"), passFail);
loopCount++;
return("next");
}
case (int)ATP_STATE.SET_ZERO:
{
// Put into slaving mode mode
uiForm.lru0.setEngWriteF(ENG.FRGM, 0.0);
PosFixture.setPosition("X0 Y0 Z0");
sampleCount = 0;
return("next");
}
case (int)ATP_STATE.WAIT_ZERO:
{
//uiForm.btLru0AhrsAtpParmValue.Text = "R:" + uiForm.lru0.dataParam[(int)PARAM.ROLL].ToString("00.00") +
// " P:" + uiForm.lru0.dataParam[(int)PARAM.PITCH].ToString("00.00") +
// " Y:" + uiForm.lru0.dataParam[(int)PARAM.YAW].ToString("00.00");
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
// Hold position to re-zero - let some align occur
if (sampleCount++ > 200)
{
// Move to next test
currentState = (int)ATP_STATE.SET_POSITION;
return("none");
}
else
{
return("wait");
}
}
}
case (int)ATP_STATE.END_ATP:
{
PosFixture.setPosition("X0 Y0 Z0");
//// Check for chain
//if (uiForm.cbChainGyrolCalToPrg.Checked)
//{
// // Start Program cycle
// setPrgRunningState(true);
// return ("wait");
//}
return("end");
}
default:
{
return("end");
}
}
}
private static string biasStateMachine()
{
int loop = 0;
const int COM_DELAY = 200; // Delay for communication
switch (currentState)
{
//
// Bias XYZ
//
case (int)BIAS_STATE.SET_BIAS:
{
uiForm.btLru0GyroParmName.Text = "Bias Values";
uiForm.btLru0GyroParmValue.Text = "Set Position";
uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
uiForm.lru0.setParameterReq((int)PARAM.GYROXR0, RATE.RATE_50HZ);
uiForm.lru0.setParameterReq((int)PARAM.GYROYR0, RATE.RATE_50HZ);
uiForm.lru0.setParameterReq((int)PARAM.GYROZR0, RATE.RATE_50HZ);
uiForm.lru0.setParameterReq((int)PARAM.GYROXR1, RATE.RATE_50HZ);
uiForm.lru0.setParameterReq((int)PARAM.GYROYR1, RATE.RATE_50HZ);
uiForm.lru0.setParameterReq((int)PARAM.GYROZR1, RATE.RATE_50HZ);
PosFixture.setPosition("X0 Y0 Z0");
// Clear bias values in unit to get raw readings without bias applied
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 0, 0);
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 1, 0);
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 2, 0);
// Clear cross coupling only if requested
if (!uiForm.cbSetUnityGain.Checked)
{
// Clear cross matrix if full calibration
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 3, 1.0); // xonx
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 4, 0);
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 5, 0);
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 6, 0);
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 7, 1.0); // yony
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 8, 0);
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 9, 0);
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 10, 0);
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 11, 1.0); // zonz
}
uiForm.lru0.setCalWriteF((int)PARAM.FLUSH, 0);
return("next");
}
case (int)BIAS_STATE.WAIT_BIAS:
{
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
sampleCount = 0;
System.Threading.Thread.Sleep(1000);
return("next");
}
}
case (int)BIAS_STATE.GET_BIASX:
{
uiForm.btLru0GyroParmName.Text = "X Bias Value";
if (sampleCount < SAMPLE_COUNT)
{
dataSample0[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.GYROXR0];
dataSample1[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.GYROXR1];
uiForm.btLru0GyroParmValue.Text = dataSample0[sampleCount].ToString("00.00000");
sampleCount++;
return("wait");
}
else
{
calData[(int)CAL_POINT.BIASX] = (dataSample0.Average() + dataSample1.Average()) / 2.0;
addTableEntry(uiForm.btLru0GyroParmName.Text, calData[(int)CAL_POINT.BIASX], "MEASURE");
sampleCount = 0;
return("next");
}
}
case (int)BIAS_STATE.GET_BIASY:
{
uiForm.btLru0GyroParmName.Text = "Y Bias Value";
if (sampleCount < SAMPLE_COUNT)
{
dataSample0[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.GYROYR0];
dataSample1[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.GYROYR1];
uiForm.btLru0GyroParmValue.Text = dataSample0[sampleCount].ToString("00.00000");
sampleCount++;
return("wait");
}
else
{
calData[(int)CAL_POINT.BIASY] = (dataSample0.Average() + dataSample1.Average()) / 2.0;
addTableEntry(uiForm.btLru0GyroParmName.Text, calData[(int)CAL_POINT.BIASY], "MEASURE");
sampleCount = 0;
return("next");
}
}
case (int)BIAS_STATE.GET_BIASZ:
{
uiForm.btLru0GyroParmName.Text = "Z Bias Value";
if (sampleCount < SAMPLE_COUNT)
{
dataSample0[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.GYROZR0];
dataSample1[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.GYROZR1];
uiForm.btLru0GyroParmValue.Text = dataSample0[sampleCount].ToString("00.00000");
sampleCount++;
return("wait");
}
else
{
calData[(int)CAL_POINT.BIASZ] = (dataSample0.Average() + dataSample1.Average()) / 2.0;
addTableEntry(uiForm.btLru0GyroParmName.Text, calData[(int)CAL_POINT.BIASZ], "MEASURE");
return("next");
}
}
case (int)BIAS_STATE.PROG_BIAS:
{
uiForm.btLru0GyroParmName.Text = "Program Bias XYZ";
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 0, calData[(int)CAL_POINT.BIASX]);
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 1, calData[(int)CAL_POINT.BIASY]);
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 2, calData[(int)CAL_POINT.BIASZ]);
uiForm.lru0.setCalWriteF((int)PARAM.FLUSH, 0);
// Start Scale if not bias only
if (!uiForm.cbBiasCalOnly.Checked)
{
setScaleRunningSate(true);
}
biasPointsRunning = false;
System.Threading.Thread.Sleep(500);
return("none");
}
default:
{
return("end");
}
}
}
private static string scaleStateMachine()
{
switch (currentState)
{
case (int)SCALE_STATE.AHRS_RES:
{
PosFixture.setPosition("X0 Y0 Z0");
uiForm.btLru0GyroParmName.Text = "AHRS Reset";
uiForm.btLru0GyroParmValue.Text = "AHRS Reset";
uiForm.btGyroCalPass.Text = passCount.ToString("0");
// Put into slaving mode
uiForm.lru0.setEngWriteF(ENG.FRGM, 0.0);
// Reset AHRS
uiForm.lru0.setEngWriteF(ENG.ARES, 1.0);
uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
uiForm.lru0.setParameterReq((int)PARAM.STATUS, RATE.RATE_1HZ);
uiForm.lru0.setParameterReq((int)PARAM.RPYWEIGHT, RATE.RATE_50HZ);
uiForm.lru0.setParameterReq((int)PARAM.BIASWEIGHT, RATE.RATE_50HZ);
loopCount = 0;
Thread.Sleep(1000);
return("next");
}
case (int)SCALE_STATE.AHRS_WAIT:
{
double weight = uiForm.lru0.dataParam[(int)PARAM.RPYWEIGHT];
uiForm.btLru0GyroParmValue.Text = weight.ToString("00");
if (((int)uiForm.lru0.dataParam[(int)PARAM.STATUS] & STATUS.AHRS_STABLE) != 0)
{
return("next");
}
else
{
return("wait");
}
}
case (int)SCALE_STATE.PREP_SCALE:
{
uiForm.btLru0GyroParmName.Text = "Scale Test Points";
uiForm.btLru0GyroParmValue.Text = "Set Position";
uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
uiForm.lru0.setParameterReq((int)PARAM.STATUS, RATE.RATE_1HZ);
uiForm.lru0.setParameterReq((int)PARAM.ROLL, RATE.RATE_50HZ);
uiForm.lru0.setParameterReq((int)PARAM.PITCH, RATE.RATE_50HZ);
uiForm.lru0.setParameterReq((int)PARAM.YAW, RATE.RATE_50HZ);
loopCount = 0;
// Put into free gyro mode
uiForm.lru0.setEngWriteF(ENG.FRGM, 1.0);
return("next");
}
case (int)SCALE_STATE.SET_SCALE:
{
if (loopCount < gyroMovePoints.Length)
{
PosFixture.setPosition(gyroMovePoints[loopCount]);
uiForm.btLru0GyroParmName.Text = gyroMovePoints[loopCount];
sampleCount = 0;
return("next");
}
else
{
currentState = (int)SCALE_STATE.END_SCALE;
return("none");
}
}
case (int)SCALE_STATE.WAIT_SCALE:
{
uiForm.btLru0GyroParmValue.Text = "R:" + uiForm.lru0.dataParam[(int)PARAM.ROLL].ToString("00.0") +
" P:" + uiForm.lru0.dataParam[(int)PARAM.PITCH].ToString("00.0") +
" Y:" + uiForm.lru0.dataParam[(int)PARAM.YAW].ToString("00.0");
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
// Wait for full motion to stop and get readings
if (sampleCount++ > 5)
{
return("next");
}
else
{
return("wait");
}
}
}
case (int)SCALE_STATE.GET_SCALE:
{
switch (loopCount)
{
// X Effect (Roll)
case 0:
xonx[passCount] = uiForm.lru0.dataParam[(int)PARAM.ROLL];
xony[passCount] = uiForm.lru0.dataParam[(int)PARAM.PITCH];
xonz[passCount] = uiForm.lru0.dataParam[(int)PARAM.YAW];
break;
case 1:
xonx[passCount] = (1.0 / ((-xonx[passCount] + uiForm.lru0.dataParam[(int)PARAM.ROLL]) / (ANGLE * 2)));
xony[passCount] = ((-xony[passCount] + uiForm.lru0.dataParam[(int)PARAM.PITCH]) / (ANGLE * 2));
xonz[passCount] = ((-xonz[passCount] + uiForm.lru0.dataParam[(int)PARAM.YAW]) / (ANGLE * 2));
addTableEntry("X on X", xonx[passCount], "MEASURE");
addTableEntry("X on Y", xony[passCount], "MEASURE");
addTableEntry("X on Z", xonz[passCount], "MEASURE");
PosFixture.setPosition("X0 Y0 Z0");
break;
// Y Effect (Pitch)
case 2:
// Capture roll left
yonx[passCount] = uiForm.lru0.dataParam[(int)PARAM.ROLL];
yony[passCount] = uiForm.lru0.dataParam[(int)PARAM.PITCH];
yonz[passCount] = uiForm.lru0.dataParam[(int)PARAM.YAW];
break;
case 3:
// Capture roll right and get overall scale
yonx[passCount] = ((-yonx[passCount] + uiForm.lru0.dataParam[(int)PARAM.ROLL]) / (ANGLE * 2));
yony[passCount] = (1.0 / ((-yony[passCount] + uiForm.lru0.dataParam[(int)PARAM.PITCH]) / (ANGLE * 2)));
yonz[passCount] = ((-yonz[passCount] + uiForm.lru0.dataParam[(int)PARAM.YAW]) / (ANGLE * 2));
addTableEntry("Y on X", yonx[passCount], "MEASURE");
addTableEntry("Y on Y", yony[passCount], "MEASURE");
addTableEntry("Y on Z", yonz[passCount], "MEASURE");
PosFixture.setPosition("X0 Y0 Z0");
break;
// Z Effect (Yaw)
case 4:
// Capture roll left
zonx[passCount] = uiForm.lru0.dataParam[(int)PARAM.ROLL];
zony[passCount] = uiForm.lru0.dataParam[(int)PARAM.PITCH];
zonz[passCount] = uiForm.lru0.dataParam[(int)PARAM.YAW];
break;
case 5:
// Capture roll right and get overall scale
zonx[passCount] = ((-zonx[passCount] + uiForm.lru0.dataParam[(int)PARAM.ROLL]) / (ANGLE * 2));
zony[passCount] = ((-zony[passCount] + uiForm.lru0.dataParam[(int)PARAM.PITCH]) / (ANGLE * 2));
zonz[passCount] = (1.0 / ((-zonz[passCount] + uiForm.lru0.dataParam[(int)PARAM.YAW]) / (ANGLE * 2)));
addTableEntry("Z on X", zonx[passCount], "MEASURE");
addTableEntry("Z on Y", zony[passCount], "MEASURE");
addTableEntry("Z on Z", zonz[passCount], "MEASURE");
PosFixture.setPosition("X0 Y0 Z0");
break;
}
loopCount++;
currentState = (int)SCALE_STATE.SET_SCALE;
return("none");
}
case (int)SCALE_STATE.END_SCALE:
{
// Put back into slave mode
uiForm.lru0.setEngWriteF(ENG.FRGM, 0.0);
PosFixture.setPosition("X0 Y0 Z0");
// Check for pass count
if (++passCount < SCALE_PASSES)
{
// Loop back
Thread.Sleep(1000);
currentState = (int)SCALE_STATE.AHRS_RES;
return("none");
}
// Check for chain
if (uiForm.cbChainGyrolCalToPrg.Checked)
{
// Start Program cycle
setPrgRunningState(true);
return("wait");
}
return("end");
}
default:
{
return("end");
}
}
}
private static string biasStateMachine()
{
int loop = 0;
switch (currentState)
{
//
// Bias XYZ
//
case (int)BIAS_STATE.SET_BIAS:
{
uiForm.btLru0GyroParmName.Text = "Bias Values";
uiForm.btLru0GyroParmValue.Text = "Set Position";
uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
uiForm.lru0.setParameterReq((int)PARAM.GYROXR, RATE.RATE_50HZ);
uiForm.lru0.setParameterReq((int)PARAM.GYROYR, RATE.RATE_50HZ);
uiForm.lru0.setParameterReq((int)PARAM.GYROZR, RATE.RATE_50HZ);
PosFixture.setPosition("X0 Y0 Z0");
// Clear bias values in unit to get raw readings without bias applied
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 0, 0);
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 1, 0);
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 2, 0);
uiForm.lru0.setCalWriteF((int)PARAM.FLUSH, 0);
return("next");
}
case (int)BIAS_STATE.WAIT_BIAS:
{
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
sampleCount = 0;
return("next");
}
}
case (int)BIAS_STATE.GET_BIASX:
{
uiForm.btLru0GyroParmName.Text = "X Bias Value";
if (sampleCount < SAMPLE_COUNT)
{
dataSample[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.GYROXR];
uiForm.btLru0GyroParmValue.Text = dataSample[sampleCount].ToString("00.00000");
sampleCount++;
return("wait");
}
else
{
calData[(int)CAL_POINT.BIASX] = dataSample.Average();
addTableEntry(uiForm.btLru0GyroParmName.Text, dataSample.Average(), "MEASURE");
sampleCount = 0;
return("next");
}
}
case (int)BIAS_STATE.GET_BIASY:
{
uiForm.btLru0GyroParmName.Text = "Y Bias Value";
if (sampleCount < SAMPLE_COUNT)
{
dataSample[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.GYROYR];
uiForm.btLru0GyroParmValue.Text = dataSample[sampleCount].ToString("00.00000");
sampleCount++;
return("wait");
}
else
{
calData[(int)CAL_POINT.BIASY] = dataSample.Average();
addTableEntry(uiForm.btLru0GyroParmName.Text, dataSample.Average(), "MEASURE");
sampleCount = 0;
return("next");
}
}
case (int)BIAS_STATE.GET_BIASZ:
{
uiForm.btLru0GyroParmName.Text = "Z Bias Value";
if (sampleCount < SAMPLE_COUNT)
{
dataSample[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.GYROZR];
uiForm.btLru0GyroParmValue.Text = dataSample[sampleCount].ToString("00.00000");
sampleCount++;
return("wait");
}
else
{
calData[(int)CAL_POINT.BIASZ] = dataSample.Average();
addTableEntry(uiForm.btLru0GyroParmName.Text, dataSample.Average(), "MEASURE");
return("next");
}
}
case (int)BIAS_STATE.PROG_BIAS:
{
uiForm.btLru0GyroParmName.Text = "Program Bias XYZ";
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 0, calData[(int)CAL_POINT.BIASX]);
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 1, calData[(int)CAL_POINT.BIASY]);
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 2, calData[(int)CAL_POINT.BIASZ]);
// Clear cross matrix
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 3, 1.0); // xonx
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 4, 0);
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 5, 0);
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 6, 0);
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 7, 1.0); // yony
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 8, 0);
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 9, 0);
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 10, 0);
uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 11, 1.0); // zonz
uiForm.lru0.setCalWriteF((int)PARAM.FLUSH, 0);
// Start Scale
setScaleRunningSate(true);
return("none");
}
default:
{
return("end");
}
}
}
// State machine update
public static void update()
{
string msg = "";
//// Show data based on state
//if ((currentState >= (int)CAL_BIAS.SET_ACCELX0) && (currentState <= (int)BIAS_STATE.GET_ACCELXN1) )
//{
// uiForm.btLru0ParmValue.Text = uiForm.lru0.dataParam[(int)PARAM.ACCELXR].ToString("00.00000");
//}
//if ((currentState >= (int)BIAS_STATE.SET_ACCELY0) && (currentState <= (int)BIAS_STATE.GET_ACCELYN1))
//{
// uiForm.btLru0ParmValue.Text = uiForm.lru0.dataParam[(int)PARAM.ACCELYR].ToString("00.00000");
//}
//if ((currentState >= (int)BIAS_STATE.SET_ACCELZ0) && (currentState <= (int)BIAS_STATE.GET_ACCELZN1))
//{
// uiForm.btLru0ParmValue.Text = uiForm.lru0.dataParam[(int)PARAM.ACCELZR].ToString("00.00000");
//}
// Handle state
if (biasPointsRunning)
{
msg = biasStateMachine();
}
else if (scalePointsRunning)
{
msg = scaleStateMachine();
}
else if (prgPointsRunning)
{
msg = prgPointsStateMachine();
}
else if (readPointsRunning)
{
msg = readPointsStateMachine();
}
// Handle next step
if (msg == "next")
{
currentState++;
}
else if (msg == "back")
{
currentState--;
}
else if (msg == "skip")
{
currentState += 2;
}
else if (msg == "wait")
{
}
else if (msg == "end")
{
// Turn off outputs
uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
biasPointsRunning = false;
scalePointsRunning = false;
prgPointsRunning = false;
readPointsRunning = false;
// Back to zero
PosFixture.setPosition("X0 Y0 Z0");
// Data
uiForm.btLru0GyroParmName.Text = "Complete";
uiForm.btLru0GyroParmValue.Text = "";
// Button color
uiForm.btStartGyroCal.BackColor = System.Drawing.Color.DimGray;
}
}
private static string scaleStateMachine()
{
switch (currentState)
{
case (int)SCALE_STATE.SETUP:
{
uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
uiForm.lru0.setParameterReq((int)PARAM.STATUS, RATE.RATE_1HZ);
uiForm.lru0.setParameterReq((int)PARAM.GYROXSUM, RATE.RATE_25HZ);
uiForm.lru0.setParameterReq((int)PARAM.GYROYSUM, RATE.RATE_25HZ);
uiForm.lru0.setParameterReq((int)PARAM.GYROZSUM, RATE.RATE_25HZ);
return("next");
}
case (int)SCALE_STATE.MOVE_START:
{
if (loopCount < gyroMovePoints.Length)
{
PosFixture.setPosition(gyroMovePoints[loopCount]);
uiForm.btLru0GyroParmName.Text = gyroMovePoints[loopCount];
System.Threading.Thread.Sleep(1000);
return("next");
}
else
{
// Exit scale calibration
currentState = (int)SCALE_STATE.END_SCALE;
return("none");
}
}
case (int)SCALE_STATE.WAIT_START:
{
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
// Clear gyro sums
uiForm.lru0.setEngWriteF(ENG.SCLR, 1.0);
System.Threading.Thread.Sleep(2000);
loopCount++; // Next point
return("next");
}
}
case (int)SCALE_STATE.MOVE_END:
{
if (loopCount < gyroMovePoints.Length)
{
PosFixture.setPosition(gyroMovePoints[loopCount]);
uiForm.btLru0GyroParmName.Text = gyroMovePoints[loopCount];
System.Threading.Thread.Sleep(500);
return("next");
}
else
{
// Exit scale calibration
currentState = (int)SCALE_STATE.END_SCALE;
return("none");
}
}
case (int)SCALE_STATE.WAIT_END:
{
uiForm.btLru0GyroParmValue.Text = "R:" + uiForm.lru0.dataParam[(int)PARAM.GYROXSUM].ToString("000.00") +
" P:" + uiForm.lru0.dataParam[(int)PARAM.GYROYSUM].ToString("000.00") +
" Y:" + uiForm.lru0.dataParam[(int)PARAM.GYROZSUM].ToString("000.00");
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
System.Threading.Thread.Sleep(500);
return("next");
}
}
case (int)SCALE_STATE.GET_SCALE:
{
switch (loopCount)
{
// X Effect (Roll)
case 1:
System.Threading.Thread.Sleep(200);
deltaX = uiForm.lru0.dataParam[(int)PARAM.GYROXSUM];
deltaY = uiForm.lru0.dataParam[(int)PARAM.GYROYSUM];
deltaZ = uiForm.lru0.dataParam[(int)PARAM.GYROZSUM];
xonx = (1.0 / (deltaX / SCALE_SWEEP_ANGLE));
xony = (deltaY / SCALE_SWEEP_ANGLE);
xonz = (deltaZ / SCALE_SWEEP_ANGLE);
break;
// Y Effect (Pitch)
case 3:
System.Threading.Thread.Sleep(200);
deltaX = uiForm.lru0.dataParam[(int)PARAM.GYROXSUM];
deltaY = uiForm.lru0.dataParam[(int)PARAM.GYROYSUM];
deltaZ = uiForm.lru0.dataParam[(int)PARAM.GYROZSUM];
yonx = (deltaX / SCALE_SWEEP_ANGLE);
yony = (1.0 / (deltaY / SCALE_SWEEP_ANGLE));
yonz = (deltaZ / SCALE_SWEEP_ANGLE);
break;
// Z Effect (Yaw)
case 5:
System.Threading.Thread.Sleep(200);
deltaX = uiForm.lru0.dataParam[(int)PARAM.GYROXSUM];
deltaY = uiForm.lru0.dataParam[(int)PARAM.GYROYSUM];
deltaZ = uiForm.lru0.dataParam[(int)PARAM.GYROZSUM];
zonx = (deltaX / SCALE_SWEEP_ANGLE);
zony = (deltaY / SCALE_SWEEP_ANGLE);
zonz = (1.0 / (deltaZ / SCALE_SWEEP_ANGLE));
break;
}
loopCount++;
currentState = (int)SCALE_STATE.MOVE_START;
return("none");
}
case (int)SCALE_STATE.END_SCALE:
{
PosFixture.setPosition("X0 Y0 Z0");
// Start Program cycle
setPrgRunningState(true);
return("wait");
}
default:
{
return("end");
}
}
}
private static string scaleStateMachine()
{
switch (currentState)
{
case (int)SCALE_STATE.AHRS_RES:
{
PosFixture.setPosition("X0 Y0 Z0");
uiForm.btLru0GyroParmName.Text = "AHRS Reset";
uiForm.btLru0GyroParmValue.Text = "AHRS Reset";
// Put into slaving mode
uiForm.lru0.setEngWriteF(ENG.FRGM, 0.0);
// Reset AHRS
uiForm.lru0.setEngWriteF(ENG.ARES, 1.0);
uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
uiForm.lru0.setParameterReq((int)PARAM.STATUS, RATE.RATE_1HZ);
uiForm.lru0.setParameterReq((int)PARAM.RPYWEIGHT, RATE.RATE_50HZ);
uiForm.lru0.setParameterReq((int)PARAM.BIASWEIGHT, RATE.RATE_50HZ);
loopCount = 0;
stableCount = 0;
Thread.Sleep(1000);
return("next");
}
case (int)SCALE_STATE.AHRS_WAIT:
{
double weight = uiForm.lru0.dataParam[(int)PARAM.RPYWEIGHT];
uiForm.btLru0GyroParmValue.Text = weight.ToString("00");
if (((int)uiForm.lru0.dataParam[(int)PARAM.STATUS] & STATUS.AHRS_STABLE) != 0)
{
if (stableCount < 100)
{
stableCount++;
return("wait");
}
else
{
return("next");
}
}
else
{
return("wait");
}
}
case (int)SCALE_STATE.PREP_SCALE:
{
uiForm.btLru0GyroParmName.Text = "Scale Test Points";
uiForm.btLru0GyroParmValue.Text = "Set Position";
uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
uiForm.lru0.setParameterReq((int)PARAM.STATUS, RATE.RATE_1HZ);
uiForm.lru0.setParameterReq((int)PARAM.ROLL, RATE.RATE_50HZ);
uiForm.lru0.setParameterReq((int)PARAM.PITCH, RATE.RATE_50HZ);
uiForm.lru0.setParameterReq((int)PARAM.YAW, RATE.RATE_50HZ);
loopCount = 0;
// Put into free gyro mode
uiForm.lru0.setEngWriteF(ENG.FRGM, 1.0);
return("next");
}
case (int)SCALE_STATE.SET_SCALE:
{
if (loopCount < gyroMovePoints.Length)
{
PosFixture.setPosition(gyroMovePoints[loopCount]);
uiForm.btLru0GyroParmName.Text = gyroMovePoints[loopCount];
sampleCount = 0;
return("next");
}
else
{
currentState = (int)SCALE_STATE.END_SCALE;
return("none");
}
}
case (int)SCALE_STATE.WAIT_SCALE:
{
uiForm.btLru0GyroParmValue.Text = "R:" + uiForm.lru0.dataParam[(int)PARAM.ROLL].ToString("00.00") +
" P:" + uiForm.lru0.dataParam[(int)PARAM.PITCH].ToString("00.00") +
" Y:" + uiForm.lru0.dataParam[(int)PARAM.YAW].ToString("00.00");
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
// Wait for full motion to stop and get readings
if (sampleCount++ > 5)
{
return("next");
}
else
{
return("wait");
}
}
}
case (int)SCALE_STATE.GET_SCALE:
{
switch (loopCount)
{
// X Effect (Roll)
case 0:
xonx0 = -uiForm.lru0.dataParam[(int)PARAM.ROLL];
xony0 = -uiForm.lru0.dataParam[(int)PARAM.PITCH];
xonz0 = -uiForm.lru0.dataParam[(int)PARAM.YAW];
break;
case 1:
if (scalePassNumber == 1)
{
xonx1 = 1.0 / ((xonx0 + uiForm.lru0.dataParam[(int)PARAM.ROLL]) / SCALE_SWEEP_ANGLE);
xony1 = ((xony0 + uiForm.lru0.dataParam[(int)PARAM.PITCH]) / SCALE_SWEEP_ANGLE);
xonz1 = ((xonz0 + uiForm.lru0.dataParam[(int)PARAM.YAW]) / SCALE_SWEEP_ANGLE);
addTableEntry("X on X 1", xonx1, "MEASURE");
addTableEntry("X on Y 1", xony1, "MEASURE");
addTableEntry("X on Z 1", xonz1, "MEASURE");
}
if (scalePassNumber == 2)
{
xonx2 = 1.0 / ((xonx0 + uiForm.lru0.dataParam[(int)PARAM.ROLL]) / SCALE_SWEEP_ANGLE);
xony2 = ((xony0 + uiForm.lru0.dataParam[(int)PARAM.PITCH]) / SCALE_SWEEP_ANGLE);
xonz2 = ((xonz0 + uiForm.lru0.dataParam[(int)PARAM.YAW]) / SCALE_SWEEP_ANGLE);
addTableEntry("X on X 2", xonx2, "MEASURE");
addTableEntry("X on Y 2", xony2, "MEASURE");
addTableEntry("X on Z 2", xonz2, "MEASURE");
}
PosFixture.setPosition("X0 Y0 Z0");
break;
// Y Effect (Pitch)
case 2:
yonx0 = -uiForm.lru0.dataParam[(int)PARAM.ROLL];
yony0 = -uiForm.lru0.dataParam[(int)PARAM.PITCH];
yonz0 = -uiForm.lru0.dataParam[(int)PARAM.YAW];
break;
case 3:
if (scalePassNumber == 1)
{
yonx1 = ((yonx0 + uiForm.lru0.dataParam[(int)PARAM.ROLL]) / SCALE_SWEEP_ANGLE);
yony1 = 1.0 / ((yony0 + uiForm.lru0.dataParam[(int)PARAM.PITCH]) / SCALE_SWEEP_ANGLE);
yonz1 = ((yonz0 + uiForm.lru0.dataParam[(int)PARAM.YAW]) / SCALE_SWEEP_ANGLE); //-
addTableEntry("Y on X 1", yonx1, "MEASURE");
addTableEntry("Y on Y 1", yony1, "MEASURE");
addTableEntry("Y on Z 1", yonz1, "MEASURE");
}
if (scalePassNumber == 2)
{
yonx2 = ((yonx0 + uiForm.lru0.dataParam[(int)PARAM.ROLL]) / SCALE_SWEEP_ANGLE);
yony2 = 1.0 / ((yony0 + uiForm.lru0.dataParam[(int)PARAM.PITCH]) / SCALE_SWEEP_ANGLE);
yonz2 = ((yonz0 + uiForm.lru0.dataParam[(int)PARAM.YAW]) / SCALE_SWEEP_ANGLE); //-
addTableEntry("Y on X 2", yonx2, "MEASURE");
addTableEntry("Y on Y 2", yony2, "MEASURE");
addTableEntry("Y on Z 2", yonz2, "MEASURE");
}
PosFixture.setPosition("X0 Y0 Z0");
break;
// Z Effect (Yaw)
case 4:
// Capture roll left
zonx0 = -uiForm.lru0.dataParam[(int)PARAM.ROLL];
zony0 = -uiForm.lru0.dataParam[(int)PARAM.PITCH];
zonz0 = -uiForm.lru0.dataParam[(int)PARAM.YAW];
break;
case 5:
if (scalePassNumber == 1)
{
zonx1 = ((zonx0 + uiForm.lru0.dataParam[(int)PARAM.ROLL]) / SCALE_SWEEP_ANGLE);
zony1 = ((zony0 + uiForm.lru0.dataParam[(int)PARAM.PITCH]) / SCALE_SWEEP_ANGLE); //-
zonz1 = 1.0 / ((zonz0 + uiForm.lru0.dataParam[(int)PARAM.YAW]) / SCALE_SWEEP_ANGLE);
addTableEntry("Z on X 1", zonx1, "MEASURE");
addTableEntry("Z on Y 1", zony1, "MEASURE");
addTableEntry("Z on Z 1", zonz1, "MEASURE");
}
if (scalePassNumber == 2)
{
zonx2 = ((zonx0 + uiForm.lru0.dataParam[(int)PARAM.ROLL]) / SCALE_SWEEP_ANGLE);
zony2 = ((zony0 + uiForm.lru0.dataParam[(int)PARAM.PITCH]) / SCALE_SWEEP_ANGLE); //-
zonz2 = 1.0 / ((zonz0 + uiForm.lru0.dataParam[(int)PARAM.YAW]) / SCALE_SWEEP_ANGLE);
addTableEntry("Z on X 2", zonx2, "MEASURE");
addTableEntry("Z on Y 2", zony2, "MEASURE");
addTableEntry("Z on Z 2", zonz2, "MEASURE");
}
PosFixture.setPosition("X0 Y0 Z0");
break;
}
loopCount++;
currentState = (int)SCALE_STATE.SET_SCALE;
return("none");
}
case (int)SCALE_STATE.END_SCALE:
{
// Put back into slave mode
uiForm.lru0.setEngWriteF(ENG.FRGM, 0.0);
PosFixture.setPosition("X0 Y0 Z0");
// Check for chain
if (uiForm.cbChainGyrolCalToPrg.Checked)
{
// Start Program cycle
setPrgRunningState(true);
return("wait");
}
return("end");
}
default:
{
return("end");
}
}
}
private static string accelStateMachine()
{
int loop = 0;
switch (currentState)
{
//
// Accel X
//
case (int)CAL_STATE.SET_ACCELX0:
{
uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
uiForm.lru0.setParameterReq((int)PARAM.ACCELXR0, RATE.RATE_50HZ);
uiForm.lru0.setParameterReq((int)PARAM.ACCELXR1, RATE.RATE_50HZ);
uiForm.btLru0AccelParmName.Text = "Accel X Zero";
///PosFixture.setPosition("X5 Y5 Z5");
PosFixture.setPosition("X0 Y0 Z0");
sampleCount = HOLD_COUNT;
return("next");
}
case (int)CAL_STATE.WAIT_ACCELX0:
{
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
if (sampleCount-- > 0)
{
return("wait");
}
else
{
sampleCount = 0;
return("next");
}
}
}
case (int)CAL_STATE.GET_ACCELX0:
{
if (sampleCount < SAMPLE_COUNT)
{
dataSample0[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELXR0];
dataSample1[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELXR1];
uiForm.btLru0AccelParmValue.Text = dataSample0[sampleCount].ToString("00.00000");
sampleCount++;
return("wait");
}
else
{
calData[(int)CAL_POINT.AXZ] = (dataSample0.Average() + dataSample1.Average()) / 2.0;
addTableEntry(uiForm.btLru0AccelParmName.Text, calData[(int)CAL_POINT.AXZ], "MEASURE");
return("next");
}
}
case (int)CAL_STATE.SET_ACCELXP1:
{
uiForm.btLru0AccelParmName.Text = "Accel X Pos";
PosFixture.setPosition("X0 Y0 Z-" + ANGLE);
sampleCount = HOLD_COUNT;
return("next");
}
case (int)CAL_STATE.WAIT_ACCELXP1:
{
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
if (sampleCount-- > 0)
{
return("wait");
}
else
{
sampleCount = 0;
return("next");
}
}
}
case (int)CAL_STATE.GET_ACCELXP1:
{
if (sampleCount < SAMPLE_COUNT)
{
dataSample0[sampleCount] = (uiForm.lru0.dataParam[(int)PARAM.ACCELXR0] - calData[(int)CAL_POINT.AXZ]) * (1.0 / Math.Sin(ANGLE_IN_RAD));
dataSample1[sampleCount] = (uiForm.lru0.dataParam[(int)PARAM.ACCELXR1] - calData[(int)CAL_POINT.AXZ]) * (1.0 / Math.Sin(ANGLE_IN_RAD));
uiForm.btLru0AccelParmValue.Text = dataSample0[sampleCount].ToString("00.00000");
sampleCount++;
return("wait");
}
else
{
calData[(int)CAL_POINT.AXP] = (dataSample0.Average() + dataSample1.Average()) / 2.0;
addTableEntry(uiForm.btLru0AccelParmName.Text, calData[(int)CAL_POINT.AXP], "MEASURE");
return("next");
}
}
case (int)CAL_STATE.SET_ACCELXN1:
{
uiForm.btLru0AccelParmName.Text = "Accel X Neg";
PosFixture.setPosition("X0 Y0 Z+" + ANGLE);
sampleCount = HOLD_COUNT;
return("next");
}
case (int)CAL_STATE.WAIT_ACCELXN1:
{
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
if (sampleCount-- > 0)
{
return("wait");
}
else
{
sampleCount = 0;
return("next");
}
}
}
case (int)CAL_STATE.GET_ACCELXN1:
{
if (sampleCount < SAMPLE_COUNT)
{
dataSample0[sampleCount] = (uiForm.lru0.dataParam[(int)PARAM.ACCELXR0] - calData[(int)CAL_POINT.AXZ]) * (1.0 / Math.Sin(ANGLE_IN_RAD));
dataSample1[sampleCount] = (uiForm.lru0.dataParam[(int)PARAM.ACCELXR1] - calData[(int)CAL_POINT.AXZ]) * (1.0 / Math.Sin(ANGLE_IN_RAD));
uiForm.btLru0AccelParmValue.Text = dataSample0[sampleCount].ToString("00.00000");
sampleCount++;
return("wait");
}
else
{
calData[(int)CAL_POINT.AXN] = (dataSample0.Average() + dataSample1.Average()) / 2.0;
addTableEntry(uiForm.btLru0AccelParmName.Text, calData[(int)CAL_POINT.AXN], "MEASURE");
return("next");
}
}
//
// Accel Y
//
case (int)CAL_STATE.SET_ACCELY0:
{
uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
uiForm.lru0.setParameterReq((int)PARAM.ACCELYR0, RATE.RATE_50HZ);
uiForm.lru0.setParameterReq((int)PARAM.ACCELYR1, RATE.RATE_50HZ);
uiForm.btLru0AccelParmName.Text = "Accel Y Zero";
PosFixture.setPosition("X0 Y0 Z0");
sampleCount = HOLD_COUNT;
return("next");
}
case (int)CAL_STATE.WAIT_ACCELY0:
{
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
if (sampleCount-- > 0)
{
return("wait");
}
else
{
sampleCount = 0;
return("next");
}
}
}
case (int)CAL_STATE.GET_ACCELY0:
{
if (sampleCount < SAMPLE_COUNT)
{
dataSample0[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELYR0];
dataSample1[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELYR1];
sampleCount++;
return("wait");
}
else
{
calData[(int)CAL_POINT.AYZ] = (dataSample0.Average() + dataSample1.Average()) / 2.0;
addTableEntry(uiForm.btLru0AccelParmName.Text, calData[(int)CAL_POINT.AYZ], "MEASURE");
return("next");
}
}
case (int)CAL_STATE.SET_ACCELYP1:
{
uiForm.btLru0AccelParmName.Text = "Accel Y Pos";
PosFixture.setPosition("X0 Y" + ANGLE + " Z" + ROLL_ON_PITCH_POS);
sampleCount = HOLD_COUNT;
return("next");
}
case (int)CAL_STATE.WAIT_ACCELYP1:
{
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
if (sampleCount-- > 0)
{
return("wait");
}
else
{
sampleCount = 0;
return("next");
}
}
}
case (int)CAL_STATE.GET_ACCELYP1:
{
if (sampleCount < SAMPLE_COUNT)
{
dataSample0[sampleCount] = (uiForm.lru0.dataParam[(int)PARAM.ACCELYR0] - calData[(int)CAL_POINT.AYZ]) * (1.0 / Math.Sin(ANGLE_IN_RAD));
dataSample1[sampleCount] = (uiForm.lru0.dataParam[(int)PARAM.ACCELYR1] - calData[(int)CAL_POINT.AYZ]) * (1.0 / Math.Sin(ANGLE_IN_RAD));
uiForm.btLru0AccelParmValue.Text = dataSample0[sampleCount].ToString("00.00000");
sampleCount++;
return("wait");
}
else
{
calData[(int)CAL_POINT.AYP] = (dataSample0.Average() + dataSample1.Average()) / 2.0;
addTableEntry(uiForm.btLru0AccelParmName.Text, calData[(int)CAL_POINT.AYP], "MEASURE");
return("next");
}
}
case (int)CAL_STATE.SET_ACCELYN1:
{
uiForm.btLru0AccelParmName.Text = "Accel Y Neg";
PosFixture.setPosition("X0 Y-" + ANGLE + " Z" + ROLL_ON_PITCH_NEG);
sampleCount = HOLD_COUNT;
return("next");
}
case (int)CAL_STATE.WAIT_ACCELYN1:
{
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
if (sampleCount-- > 0)
{
return("wait");
}
else
{
sampleCount = 0;
return("next");
}
}
}
case (int)CAL_STATE.GET_ACCELYN1:
{
if (sampleCount < SAMPLE_COUNT)
{
dataSample0[sampleCount] = (uiForm.lru0.dataParam[(int)PARAM.ACCELYR0] - calData[(int)CAL_POINT.AYZ]) * (1.0 / Math.Sin(ANGLE_IN_RAD));
dataSample1[sampleCount] = (uiForm.lru0.dataParam[(int)PARAM.ACCELYR1] - calData[(int)CAL_POINT.AYZ]) * (1.0 / Math.Sin(ANGLE_IN_RAD));
uiForm.btLru0AccelParmValue.Text = dataSample0[sampleCount].ToString("00.00000");
sampleCount++;
return("wait");
}
else
{
calData[(int)CAL_POINT.AYN] = (dataSample0.Average() + dataSample1.Average()) / 2.0;
addTableEntry(uiForm.btLru0AccelParmName.Text, calData[(int)CAL_POINT.AYN], "MEASURE");
return("next");
}
}
//
// Accel Z
//
case (int)CAL_STATE.SET_ACCELZ0:
{
uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
uiForm.lru0.setParameterReq((int)PARAM.ACCELZR0, RATE.RATE_50HZ);
uiForm.lru0.setParameterReq((int)PARAM.ACCELZR1, RATE.RATE_50HZ);
uiForm.btLru0AccelParmName.Text = "Accel Z Zero";
PosFixture.setPosition("X0 Y90 Z0");
sampleCount = HOLD_COUNT;
return("next");
}
case (int)CAL_STATE.WAIT_ACCELZ0:
{
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
if (sampleCount-- > 0)
{
return("wait");
}
else
{
sampleCount = 0;
return("next");
}
}
}
case (int)CAL_STATE.GET_ACCELZ0:
{
if (sampleCount < SAMPLE_COUNT)
{
dataSample0[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELZR0];
dataSample1[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELZR1];
uiForm.btLru0AccelParmValue.Text = dataSample0[sampleCount].ToString("00.00000");
sampleCount++;
return("wait");
}
else
{
calData[(int)CAL_POINT.AZZ] = (dataSample0.Average() + dataSample1.Average()) / 2.0;
addTableEntry(uiForm.btLru0AccelParmName.Text, calData[(int)CAL_POINT.AZZ], "MEASURE");
return("next");
}
}
case (int)CAL_STATE.SET_ACCELZP1:
{
uiForm.btLru0AccelParmName.Text = "Accel Z Pos";
PosFixture.setPosition("X0 Y0 Z0");
sampleCount = HOLD_COUNT;
return("next");
}
case (int)CAL_STATE.WAIT_ACCELZP1:
{
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
if (sampleCount-- > 0)
{
return("wait");
}
else
{
sampleCount = 0;
return("next");
}
}
}
case (int)CAL_STATE.GET_ACCELZP1:
{
if (sampleCount < SAMPLE_COUNT)
{
dataSample0[sampleCount] = (uiForm.lru0.dataParam[(int)PARAM.ACCELZR0] - calData[(int)CAL_POINT.AZZ]);
dataSample1[sampleCount] = (uiForm.lru0.dataParam[(int)PARAM.ACCELZR1] - calData[(int)CAL_POINT.AZZ]);
uiForm.btLru0AccelParmValue.Text = dataSample0[sampleCount].ToString("00.00000");
sampleCount++;
return("wait");
}
else
{
calData[(int)CAL_POINT.AZP] = (dataSample0.Average() + dataSample1.Average()) / 2.0;
addTableEntry(uiForm.btLru0AccelParmName.Text, calData[(int)CAL_POINT.AZP], "MEASURE");
return("next");
}
}
case (int)CAL_STATE.SET_ACCELZN1:
{
uiForm.btLru0AccelParmName.Text = "Accel Z Neg";
PosFixture.setPosition("X0 Y180 Z0");
sampleCount = HOLD_COUNT;
return("next");
}
case (int)CAL_STATE.WAIT_ACCELZN1:
{
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
if (sampleCount-- > 0)
{
return("wait");
}
else
{
sampleCount = 0;
return("next");
}
}
}
case (int)CAL_STATE.GET_ACCELZN1:
{
if (sampleCount < SAMPLE_COUNT)
{
dataSample0[sampleCount] = (uiForm.lru0.dataParam[(int)PARAM.ACCELZR0] - calData[(int)CAL_POINT.AZZ]);
dataSample1[sampleCount] = (uiForm.lru0.dataParam[(int)PARAM.ACCELZR1] - calData[(int)CAL_POINT.AZZ]);
uiForm.btLru0AccelParmValue.Text = dataSample0[sampleCount].ToString("00.00000");
sampleCount++;
return("wait");
}
else
{
calData[(int)CAL_POINT.AZN] = (dataSample0.Average() + dataSample1.Average()) / 2.0;
addTableEntry(uiForm.btLru0AccelParmName.Text, calData[(int)CAL_POINT.AZN], "MEASURE");
// Back to zero
PosFixture.setPosition("X0 Y0 Z0");
// Start Prog cycle
setPrgRunningState(true);
return("wait");
}
}
default:
{
return("end");
}
}
}
private static string biasStateMachine()
{
switch (currentState)
{
case (int)BIAS_STATE.SET_BIAS:
{
if (loopCount < movePoints.Length)
{
PosFixture.setPosition(movePoints[loopCount]);
uiForm.btLru0MagParmName.Text = movePoints[loopCount];
return("next");
}
else
{
//currentState = (int)BIAS_STATE.PROG_BIAS;
return("none");
}
}
case (int)BIAS_STATE.WAIT_BIAS:
{
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
sampleCount = 0;
return("next");
}
}
case (int)BIAS_STATE.GET_BIAS:
{
uiForm.btLru0MagParmName.Text = movePointDesc[loopCount];
switch (loopCount)
{
// X Positive
case 0:
xBiasPos = uiForm.lru0.dataParam[(int)PARAM.MAGX];
break;
// X Negative
case 1:
xBiasNeg = uiForm.lru0.dataParam[(int)PARAM.MAGX];
break;
}
loopCount++;
currentState = (int)BIAS_STATE.SET_BIAS;
return("none");
}
case (int)BIAS_STATE.PROG_BIAS:
{
return("end");
//uiForm.btLru0MagParmName.Text = "Program Bias XYZ";
//uiForm.lru0.setCalWriteF(FLASH_LOC_MAG_CAL_START + 0, xBiasPos - xBiasNeg;
//uiForm.lru0.setCalWriteF(FLASH_LOC_MAG_CAL_START + 1, calData[(int)CAL_POINT.BIASY]);
//uiForm.lru0.setCalWriteF(FLASH_LOC_MAG_CAL_START + 2, calData[(int)CAL_POINT.BIASZ]);
//// Clear scales
//uiForm.lru0.setCalWriteF(FLASH_LOC_MAG_CAL_START + 3, 1);
//uiForm.lru0.setCalWriteF(FLASH_LOC_MAG_CAL_START + 4, 1);
//uiForm.lru0.setCalWriteF(FLASH_LOC_MAG_CAL_START + 5, 1);
//uiForm.lru0.setCalWriteF((int)PARAM.FLUSH, 0);
//// Start Scale
//setScaleRunningSate(true);
//return ("none");
}
default:
{
return("end");
}
}
}
// State machine update
public static void update()
{
string msg = "";
// Handle state
if (biasPointsRunning)
{
// Display Current
uiForm.btLru0MagXCur.Text = uiForm.lru0.dataParam[(int)PARAM.MAGX].ToString("0000");
uiForm.btLru0MagYCur.Text = uiForm.lru0.dataParam[(int)PARAM.MAGY].ToString("0000");
uiForm.btLru0MagZCur.Text = uiForm.lru0.dataParam[(int)PARAM.MAGZ].ToString("0000");
// Default Colors
uiForm.btLru0MagXMin.BackColor = Color.Transparent;
uiForm.btLru0MagYMin.BackColor = Color.Transparent;
uiForm.btLru0MagZMin.BackColor = Color.Transparent;
uiForm.btLru0MagXMax.BackColor = Color.Transparent;
uiForm.btLru0MagYMax.BackColor = Color.Transparent;
uiForm.btLru0MagZMax.BackColor = Color.Transparent;
uiForm.btLru0MagXNorm.BackColor = Color.Transparent;
uiForm.btLru0MagYNorm.BackColor = Color.Transparent;
uiForm.btLru0MagZNorm.BackColor = Color.Transparent;
if ((uiForm.lru0.dataParam[(int)PARAM.MAGX] != 0) &&
(uiForm.lru0.dataParam[(int)PARAM.MAGY] != 0) &&
(uiForm.lru0.dataParam[(int)PARAM.MAGZ] != 0))
{
// Mag Min
if (uiForm.lru0.dataParam[(int)PARAM.MAGX] < double.Parse(uiForm.btLru0MagXMin.Text))
{
uiForm.btLru0MagXMin.Text = uiForm.lru0.dataParam[(int)PARAM.MAGX].ToString("F1");
uiForm.btLru0MagXMin.BackColor = Color.DarkGreen;
}
if (uiForm.lru0.dataParam[(int)PARAM.MAGY] < double.Parse(uiForm.btLru0MagYMin.Text))
{
uiForm.btLru0MagYMin.Text = uiForm.lru0.dataParam[(int)PARAM.MAGY].ToString("F1");
uiForm.btLru0MagYMin.BackColor = Color.DarkGreen;
}
if (uiForm.lru0.dataParam[(int)PARAM.MAGZ] < double.Parse(uiForm.btLru0MagZMin.Text))
{
uiForm.btLru0MagZMin.Text = uiForm.lru0.dataParam[(int)PARAM.MAGZ].ToString("F1");
uiForm.btLru0MagZMin.BackColor = Color.DarkGreen;
}
// Mag Max
if (uiForm.lru0.dataParam[(int)PARAM.MAGX] > double.Parse(uiForm.btLru0MagXMax.Text))
{
uiForm.btLru0MagXMax.Text = uiForm.lru0.dataParam[(int)PARAM.MAGX].ToString("F1");
uiForm.btLru0MagXMax.BackColor = Color.DarkGreen;
}
if (uiForm.lru0.dataParam[(int)PARAM.MAGY] > double.Parse(uiForm.btLru0MagYMax.Text))
{
uiForm.btLru0MagYMax.Text = uiForm.lru0.dataParam[(int)PARAM.MAGY].ToString("F1");
uiForm.btLru0MagYMax.BackColor = Color.DarkGreen;
}
if (uiForm.lru0.dataParam[(int)PARAM.MAGZ] > double.Parse(uiForm.btLru0MagZMax.Text))
{
uiForm.btLru0MagZMax.Text = uiForm.lru0.dataParam[(int)PARAM.MAGZ].ToString("F1");
uiForm.btLru0MagZMax.BackColor = Color.DarkGreen;
}
// Delta
uiForm.btLru0MagXDelta.Text = Math.Abs(double.Parse(uiForm.btLru0MagXMax.Text) - double.Parse(uiForm.btLru0MagXMin.Text)).ToString("F1");
uiForm.btLru0MagYDelta.Text = Math.Abs(double.Parse(uiForm.btLru0MagYMax.Text) - double.Parse(uiForm.btLru0MagYMin.Text)).ToString("F1");
uiForm.btLru0MagZDelta.Text = Math.Abs(double.Parse(uiForm.btLru0MagZMax.Text) - double.Parse(uiForm.btLru0MagZMin.Text)).ToString("F1");
uiForm.btLru0MagXOffset.Text = (-double.Parse(uiForm.btLru0MagXMin.Text) - (double.Parse(uiForm.btLru0MagXDelta.Text) / 2)).ToString("F2");
uiForm.btLru0MagYOffset.Text = (-double.Parse(uiForm.btLru0MagYMin.Text) - (double.Parse(uiForm.btLru0MagYDelta.Text) / 2)).ToString("F2");
uiForm.btLru0MagZOffset.Text = (-double.Parse(uiForm.btLru0MagZMin.Text) - (double.Parse(uiForm.btLru0MagZDelta.Text) / 2)).ToString("F2");
uiForm.btLru0MagXCent.Text = (uiForm.lru0.dataParam[(int)PARAM.MAGX] + double.Parse(uiForm.btLru0MagXOffset.Text)).ToString("F3");
uiForm.btLru0MagYCent.Text = (uiForm.lru0.dataParam[(int)PARAM.MAGY] + double.Parse(uiForm.btLru0MagYOffset.Text)).ToString("F3");
uiForm.btLru0MagZCent.Text = (uiForm.lru0.dataParam[(int)PARAM.MAGZ] + double.Parse(uiForm.btLru0MagZOffset.Text)).ToString("F3");
uiForm.btLru0MagXNorm.Text = (double.Parse(uiForm.btLru0MagXCent.Text) / (double.Parse(uiForm.btLru0MagXDelta.Text) / 2)).ToString("F3");
uiForm.btLru0MagYNorm.Text = (double.Parse(uiForm.btLru0MagYCent.Text) / (double.Parse(uiForm.btLru0MagYDelta.Text) / 2)).ToString("F3");
uiForm.btLru0MagZNorm.Text = (double.Parse(uiForm.btLru0MagZCent.Text) / (double.Parse(uiForm.btLru0MagZDelta.Text) / 2)).ToString("F3");
if (Math.Abs(double.Parse(uiForm.btLru0MagXNorm.Text)) > 0.95)
{
uiForm.btLru0MagXNorm.BackColor = Color.DarkGreen;
}
if (Math.Abs(double.Parse(uiForm.btLru0MagYNorm.Text)) > 0.95)
{
uiForm.btLru0MagYNorm.BackColor = Color.DarkGreen;
}
if (Math.Abs(double.Parse(uiForm.btLru0MagZNorm.Text)) > 0.95)
{
uiForm.btLru0MagZNorm.BackColor = Color.DarkGreen;
}
// Computed Heading
double heading = Math.Atan2(double.Parse(uiForm.btLru0MagYNorm.Text), double.Parse(uiForm.btLru0MagXNorm.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;
//}
uiForm.btLru0Heading.Text = heading.ToString("F0");
}
// msg = biasStateMachine();
}
//else if (scalePointsRunning)
//{
// msg = scaleStateMachine();
//}
//else if (prgPointsRunning)
//{
// msg = prgPointsStateMachine();
//}
//else if (readPointsRunning)
//{
// msg = readPointsStateMachine();
//}
// Handle next step
if (msg == "next")
{
currentState++;
}
else if (msg == "back")
{
currentState--;
}
else if (msg == "skip")
{
currentState += 2;
}
else if (msg == "wait")
{
}
else if (msg == "end")
{
// Turn off outputs
uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
biasPointsRunning = false;
scalePointsRunning = false;
prgPointsRunning = false;
readPointsRunning = false;
// Back to zero
PosFixture.setPosition("X0 Y0 Z0");
// Data
uiForm.btLru0MagParmName.Text = "Complete";
uiForm.btLru0MagParmValue.Text = "";
// Button color
uiForm.btStartMagCal.BackColor = System.Drawing.Color.DimGray;
}
}
private static string accelStateMachine()
{
int loop = 0;
switch (currentState)
{
//
// Accel X
//
case (int)CAL_STATE.SET_ACCELX0:
{
uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
uiForm.lru0.setParameterReq((int)PARAM.ACCELXR, RATE.RATE_50HZ);
uiForm.btLru0ParmName.Text = "Accel X Zero";
PosFixture.setPosition("X10 Y0 Z0");
return("next");
}
case (int)CAL_STATE.WAIT_ACCELX0:
{
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
sampleCount = 0;
return("next");
}
}
case (int)CAL_STATE.GET_ACCELX0:
{
if (sampleCount < SAMPLE_COUNT)
{
dataSample[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELXR];
sampleCount++;
return("wait");
}
else
{
axz = dataSample.Average();
addTableEntry(uiForm.btLru0ParmName.Text, dataSample.Average());
return("next");
}
}
case (int)CAL_STATE.SET_ACCELXP1:
{
uiForm.btLru0ParmName.Text = "Accel X Pos";
PosFixture.setPosition("X0 Y0 Z-90");
return("next");
}
case (int)CAL_STATE.WAIT_ACCELXP1:
{
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
sampleCount = 0;
return("next");
}
}
case (int)CAL_STATE.GET_ACCELXP1:
{
if (sampleCount < SAMPLE_COUNT)
{
dataSample[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELXR];
sampleCount++;
return("wait");
}
else
{
axp = dataSample.Average();
addTableEntry(uiForm.btLru0ParmName.Text, dataSample.Average());
return("next");
}
}
case (int)CAL_STATE.SET_ACCELXN1:
{
uiForm.btLru0ParmName.Text = "Accel X Neg";
PosFixture.setPosition("X0 Y0 Z+90");
return("next");
}
case (int)CAL_STATE.WAIT_ACCELXN1:
{
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
sampleCount = 0;
return("next");
}
}
case (int)CAL_STATE.GET_ACCELXN1:
{
if (sampleCount < SAMPLE_COUNT)
{
dataSample[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELXR];
sampleCount++;
return("wait");
}
else
{
axn = dataSample.Average();
addTableEntry(uiForm.btLru0ParmName.Text, dataSample.Average());
return("next");
}
}
//
// Accel Y
//
case (int)CAL_STATE.SET_ACCELY0:
{
uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
uiForm.lru0.setParameterReq((int)PARAM.ACCELYR, RATE.RATE_50HZ);
uiForm.btLru0ParmName.Text = "Accel Y Zero";
PosFixture.setPosition("X0 Y0 Z0");
return("next");
}
case (int)CAL_STATE.WAIT_ACCELY0:
{
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
sampleCount = 0;
return("next");
}
}
case (int)CAL_STATE.GET_ACCELY0:
{
if (sampleCount < SAMPLE_COUNT)
{
dataSample[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELYR];
sampleCount++;
return("wait");
}
else
{
ayz = dataSample.Average();
addTableEntry(uiForm.btLru0ParmName.Text, dataSample.Average());
return("next");
}
}
case (int)CAL_STATE.SET_ACCELYP1:
{
uiForm.btLru0ParmName.Text = "Accel Y Pos";
PosFixture.setPosition("X0 Y90 Z0");
return("next");
}
case (int)CAL_STATE.WAIT_ACCELYP1:
{
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
sampleCount = 0;
return("next");
}
}
case (int)CAL_STATE.GET_ACCELYP1:
{
if (sampleCount < SAMPLE_COUNT)
{
dataSample[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELYR];
sampleCount++;
return("wait");
}
else
{
ayp = dataSample.Average();
addTableEntry(uiForm.btLru0ParmName.Text, dataSample.Average());
return("next");
}
}
case (int)CAL_STATE.SET_ACCELYN1:
{
uiForm.btLru0ParmName.Text = "Accel Y Neg";
PosFixture.setPosition("X0 Y-90 Z0");
return("next");
}
case (int)CAL_STATE.WAIT_ACCELYN1:
{
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
sampleCount = 0;
return("next");
}
}
case (int)CAL_STATE.GET_ACCELYN1:
{
if (sampleCount < SAMPLE_COUNT)
{
dataSample[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELYR];
sampleCount++;
return("wait");
}
else
{
ayn = dataSample.Average();
addTableEntry(uiForm.btLru0ParmName.Text, dataSample.Average());
return("next");
}
}
//
// Accel Z
//
case (int)CAL_STATE.SET_ACCELZ0:
{
uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
uiForm.lru0.setParameterReq((int)PARAM.ACCELZR, RATE.RATE_50HZ);
uiForm.btLru0ParmName.Text = "Accel Z Zero";
PosFixture.setPosition("X0 Y90 Z0");
return("next");
}
case (int)CAL_STATE.WAIT_ACCELZ0:
{
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
sampleCount = 0;
return("next");
}
}
case (int)CAL_STATE.GET_ACCELZ0:
{
if (sampleCount < SAMPLE_COUNT)
{
dataSample[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELZR];
sampleCount++;
return("wait");
}
else
{
azz = dataSample.Average();
addTableEntry(uiForm.btLru0ParmName.Text, dataSample.Average());
return("next");
}
}
case (int)CAL_STATE.SET_ACCELZP1:
{
uiForm.btLru0ParmName.Text = "Accel Z Pos";
PosFixture.setPosition("X0 Y0 Z0");
return("next");
}
case (int)CAL_STATE.WAIT_ACCELZP1:
{
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
sampleCount = 0;
return("next");
}
}
case (int)CAL_STATE.GET_ACCELZP1:
{
if (sampleCount < SAMPLE_COUNT)
{
dataSample[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELZR];
sampleCount++;
return("wait");
}
else
{
azp = dataSample.Average();
addTableEntry(uiForm.btLru0ParmName.Text, dataSample.Average());
return("next");
}
}
case (int)CAL_STATE.SET_ACCELZN1:
{
uiForm.btLru0ParmName.Text = "Accel Z Neg";
PosFixture.setPosition("X0 Y180 Z0");
return("next");
}
case (int)CAL_STATE.WAIT_ACCELZN1:
{
if (PosFixture.getInMotion())
{
return("wait");
}
else
{
sampleCount = 0;
return("next");
}
}
case (int)CAL_STATE.GET_ACCELZN1:
{
if (sampleCount < SAMPLE_COUNT)
{
dataSample[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELZR];
sampleCount++;
return("wait");
}
else
{
azn = dataSample.Average();
addTableEntry(uiForm.btLru0ParmName.Text, dataSample.Average());
return("next");
}
}
default:
{
return("end");
}
}
}
// State machine update
public static void update()
{
string msg = "";
// Show data based on state
if ((currentState >= (int)CAL_STATE.SET_ACCELX0) && (currentState <= (int)CAL_STATE.GET_ACCELXN1))
{
uiForm.btLru0ParmValue.Text = uiForm.lru0.dataParam[(int)PARAM.ACCELXR].ToString("00.00000");
}
if ((currentState >= (int)CAL_STATE.SET_ACCELY0) && (currentState <= (int)CAL_STATE.GET_ACCELYN1))
{
uiForm.btLru0ParmValue.Text = uiForm.lru0.dataParam[(int)PARAM.ACCELYR].ToString("00.00000");
}
if ((currentState >= (int)CAL_STATE.SET_ACCELZ0) && (currentState <= (int)CAL_STATE.GET_ACCELZN1))
{
uiForm.btLru0ParmValue.Text = uiForm.lru0.dataParam[(int)PARAM.ACCELZR].ToString("00.00000");
}
// Handle state
if (calAccelRunning)
{
// General field updates
uiForm.bADCTestPointNum.Text = (BcaCalPoints.currentCalPoint + 1).ToString() + " OF " + BcaCalPoints.bcaCalPoint.Length.ToString();
// Accel cal points
msg = accelStateMachine();
}
else
{
//// Idle data
//try
//{
// uiForm.lru0.setParameterReq((int)PARAM.ROLL, RATE.RATE_50HZ);
// uiForm.lru0.setParameterReq((int)PARAM.PITCH, RATE.RATE_50HZ);
//}
//catch
//{
//}
//// LRU Roll Pitch values
//uiForm.btLru0ParmName.Text = uiForm.lru0.dataParam[(int)PARAM.ROLL].ToString("000.00");
//uiForm.btLru0ParmValue.Text = uiForm.lru0.dataParam[(int)PARAM.PITCH].ToString("00.00");
}
//if (PosFixture.getInHomeMotion())
//{
// uiForm.btHomeFixture.BackColor = System.Drawing.Color.Red;
//}
//else
//{
// uiForm.btHomeFixture.BackColor = System.Drawing.Color.Transparent;
//}
// Get current state from running SM
//if (calGyroRunning)
//{
// // General field updates
// //// Gyro raw rates
// //uiForm.btGyroXRate.Text = uiForm.lru0.dataParam[(int)PARAM.GYROXR].ToString("0000.00");
// //uiForm.btGyroYRate.Text = uiForm.lru0.dataParam[(int)PARAM.GYROYR].ToString("0000.00");
// //uiForm.btGyroZRate.Text = uiForm.lru0.dataParam[(int)PARAM.GYROZR].ToString("0000.00");
// msg = gyroCalstateMachine();
//}
// Handle next step
if (msg == "next")
{
currentState++;
}
else if (msg == "back")
{
currentState--;
}
else if (msg == "skip")
{
currentState += 2;
}
else if (msg == "wait")
{
}
else if (msg == "end")
{
// Turn off outputs
uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
calAccelRunning = false;
calGyroRunning = false;
prgPointsRunning = false;
readPointsRunning = false;
// Back to zero
PosFixture.setPosition("X0 Y0 Z0");
// Data
uiForm.btLru0ParmName.Text = "Complete";
uiForm.btLru0ParmValue.Text = "";
// Button color
uiForm.btStartAccelCal.BackColor = System.Drawing.Color.DimGray;
}
}
