bool StepGuider::BeginCalibration(const PHD_Point& currentLocation)
{
bool bError = false;
try
{
if (!IsConnected())
{
throw ERROR_INFO("Not connected");
}
if (!currentLocation.IsValid())
{
throw ERROR_INFO("Must have a valid start position");
}
ClearCalibration();
m_calibrationState = CALIBRATION_STATE_GOTO_LOWER_RIGHT_CORNER;
m_calibrationStartingLocation.Invalidate();
m_calibrationDetails.raSteps.clear();
m_calibrationDetails.decSteps.clear();
}
catch (wxString Msg)
{
POSSIBLY_UNUSED(Msg);
bError = true;
}
return bError;
}
CVisCalibrate::CVisCalibrate()
{
int ct,i,j,k;
// Allocate data vectors
tN = REPS*XPTS*YPTS;
xscr = new float[tN];
yscr = new float[tN];
xpos = new float[tN];
ypos = new float[tN];
zpos = new float[tN];
// Precalculate screen target positions
for(ct=i=0;i<REPS;i++)
{
for(j=0;j<XPTS;j++)
{
X = (float)(MINX + (MAXX-MINX)*(float)j/(float)(XPTS-1));
for(k=0;k<YPTS;k++)
{
Y = (float)(MINY + (MAXY-MINY)*(float)k/(float)(YPTS-1));
xscr[ct]=X*VisWindow->width;
yscr[ct]=Y*VisWindow->height;
ct++;
}
}
}
// Reset to the Default Unity Calibration
ClearCalibration();
// Turn Logmode off its on in VisWindow
logmode_hold = VisWindow->logmode;
if(logmode_hold) VisWindow->put_logmode(0);
// Create a VisDisk
HRESULT hr;
hr = CoInitialize(0); // initialize COM
hr = CoCreateInstance( // get the interface pointer
CLSID_VisDisk, // clsid
NULL, // outer unknown
CLSCTX_SERVER, // server context
IID_IVisDisk, // refid
(void **)&pID); // MULTI_QI array
pID->put_radius((float).005*(VisWindow->width));
pID->put_fill(0);
pID->put_show(1);
tCount=0;
// posDim = 3;
posDim = 2; // For Now, with 2D Display, Must have 2D input
presentTarget();
}
STDMETHODIMP CVisWindow::put_calibrated(double newVal)
{
if(newVal){
ReadCalibration();
calibrated = IsCalibrated();
}
else{
ClearCalibration();
calibrated = IsCalibrated();
}
return S_OK;
}
//////// Read Calibration
int
ReadCalibration()
{
FILE *fp = fopen(CALIBRATION_FILENAME,"r");
if(!fp){
ClearCalibration();
return 1;
}
char str[360];
// Read 1st Line: Ndat posDim
fgets(str,360,fp);
if( sscanf(str,"%d %d",&calN,&posDim) < 2 )
{
ClearCalibration();
return 1;
}
// Read 2nd Line
fgets(str,360,fp);
if( sscanf(str,"%f %f %f %f %f %f",Xcal,Xcal+1,Xcal+2,Xcal+3,Xcal+4,Xcal+5) < 6 )
{
ClearCalibration();
return 1;
}
// Read 3rd Line
fgets(str,360,fp);
if( sscanf(str,"%f %f %f %f %f %f",Ycal,Ycal+1,Ycal+2,Ycal+3,Ycal+4,Ycal+5) < 6 )
{
ClearCalibration();
return 1;
}
fclose(fp);
// Success
calibratedFlag=1;
return 0;
}
bool StepGuider::UpdateCalibrationState(const PHD_Point& currentLocation)
{
bool bError = false;
try
{
if (!m_calibrationStartingLocation.IsValid())
{
m_calibrationStartingLocation = currentLocation;
Debug.AddLine(wxString::Format("Stepguider::UpdateCalibrationstate: starting location = %.2f,%.2f", currentLocation.X, currentLocation.Y));
}
wxString status0, status1;
int stepsRemainingUp = MaxPosition(UP) - CurrentPosition(UP);
int stepsRemainingDown = MaxPosition(DOWN) - CurrentPosition(DOWN);
int stepsRemainingRight = MaxPosition(RIGHT) - CurrentPosition(RIGHT);
int stepsRemainingLeft = MaxPosition(LEFT) - CurrentPosition(LEFT);
stepsRemainingUp /= m_calibrationStepsPerIteration;
stepsRemainingDown /= m_calibrationStepsPerIteration;
stepsRemainingRight /= m_calibrationStepsPerIteration;
stepsRemainingLeft /= m_calibrationStepsPerIteration;
int stepsRemainingDownAndRight = wxMax(stepsRemainingDown, stepsRemainingRight);
assert(stepsRemainingUp >= 0);
assert(stepsRemainingDown >= 0);
assert(stepsRemainingRight >= 0);
assert(stepsRemainingLeft >= 0);
assert(stepsRemainingDownAndRight >= 0);
bool moveUp = false;
bool moveDown = false;
bool moveRight = false;
bool moveLeft = false;
double x_dist;
double y_dist;
switch (m_calibrationState)
{
case CALIBRATION_STATE_GOTO_LOWER_RIGHT_CORNER:
if (stepsRemainingDownAndRight > 0)
{
status0.Printf(_("Init Calibration: %3d"), stepsRemainingDownAndRight);
moveDown = stepsRemainingDown > 0;
moveRight = stepsRemainingRight > 0;
break;
}
Debug.AddLine(wxString::Format("Falling through to state AVERAGE_STARTING_LOCATION, position=(%.2f, %.2f)",
currentLocation.X, currentLocation.Y));
m_calibrationAverageSamples = 0;
m_calibrationAveragedLocation.SetXY(0.0, 0.0);
m_calibrationState = CALIBRATION_STATE_AVERAGE_STARTING_LOCATION;
// fall through
case CALIBRATION_STATE_AVERAGE_STARTING_LOCATION:
m_calibrationAverageSamples++;
m_calibrationAveragedLocation += currentLocation;
status0.Printf(_("Averaging: %3d"), m_samplesToAverage - m_calibrationAverageSamples + 1);
if (m_calibrationAverageSamples < m_samplesToAverage )
{
break;
}
m_calibrationAveragedLocation /= m_calibrationAverageSamples;
m_calibrationStartingLocation = m_calibrationAveragedLocation;
m_calibrationIterations = 0;
Debug.AddLine(wxString::Format("Falling through to state GO_LEFT, startinglocation=(%.2f, %.2f)",
m_calibrationStartingLocation.X, m_calibrationStartingLocation.Y));
m_calibrationState = CALIBRATION_STATE_GO_LEFT;
// fall through
case CALIBRATION_STATE_GO_LEFT:
if (stepsRemainingLeft > 0)
{
status0.Printf(_("Left Calibration: %3d"), stepsRemainingLeft);
m_calibrationIterations++;
moveLeft = true;
x_dist = m_calibrationStartingLocation.dX(currentLocation);
y_dist = m_calibrationStartingLocation.dY(currentLocation);
GuideLog.CalibrationStep(this, "Left", stepsRemainingLeft,
x_dist, y_dist,
currentLocation, m_calibrationStartingLocation.Distance(currentLocation));
m_calibrationDetails.raSteps.push_back(wxRealPoint(x_dist, y_dist)); // Just put "left" in "ra" steps
break;
}
Debug.AddLine(wxString::Format("Falling through to state AVERAGE_CENTER_LOCATION, position=(%.2f, %.2f)",
currentLocation.X, currentLocation.Y));
m_calibrationAverageSamples = 0;
m_calibrationAveragedLocation.SetXY(0.0, 0.0);
m_calibrationState = CALIBRATION_STATE_AVERAGE_CENTER_LOCATION;
// fall through
case CALIBRATION_STATE_AVERAGE_CENTER_LOCATION:
m_calibrationAverageSamples++;
m_calibrationAveragedLocation += currentLocation;
status0.Printf(_("Averaging: %3d"), m_samplesToAverage -m_calibrationAverageSamples+1);
if (m_calibrationAverageSamples < m_samplesToAverage )
{
break;
}
m_calibrationAveragedLocation /= m_calibrationAverageSamples;
m_calibration.xAngle = m_calibrationStartingLocation.Angle(m_calibrationAveragedLocation);
m_calibration.xRate = m_calibrationStartingLocation.Distance(m_calibrationAveragedLocation) /
(m_calibrationIterations * m_calibrationStepsPerIteration);
status1.Printf(_("angle=%.1f rate=%.2f"), m_calibration.xAngle * 180. / M_PI, m_calibration.xRate);
GuideLog.CalibrationDirectComplete(this, "Left", m_calibration.xAngle, m_calibration.xRate);
Debug.AddLine(wxString::Format("LEFT calibration completes with angle=%.1f rate=%.2f", m_calibration.xAngle * 180. / M_PI, m_calibration.xRate));
Debug.AddLine(wxString::Format("distance=%.2f iterations=%d", m_calibrationStartingLocation.Distance(m_calibrationAveragedLocation), m_calibrationIterations));
m_calibrationStartingLocation = m_calibrationAveragedLocation;
m_calibrationIterations = 0;
m_calibrationState = CALIBRATION_STATE_GO_UP;
Debug.AddLine(wxString::Format("Falling through to state GO_UP, startinglocation=(%.2f, %.2f)",
m_calibrationStartingLocation.X, m_calibrationStartingLocation.Y));
// fall through
case CALIBRATION_STATE_GO_UP:
if (stepsRemainingUp > 0)
{
status0.Printf(_("up Calibration: %3d"), stepsRemainingUp);
m_calibrationIterations++;
moveUp = true;
x_dist = m_calibrationStartingLocation.dX(currentLocation);
y_dist = m_calibrationStartingLocation.dY(currentLocation);
GuideLog.CalibrationStep(this, "Up", stepsRemainingLeft,
x_dist, y_dist,
currentLocation, m_calibrationStartingLocation.Distance(currentLocation));
m_calibrationDetails.decSteps.push_back(wxRealPoint(x_dist, y_dist)); // Just put "up" in "dec" steps
break;
}
Debug.AddLine(wxString::Format("Falling through to state AVERAGE_ENDING_LOCATION, position=(%.2f, %.2f)",
currentLocation.X, currentLocation.Y));
m_calibrationAverageSamples = 0;
m_calibrationAveragedLocation.SetXY(0.0, 0.0);
m_calibrationState = CALIBRATION_STATE_AVERAGE_ENDING_LOCATION;
// fall through
case CALIBRATION_STATE_AVERAGE_ENDING_LOCATION:
m_calibrationAverageSamples++;
m_calibrationAveragedLocation += currentLocation;
status0.Printf(_("Averaging: %3d"), m_samplesToAverage -m_calibrationAverageSamples+1);
if (m_calibrationAverageSamples < m_samplesToAverage )
{
break;
}
m_calibrationAveragedLocation /= m_calibrationAverageSamples;
m_calibration.yAngle = m_calibrationAveragedLocation.Angle(m_calibrationStartingLocation);
m_calibration.yRate = m_calibrationStartingLocation.Distance(m_calibrationAveragedLocation) /
(m_calibrationIterations * m_calibrationStepsPerIteration);
status1.Printf(_("angle=%.1f rate=%.2f"), m_calibration.yAngle * 180. / M_PI, m_calibration.yRate);
GuideLog.CalibrationDirectComplete(this, "Up", m_calibration.yAngle, m_calibration.yRate);
Debug.AddLine(wxString::Format("UP calibration completes with angle=%.1f rate=%.2f", m_calibration.yAngle * 180. / M_PI, m_calibration.yRate));
Debug.AddLine(wxString::Format("distance=%.2f iterations=%d", m_calibrationStartingLocation.Distance(m_calibrationAveragedLocation), m_calibrationIterations));
m_calibrationStartingLocation = m_calibrationAveragedLocation;
m_calibrationState = CALIBRATION_STATE_RECENTER;
Debug.AddLine(wxString::Format("Falling through to state RECENTER, position=(%.2f, %.2f)",
currentLocation.X, currentLocation.Y));
// fall through
case CALIBRATION_STATE_RECENTER:
status0.Printf(_("Finish Calibration: %3d"), stepsRemainingDownAndRight/2);
moveRight = (CurrentPosition(LEFT) >= m_calibrationStepsPerIteration);
moveDown = (CurrentPosition(UP) >= m_calibrationStepsPerIteration);
if (moveRight || moveDown)
{
Debug.AddLine(wxString::Format("CurrentPosition(LEFT)=%d CurrentPosition(UP)=%d", CurrentPosition(LEFT), CurrentPosition(UP)));
break;
}
m_calibrationState = CALIBRATION_STATE_COMPLETE;
Debug.AddLine(wxString::Format("Falling through to state COMPLETE, position=(%.2f, %.2f)",
currentLocation.X, currentLocation.Y));
// fall through
case CALIBRATION_STATE_COMPLETE:
m_calibration.declination = 0.;
m_calibration.pierSide = PIER_SIDE_UNKNOWN;
m_calibration.rotatorAngle = Rotator::RotatorPosition();
SetCalibration(m_calibration);
SetCalibrationDetails(m_calibrationDetails, m_calibration.xAngle, m_calibration.yAngle);
status1 = _T("calibration complete");
GuideLog.CalibrationComplete(this);
Debug.AddLine("Calibration Complete");
break;
default:
assert(false);
break;
}
if (moveUp)
{
assert(!moveDown);
pFrame->ScheduleCalibrationMove(this, UP, m_calibrationStepsPerIteration);
}
if (moveDown)
{
assert(!moveUp);
pFrame->ScheduleCalibrationMove(this, DOWN, m_calibrationStepsPerIteration);
}
if (moveRight)
{
assert(!moveLeft);
pFrame->ScheduleCalibrationMove(this, RIGHT, m_calibrationStepsPerIteration);
}
if (moveLeft)
{
assert(!moveRight);
pFrame->ScheduleCalibrationMove(this, LEFT, m_calibrationStepsPerIteration);
}
if (m_calibrationState != CALIBRATION_STATE_COMPLETE)
{
if (status1.IsEmpty())
{
double dX = m_calibrationStartingLocation.dX(currentLocation);
double dY = m_calibrationStartingLocation.dY(currentLocation);
double dist = m_calibrationStartingLocation.Distance(currentLocation);
status1.Printf(_T("dx=%4.1f dy=%4.1f dist=%4.1f"), dX, dY, dist);
}
}
if (!status0.IsEmpty())
{
pFrame->SetStatusText(status0, 0);
}
if (!status1.IsEmpty())
{
pFrame->SetStatusText(status1, 1);
}
}
catch (wxString Msg)
{
POSSIBLY_UNUSED(Msg);
bError = true;
ClearCalibration();
}
return bError;
}
