diff --git a/kstars/CMakeLists.txt b/kstars/CMakeLists.txt --- a/kstars/CMakeLists.txt +++ b/kstars/CMakeLists.txt @@ -173,6 +173,7 @@ # Focus ekos/focus/focus.cpp ekos/focus/focusalgorithms.cpp + ekos/focus/polynomialfit.cpp # Mount ekos/mount/mount.cpp diff --git a/kstars/ekos/focus/focus.h b/kstars/ekos/focus/focus.h --- a/kstars/ekos/focus/focus.h +++ b/kstars/ekos/focus/focus.h @@ -24,6 +24,7 @@ { class FocusAlgorithmInterface; +class PolynomialFit; /** * @class Focus @@ -400,6 +401,7 @@ //////////////////////////////////////////////////////////////////// void initPlots(); void drawHFRPlot(); + void drawHFRIndeces(); void drawProfilePlot(); //////////////////////////////////////////////////////////////////// @@ -410,8 +412,6 @@ void autoFocusRel(); void resetButtons(); void stop(bool aborted = false); - bool findMinimum(double expected, double *position, double *hfr); - static double fn1(double x, void *params); void initView(); @@ -601,8 +601,8 @@ QCustomPlot *profilePlot { nullptr }; QDialog *profileDialog { nullptr }; - /// Polynomial fitting coefficients - std::vector coeff; + /// Polynomial fitting. + std::unique_ptr polynomialFit; int polySolutionFound { 0 }; QCPGraph *polynomialGraph = nullptr; QCPGraph *focusPoint = nullptr; diff --git a/kstars/ekos/focus/focus.cpp b/kstars/ekos/focus/focus.cpp --- a/kstars/ekos/focus/focus.cpp +++ b/kstars/ekos/focus/focus.cpp @@ -11,6 +11,7 @@ #include "focusadaptor.h" #include "focusalgorithms.h" +#include "polynomialfit.h" #include "kstars.h" #include "kstarsdata.h" #include "Options.h" @@ -1539,14 +1540,13 @@ focusPoint->data()->clear(); polynomialGraphIsShown = false; HFRPlot->clearItems(); + polynomialFit.reset(); drawHFRPlot(); } -void Focus::drawHFRPlot() +void Focus::drawHFRIndeces() { - v_graph->setData(hfr_position, hfr_value); - // Put the sample number inside the plot point's circle. for (int i = 0; i < hfr_position.size(); ++i) { @@ -1559,6 +1559,17 @@ textLabel->setPen(Qt::NoPen); textLabel->setColor(Qt::red); } +} + +void Focus::drawHFRPlot() +{ + // DrawHFRPlot is the base on which other things are built upon. + // Clear any previous annotations. + HFRPlot->clearItems(); + + v_graph->setData(hfr_position, hfr_value); + + drawHFRIndeces(); double minHFRVal = currentHFR / 2.5; if (hfr_value.size() > 0) @@ -1699,6 +1710,22 @@ drawHFRPlot(); if (focusAlgorithm == FOCUS_LINEAR) { + if (hfr_position.size() > 3) + { + // For now, just plots, doesn't use the polynomial algorithmically. + polynomialFit.reset(new PolynomialFit(2, hfr_position, hfr_value)); + double min_position, min_value; + const FocusAlgorithmInterface::FocusParams& params = linearFocuser->getParams(); + double searchMin = std::max(params.minPositionAllowed, params.startPosition - params.maxTravel); + double searchMax = std::min(params.maxPositionAllowed, params.startPosition + params.maxTravel); + if (polynomialFit->findMinimum(linearFocuser->getParams().startPosition, + searchMin, searchMax, &min_position, &min_value)) + { + polynomialFit->drawPolynomial(HFRPlot, polynomialGraph); + polynomialFit->drawMinimum(HFRPlot, focusPoint, min_position, min_value, font()); + } + } + const int nextPosition = adjustLinearPosition( static_cast(currentPosition), linearFocuser->newMeasurement(currentPosition, currentHFR)); @@ -1908,39 +1935,21 @@ bool polyMinimumFound = false; if (focusAlgorithm == FOCUS_POLYNOMIAL && hfr_position.count() > 5) { - double chisq = 0, min_position = 0, min_hfr = 0; - coeff = gsl_polynomial_fit(hfr_position.data(), hfr_value.data(), hfr_position.count(), 3, chisq); - - polyMinimumFound = findMinimum(minHFRPos, &min_position, &min_hfr); - - qCDebug(KSTARS_EKOS_FOCUS) << "Polynomial Coefficients c0:" << coeff[0] << "c1:" << coeff[1] << "c2:" << coeff[2] - << "c3:" << coeff[3]; + polynomialFit.reset(new PolynomialFit(3, hfr_position, hfr_value)); + double a = *std::min_element(hfr_position.constBegin(), hfr_position.constEnd()); + double b = *std::max_element(hfr_position.constBegin(), hfr_position.constEnd()); + double min_position = 0, min_hfr = 0; + polyMinimumFound = polynomialFit->findMinimum(minHFRPos, a, b, &min_position, &min_hfr); qCDebug(KSTARS_EKOS_FOCUS) << "Found Minimum?" << (polyMinimumFound ? "Yes" : "No"); - if (polyMinimumFound) { qCDebug(KSTARS_EKOS_FOCUS) << "Minimum Solution:" << min_hfr << "@" << min_position; polySolutionFound++; targetPosition = floor(min_position); appendLogText(i18n("Found polynomial solution @ %1", QString::number(min_position, 'f', 0))); - graphPolynomialFunction(); - focusPoint->data()->clear(); - focusPoint->addData(min_position, min_hfr); - - HFRPlot->clearItems(); - - QCPItemText *textLabel = new QCPItemText(HFRPlot); - textLabel->setPositionAlignment(Qt::AlignVCenter | Qt::AlignHCenter); - textLabel->position->setType(QCPItemPosition::ptPlotCoords); - textLabel->position->setCoords(min_position, min_hfr / 2); - textLabel->setColor(Qt::red); - textLabel->setPadding(QMargins(0, 0, 0, 0)); - textLabel->setBrush(Qt::white); - textLabel->setPen(Qt::NoPen); - textLabel->setFont(QFont(font().family(), 8)); - textLabel->setText(QString::number(min_position)); - + polynomialFit->drawPolynomial(HFRPlot, polynomialGraph); + polynomialFit->drawMinimum(HFRPlot, focusPoint, min_position, min_hfr, font()); } } @@ -2050,19 +2059,10 @@ void Focus::graphPolynomialFunction() { - if(polynomialGraph) + if (polynomialGraph && polynomialFit) { - polynomialGraph->data()->clear(); - QCPRange range = HFRPlot->xAxis->range(); - double interval = range.size() / 20.0; - - for(double x = range.lower ; x < range.upper ; x += interval) - { - double y = fn1(x, this); - polynomialGraph->addData(x, y); - } - HFRPlot->replot(); polynomialGraphIsShown = true; + polynomialFit->drawPolynomial(HFRPlot, polynomialGraph); } } @@ -2932,67 +2932,6 @@ } } -double Focus::fn1(double x, void *params) -{ - Focus *module = static_cast(params); - - if(module && !module->coeff.empty()) - return (module->coeff[0] + module->coeff[1] * x + module->coeff[2] * pow(x, 2) + module->coeff[3] * pow(x, 3)); - else - return -1; -} - -bool Focus::findMinimum(double expected, double *position, double *hfr) -{ - int status; - int iter = 0, max_iter = 100; - const gsl_min_fminimizer_type *T; - gsl_min_fminimizer *s; - double m = expected; - double a = *std::min_element(hfr_position.constBegin(), hfr_position.constEnd()); - double b = *std::max_element(hfr_position.constBegin(), hfr_position.constEnd()); - gsl_function F; - - F.function = &Focus::fn1; - F.params = this; - - // Must turn off error handler or it aborts on error - gsl_set_error_handler_off(); - - T = gsl_min_fminimizer_brent; - s = gsl_min_fminimizer_alloc(T); - status = gsl_min_fminimizer_set(s, &F, m, a, b); - - if (status != GSL_SUCCESS) - { - qCWarning(KSTARS_EKOS_FOCUS) << "Focus GSL error:" << gsl_strerror(status); - return false; - } - - do - { - iter++; - status = gsl_min_fminimizer_iterate(s); - - m = gsl_min_fminimizer_x_minimum(s); - a = gsl_min_fminimizer_x_lower(s); - b = gsl_min_fminimizer_x_upper(s); - - status = gsl_min_test_interval(a, b, 0.01, 0.0); - - if (status == GSL_SUCCESS) - { - *position = m; - *hfr = fn1(m, this); - } - } - while (status == GSL_CONTINUE && iter < max_iter); - - gsl_min_fminimizer_free(s); - - return (status == GSL_SUCCESS); -} - void Focus::removeDevice(ISD::GDInterface *deviceRemoved) { // Check in Focusers @@ -3477,7 +3416,8 @@ connect(HFRPlot->xAxis, static_cast(&QCPAxis::rangeChanged), this, [this]() { - if(polynomialGraphIsShown) + drawHFRIndeces(); + if (polynomialGraphIsShown) { if (focusAlgorithm == FOCUS_POLYNOMIAL) graphPolynomialFunction(); diff --git a/kstars/ekos/focus/focusalgorithms.h b/kstars/ekos/focus/focusalgorithms.h --- a/kstars/ekos/focus/focusalgorithms.h +++ b/kstars/ekos/focus/focusalgorithms.h @@ -29,8 +29,8 @@ int maxTravel; // Initial sampling interval for the algorithm. int initialStepSize; - // Current absolute position of the focuser. - int currentPosition; + // Absolute position of the focuser when the algorithm starts. + int startPosition; // Minimum position the focuser is allowed to reach. int minPositionAllowed; // Maximum position the focuser is allowed to reach. @@ -40,11 +40,11 @@ // The focus algorithm may terminate if it gets within this fraction of the best focus, e.g. 0.10. double focusTolerance; - FocusParams(int _maxTravel, int _initialStepSize, int _currentPosition, + FocusParams(int _maxTravel, int _initialStepSize, int _startPosition, int _minPositionAllowed, int _maxPositionAllowed, int _maxIterations, double _focusTolerance) : maxTravel(_maxTravel), initialStepSize(_initialStepSize), - currentPosition(_currentPosition), minPositionAllowed(_minPositionAllowed), + startPosition(_startPosition), minPositionAllowed(_minPositionAllowed), maxPositionAllowed(_maxPositionAllowed), maxIterations(_maxIterations), focusTolerance(_focusTolerance) {} }; @@ -54,23 +54,26 @@ virtual ~FocusAlgorithmInterface() {} // After construction, this should be called to get the initial position desired by the - // focus algorithm. It returns the initial position passed to the constructor if + // focus algorithm. It returns the start position passed to the constructor if // it has no movement request. virtual int initialPosition() = 0; // Pass in the recent measurement. Returns the position for the next measurement, // or -1 if the algorithms done or if there's an error. virtual int newMeasurement(int position, double value) = 0; // Returns true if the algorithm has terminated either successfully or in error. - bool isDone() { return done; } + bool isDone() const { return done; } // Returns the best position. Should be called after isDone() returns true. // Returns -1 if there's an error. - int solution() { return focusSolution; } + int solution() const { return focusSolution; } // Returns human-readable extra error information about why the algorithm is done. - QString doneReason() { return doneString;} + QString doneReason() const { return doneString;} + + // Returns the params used to construct this object. + const FocusParams& getParams() const { return params; } protected: FocusParams params; diff --git a/kstars/ekos/focus/focusalgorithms.cpp b/kstars/ekos/focus/focusalgorithms.cpp --- a/kstars/ekos/focus/focusalgorithms.cpp +++ b/kstars/ekos/focus/focusalgorithms.cpp @@ -94,25 +94,25 @@ LinearFocusAlgorithm::LinearFocusAlgorithm(const FocusParams &focusParams) : FocusAlgorithmInterface(focusParams) { - requestedPosition = params.currentPosition; + requestedPosition = params.startPosition; stepSize = params.initialStepSize; minimumFound = false; numSteps = 0; minimumReFound = false; searchFailed = false; minValue = 0; - maxPositionLimit = std::min(params.maxPositionAllowed, params.currentPosition + params.maxTravel); - minPositionLimit = std::max(params.minPositionAllowed, params.currentPosition - params.maxTravel); + maxPositionLimit = std::min(params.maxPositionAllowed, params.startPosition + params.maxTravel); + minPositionLimit = std::max(params.minPositionAllowed, params.startPosition - params.maxTravel); qCDebug(KSTARS_EKOS_FOCUS) << QString("LinearFocuser: Travel %1 initStep %2 pos %3 min %4 max %5 maxIters %6 tolerance %7 minlimit %8 maxlimit %9") - .arg(params.maxTravel).arg(params.initialStepSize).arg(params.currentPosition).arg(params.minPositionAllowed) + .arg(params.maxTravel).arg(params.initialStepSize).arg(params.startPosition).arg(params.minPositionAllowed) .arg(params.maxPositionAllowed).arg(params.maxIterations).arg(params.focusTolerance).arg(minPositionLimit).arg(maxPositionLimit); computeInitialPosition(); } void LinearFocusAlgorithm::computeInitialPosition() { - const int position = params.currentPosition; + const int position = params.startPosition; int start, end; // If the bounds allow, set the focus to half-travel above the current position diff --git a/kstars/ekos/focus/polynomialfit.h b/kstars/ekos/focus/polynomialfit.h new file mode 100644 --- /dev/null +++ b/kstars/ekos/focus/polynomialfit.h @@ -0,0 +1,50 @@ +/* Ekos polynomial fit utilities Algorithms + Copyright (C) 2019 Hy Murveit + + This application is free software; you can redistribute it and/or + modify it under the terms of the GNU General Public + License as published by the Free Software Foundation; either + version 2 of the License, or (at your option) any later version. +*/ + +#pragma once + +#include +#include + +namespace Ekos +{ + +class PolynomialFit +{ +public: + // Constructor. Pass in the degree of the desired polynomial fit, and a vector with the x and y values. + // The constructor solves for the polynomial coefficients. + PolynomialFit(int degree, const QVector& x, const QVector& y); + + // Returns the minimum position and value in the pointers for the solved polynomial. + // Returns false if the polynomial couldn't be solved. + bool findMinimum(double expected, double minPosition, double maxPosition, double *position, double *value); + + // Draws the polynomial on the plot's graph. + void drawPolynomial(QCustomPlot *plot, QCPGraph *graph); + // Annotate's the plot's solution graph with the solution position. + void drawMinimum(QCustomPlot *plot, QCPGraph *solutionGraph, + double solutionPosition, double solutionValue, const QFont& font); + +private: + // Solves for the polynomial coefficients. + void solve(const QVector& positions, const QVector& values); + + // Calculates the value of the polynomial at x. + // Params will be cast to a PolynomialFit*. + static double polynomialFunction(double x, void *params); + + // Polynomial degree. + int degree; + // The data values. + QVector x, y; + // The solved polynomial coefficients. + std::vector coefficients; +}; +} diff --git a/kstars/ekos/focus/polynomialfit.cpp b/kstars/ekos/focus/polynomialfit.cpp new file mode 100644 --- /dev/null +++ b/kstars/ekos/focus/polynomialfit.cpp @@ -0,0 +1,118 @@ +#include "polynomialfit.h" + +#include "ekos/ekos.h" +#include +#include +#include +#include + +namespace Ekos +{ + +PolynomialFit::PolynomialFit(int degree_, const QVector& x_, const QVector& y_) + : degree(degree_), x(x_), y(y_) +{ + solve(x, y); +} + +void PolynomialFit::solve(const QVector& x, const QVector& y) +{ + double chisq = 0; + coefficients = gsl_polynomial_fit(x.data(), y.data(), x.count(), degree, chisq); +} + +double PolynomialFit::polynomialFunction(double x, void *params) +{ + PolynomialFit *instance = static_cast(params); + + if (instance && !instance->coefficients.empty()) + { + const int order = instance->coefficients.size() - 1; + double sum = 0; + for (int i = 0; i <= order; ++i) + sum += instance->coefficients[i] * pow(x, i); + return sum; + } + return -1; +} + +bool PolynomialFit::findMinimum(double expected, double minPosition, double maxPosition, double *position, double *value) +{ + int status; + int iter = 0, max_iter = 100; + const gsl_min_fminimizer_type *T; + gsl_min_fminimizer *s; + double m = expected; + + gsl_function F; + F.function = &PolynomialFit::polynomialFunction; + F.params = this; + + // Must turn off error handler or it aborts on error + gsl_set_error_handler_off(); + + T = gsl_min_fminimizer_brent; + s = gsl_min_fminimizer_alloc(T); + status = gsl_min_fminimizer_set(s, &F, expected, minPosition, maxPosition); + + if (status != GSL_SUCCESS) + { + qCWarning(KSTARS_EKOS_FOCUS) << "Focus GSL error:" << gsl_strerror(status); + return false; + } + + do + { + iter++; + status = gsl_min_fminimizer_iterate(s); + + m = gsl_min_fminimizer_x_minimum(s); + minPosition = gsl_min_fminimizer_x_lower(s); + maxPosition = gsl_min_fminimizer_x_upper(s); + + status = gsl_min_test_interval(minPosition, maxPosition, 0.01, 0.0); + + if (status == GSL_SUCCESS) + { + *position = m; + *value = polynomialFunction(m, this); + } + } + while (status == GSL_CONTINUE && iter < max_iter); + + gsl_min_fminimizer_free(s); + return (status == GSL_SUCCESS); +} + +void PolynomialFit::drawPolynomial(QCustomPlot *plot, QCPGraph *graph) +{ + graph->data()->clear(); + QCPRange range = plot->xAxis->range(); + double interval = range.size() / 20.0; + + for(double x = range.lower ; x < range.upper ; x += interval) + { + double y = polynomialFunction(x, this); + graph->addData(x, y); + } + plot->replot(); +} + +void PolynomialFit::drawMinimum(QCustomPlot *plot, QCPGraph *solutionGraph, + double solutionPosition, double solutionValue, const QFont& font) +{ + solutionGraph->data()->clear(); + solutionGraph->addData(solutionPosition, solutionValue); + QCPItemText *textLabel = new QCPItemText(plot); + textLabel->setPositionAlignment(Qt::AlignVCenter | Qt::AlignHCenter); + textLabel->position->setType(QCPItemPosition::ptPlotCoords); + textLabel->position->setCoords(solutionPosition, solutionValue / 2); + textLabel->setColor(Qt::red); + textLabel->setPadding(QMargins(0, 0, 0, 0)); + textLabel->setBrush(Qt::white); + textLabel->setPen(Qt::NoPen); + textLabel->setFont(QFont(font.family(), 8)); + textLabel->setText(QString::number(solutionPosition)); +} +} // namespace +