diff --git a/kstars/CMakeLists.txt b/kstars/CMakeLists.txt --- a/kstars/CMakeLists.txt +++ b/kstars/CMakeLists.txt @@ -172,6 +172,7 @@ # Focus ekos/focus/focus.cpp + ekos/focus/focusalgorithms.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 @@ -22,6 +22,9 @@ namespace Ekos { + +class FocusAlgorithmInterface; + /** * @class Focus * @short Supports manual focusing and auto focusing using relative and absolute INDI focusers. @@ -48,7 +51,7 @@ typedef enum { FOCUS_NONE, FOCUS_IN, FOCUS_OUT } FocusDirection; typedef enum { FOCUS_MANUAL, FOCUS_AUTO } FocusType; - typedef enum { FOCUS_ITERATIVE, FOCUS_POLYNOMIAL } FocusAlgorithm; + typedef enum { FOCUS_ITERATIVE, FOCUS_POLYNOMIAL, FOCUS_LINEAR } FocusAlgorithm; /** @defgroup FocusDBusInterface Ekos DBus Interface - Focus Module * Ekos::Focus interface provides advanced scripting capabilities to perform manual and automatic focusing operations. @@ -412,6 +415,9 @@ void initView(); + // Move the focuser in (negative) or out (positive amount). + bool changeFocus(int amount); + /** * @brief syncTrackingBoxPosition Sync the tracking box to the current selected star center */ @@ -601,5 +607,8 @@ // Filter Manager QSharedPointer filterManager; + + // Experimental linear focuser. + std::unique_ptr linearFocuser; }; } 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 @@ -10,6 +10,7 @@ #include "focus.h" #include "focusadaptor.h" +#include "focusalgorithms.h" #include "kstars.h" #include "kstarsdata.h" #include "Options.h" @@ -660,6 +661,24 @@ KSNotification::event(QLatin1String("FocusStarted"), i18n("Autofocus operation started")); + if (focusAlgorithm == FOCUS_LINEAR && (canAbsMove || canRelMove)) + { + const int position = static_cast(currentPosition); + FocusAlgorithmInterface::FocusParams params( + maxTravelIN->value(), stepIN->value(), position, absMotionMin, absMotionMax, + MAXIMUM_ABS_ITERATIONS, toleranceIN->value() / 100.0); + linearFocuser.reset(MakeLinearFocuser(params)); + const int newPosition = linearFocuser->initialPosition(); + if (newPosition != position) + { + if (!changeFocus(newPosition - position)) { + abort(); + setAutoFocusResult(false); + } + // Avoid the capture below. + return; + } + } capture(); } @@ -874,45 +893,21 @@ bool Focus::focusIn(int ms) { - if (currentFocuser == nullptr) - return false; - - if (currentFocuser->isConnected() == false) - { - appendLogText(i18n("Error: Lost connection to Focuser.")); - return false; - } - if (ms == -1) ms = stepIN->value(); - - qCDebug(KSTARS_EKOS_FOCUS) << "Focus in (" << ms << ")"; - - lastFocusDirection = FOCUS_IN; - - currentFocuser->focusIn(); - - if (canAbsMove) - { - currentFocuser->moveAbs(currentPosition - ms); - appendLogText(i18n("Focusing inward by %1 steps...", ms)); - } - else if (canRelMove) - { - currentFocuser->moveRel(ms); - appendLogText(i18n("Focusing inward by %1 steps...", ms)); - } - else - { - currentFocuser->moveByTimer(ms); - appendLogText(i18n("Focusing inward by %1 ms...", ms)); - } - - return true; + return changeFocus(-ms); } bool Focus::focusOut(int ms) { + if (ms == -1) + ms = stepIN->value(); + return changeFocus(ms); +} + +// If amount > 0 we focus out, otherwise in. +bool Focus::changeFocus(int amount) +{ if (currentFocuser == nullptr) return false; @@ -922,29 +917,32 @@ return false; } - lastFocusDirection = FOCUS_OUT; - - if (ms == -1) - ms = stepIN->value(); - - qCDebug(KSTARS_EKOS_FOCUS) << "Focus out (" << ms << ")"; + const int absAmount = abs(amount); + const bool focusingOut = amount > 0; + const QString dirStr = focusingOut ? "outward" : "inward"; + lastFocusDirection = focusingOut ? FOCUS_OUT : FOCUS_IN; - currentFocuser->focusOut(); + qCDebug(KSTARS_EKOS_FOCUS) << "Focus " << dirStr << " (" << absAmount << ")"; + if (focusingOut) + currentFocuser->focusOut(); + else + currentFocuser->focusIn(); + if (canAbsMove) { - currentFocuser->moveAbs(currentPosition + ms); - appendLogText(i18n("Focusing outward by %1 steps...", ms)); + currentFocuser->moveAbs(currentPosition + amount); + appendLogText(i18n("Focusing %2 by %1 steps...", absAmount, dirStr)); } else if (canRelMove) { - currentFocuser->moveRel(ms); - appendLogText(i18n("Focusing outward by %1 steps...", ms)); + currentFocuser->moveRel(amount); + appendLogText(i18n("Focusing %2 by %1 steps...", absAmount, dirStr)); } else { - currentFocuser->moveByTimer(ms); - appendLogText(i18n("Focusing outward by %1 ms...", ms)); + currentFocuser->moveByTimer(amount); + appendLogText(i18n("Focusing %2 by %1 ms...", absAmount, dirStr)); } return true; @@ -1530,11 +1528,15 @@ { v_graph->setData(hfr_position, hfr_value); + double minHFRVal = currentHFR / 2.5; + if (hfr_value.size() > 0) + minHFRVal = std::max(0, static_cast(0.9 * *std::min_element(hfr_value.begin(), hfr_value.end()))); + if (inFocusLoop == false && (canAbsMove || canRelMove)) { //HFRPlot->xAxis->setLabel(i18n("Position")); HFRPlot->xAxis->setRange(minPos - pulseDuration, maxPos + pulseDuration); - HFRPlot->yAxis->setRange(currentHFR / 2.5, maxHFR); + HFRPlot->yAxis->setRange(minHFRVal, maxHFR); } else { @@ -1664,6 +1666,37 @@ drawHFRPlot(); + if (focusAlgorithm == FOCUS_LINEAR) { + const int nextPosition = linearFocuser->newMeasurement(currentPosition, currentHFR); + if (nextPosition == -1) + { + if (linearFocuser->isDone() && linearFocuser->solution() != -1) + { + appendLogText(i18n("Autofocus complete after %1 iterations.", hfr_position.count())); + stop(); + setAutoFocusResult(true); + } + else + { + qCDebug(KSTARS_EKOS_FOCUS) << linearFocuser->doneReason(); + appendLogText("Linear autofocus algorithm aborted."); + abort(); + setAutoFocusResult(false); + } + return; + } + else + { + delta = nextPosition - currentPosition; + if (!changeFocus(static_cast(delta))) + { + abort(); + setAutoFocusResult(false); + } + return; + } + } + switch (lastFocusDirection) { case FOCUS_NONE: @@ -1675,7 +1708,7 @@ HFRInc = 0; focusOutLimit = 0; focusInLimit = 0; - if (focusOut(pulseDuration) == false) + if (!changeFocus(pulseDuration)) { abort(); setAutoFocusResult(false); @@ -1970,16 +2003,8 @@ delta = limitedDelta; } - qCDebug(KSTARS_EKOS_FOCUS) << "Focusing " << ((delta < 0) ? "IN" : "OUT"); - // Now cross your fingers and wait - bool rc = false; - if (delta > 0) - rc = focusOut(delta); - else - rc = focusIn(fabs(delta)); - - if (rc == false) + if (!changeFocus(delta)) { abort(); setAutoFocusResult(false); @@ -2044,7 +2069,7 @@ case FOCUS_NONE: lastHFR = currentHFR; minHFR = 1e6; - focusIn(pulseDuration); + changeFocus(-pulseDuration); break; case FOCUS_IN: @@ -2064,10 +2089,7 @@ minHFR = currentHFR; lastHFR = currentHFR; - if (lastFocusDirection == FOCUS_IN) - focusIn(pulseDuration); - else - focusOut(pulseDuration); + changeFocus(lastFocusDirection == FOCUS_IN ? -pulseDuration : pulseDuration); HFRInc = 0; } else @@ -2080,14 +2102,7 @@ pulseDuration *= 0.75; - bool rc = false; - - if (lastFocusDirection == FOCUS_IN) - rc = focusOut(pulseDuration); - else - rc = focusIn(pulseDuration); - - if (rc == false) + if (!changeFocus(lastFocusDirection == FOCUS_IN ? pulseDuration : -pulseDuration)) { abort(); setAutoFocusResult(false); @@ -2776,10 +2791,7 @@ else relativeOffset = value - currentPosition; - if (relativeOffset > 0) - focusOut(relativeOffset); - else - focusIn(abs(relativeOffset)); + changeFocus(relativeOffset); } void Focus::toggleFocusingWidgetFullScreen() diff --git a/kstars/ekos/focus/focus.ui b/kstars/ekos/focus/focus.ui --- a/kstars/ekos/focus/focus.ui +++ b/kstars/ekos/focus/focus.ui @@ -6,7 +6,7 @@ 0 0 - 702 + 790 468 @@ -774,7 +774,7 @@ QTabWidget::Rounded - 0 + 1 @@ -1191,6 +1191,7 @@ <ul> <li><b>Iterative</b>: Moves focuser by discreet steps initially decided by the step size. Once a curve slope is calculated, further step sizes are calculated to reach optimal solution. The algorithm stops when the measured HFR is within percentage tolerance of the minimum HFR recorded in the procedure.</li> <li><b>Polynomial</b>: Starts with iterative method. Upon crossing to the other side of the V-Curve, polynomial fitting coefficients along with possible minimum solution are calculated. This algorithm can be faster than purely iterative approach given a good data set.</li> +<li><b>Linear</b>: Samples focus inward in a regular fashion, using 2 passes. May be slower, but hopefully resilient to backlash. Should start with focuser positioned near good focus. Set InitialStepSize and MaxTravel for the desired sampling interval and range around start focus position. Tolerance shouldn't be too tight, e.g. use 5%.</li> </ul> </body></html> @@ -1204,6 +1205,11 @@ Polynomial + + + Linear (Experimental) + + @@ -1311,7 +1317,7 @@ 3 - + 3 diff --git a/kstars/ekos/focus/focusalgorithms.h b/kstars/ekos/focus/focusalgorithms.h new file mode 100644 --- /dev/null +++ b/kstars/ekos/focus/focusalgorithms.h @@ -0,0 +1,85 @@ +/* Ekos Focus 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 + +namespace Ekos +{ + + /** + * @class FocusAlgorithmInterface + * @short Interface intender for autofocus algorithms. + * + * @author Hy Murveit + * @version 1.0 + */ +class FocusAlgorithmInterface +{ +public: + struct FocusParams { + // Maximum movement from current position allowed for the algorithm. + int maxTravel; + // Initial sampling interval for the algorithm. + int initialStepSize; + // Current absolute position of the focuser. + int currentPosition; + // Minimum position the focuser is allowed to reach. + int minPositionAllowed; + // Maximum position the focuser is allowed to reach. + int maxPositionAllowed; + // Maximum number of iterations (captures) the focuser may try. + int maxIterations; + // 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, + int _minPositionAllowed, int _maxPositionAllowed, + int _maxIterations, double _focusTolerance) : + maxTravel(_maxTravel), initialStepSize(_initialStepSize), + currentPosition(_currentPosition), minPositionAllowed(_minPositionAllowed), + maxPositionAllowed(_maxPositionAllowed), maxIterations(_maxIterations), + focusTolerance(_focusTolerance) {} + }; + + // Constructor initializes an autofocus algorithm from the input params. + FocusAlgorithmInterface(const FocusParams &_params) : params(_params) {} + 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 + // 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; } + + // Returns the best position. Should be called after isDone() returns true. + // Returns -1 if there's an error. + int solution() { return focusSolution; } + + // Returns human-readable extra error information about why the algorithm is done. + QString doneReason() { return doneString;} + + protected: + FocusParams params; + bool done = false; + int focusSolution = -1; + QString doneString; + }; + +// Creates a LinearFocuser. Caller responsible for the memory. +FocusAlgorithmInterface *MakeLinearFocuser(const FocusAlgorithmInterface::FocusParams& params); +} + diff --git a/kstars/ekos/focus/focusalgorithms.cpp b/kstars/ekos/focus/focusalgorithms.cpp new file mode 100644 --- /dev/null +++ b/kstars/ekos/focus/focusalgorithms.cpp @@ -0,0 +1,344 @@ +/* Ekos + 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. + */ + +#include "focusalgorithms.h" + +#include +#include "kstars.h" + +#include + +namespace Ekos +{ +/** + * @class LinearFocusAlgorithm + * @short Autofocus algorithm that linearly samples HFR values. + * + * @author Hy Murveit + * @version 1.0 + */ +class LinearFocusAlgorithm : public FocusAlgorithmInterface +{ +public: + + // Constructor initializes a linear autofocus algorithm, starting at some initial position, + // sampling HFR values, decreasing the position by some step size, until the algorithm believe's + // it's seen a minimum. It then tries to find that minimum in a 2nd pass. + LinearFocusAlgorithm(const FocusParams ¶ms); + + // 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 + // it has no movement request. + int initialPosition() override { return requestedPosition; } + + // Pass in the measurement for the last requested position. Returns the position for the next + // requested measurement, or -1 if the algorithm's done or if there's an error. + int newMeasurement(int position, double value) override; + +private: + + // Determines the desired focus position for the first sample. + void computeInitialPosition(); + + // Returns a score evaluating whether index is the location of a minumum in the + // HFR vector. -1 means no. A positive integer indicates yes, but can be overruled + // by another index with a better score. + int evaluateMinimum(int index, bool lastSample); + + // Sets the internal state for re-finding the minimum, and returns the requested next + // position to sample. + int setupSecondPass(int minIndex, double margin = 2.0); + + // Used in the 2nd pass. Focus is getting worse. Requires several consecutive samples getting worse. + bool gettingWorse(); + + // A vector containing the HFR values sampled by this algorithm so far. + QVector values; + // A vector containing the focus positions corresponding to the HFR values stored above. + QVector positions; + + // Focus position requested by this algorithm the previous step. + int requestedPosition; + // Number of iterations processed so far. + int numSteps; + // The best value in the first pass. The 2nd pass attempts to get within + // tolerance of this value. + double minValue; + // The sampling interval--the number of focuser steps reduced each iteration. + int stepSize; + // The minimum focus position to use. Computed from the focuser limits and maxTravel. + int minPositionLimit; + // The maximum focus position to use. Computed from the focuser limits and maxTravel. + int maxPositionLimit; + // The index of the minimum found in the first pass. + int firstPassMinIndex; + // True if the first iteration has already found a minimum. + bool minimumFound; + // True if the 2nd pass re-found the minimum, and thus the algorithm is done. + bool minimumReFound; + // True if the algorithm failed, and thus is done. + bool searchFailed; +}; + +FocusAlgorithmInterface *MakeLinearFocuser(const FocusAlgorithmInterface::FocusParams& params) +{ + return new LinearFocusAlgorithm(params); +} + +LinearFocusAlgorithm::LinearFocusAlgorithm(const FocusParams &focusParams) + : FocusAlgorithmInterface(focusParams) +{ + requestedPosition = params.currentPosition; + 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); + 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.maxPositionAllowed).arg(params.maxIterations).arg(params.focusTolerance).arg(minPositionLimit).arg(maxPositionLimit); + computeInitialPosition(); +} + +void LinearFocusAlgorithm::computeInitialPosition() +{ + const int position = params.currentPosition; + int start, end; + int halfMaxTravel = params.maxTravel / 2; + + // If the bounds allow, set the focus to half-travel above the current position + // and sample focusing in down-to half-travel below the current position. + if (position + halfMaxTravel <= maxPositionLimit && position - halfMaxTravel >= minPositionLimit) + { + start = position + halfMaxTravel; + end = position - halfMaxTravel; + } + else if (position + halfMaxTravel > maxPositionLimit) + { + // If the above hits the focus-out bound, start from the highest focus position possible + // and sample down the travel amount. + start = maxPositionLimit; + end = std::max(minPositionLimit, start - params.maxTravel); + } + else + { + // If the range above hits the focus-in bound, try to start from max-travel above the min position + // and sample down to the min position. + start = std::min(minPositionLimit + params.maxTravel, maxPositionLimit); + end = minPositionLimit; + } + + // Now that the start and end of the sampling interval is set, + // check to see if the params were reasonably set up. + // If too many steps (more than half the allotment) are required, honor stepSize over maxTravel. + const int nSteps = (start - end) / params.initialStepSize; + if (nSteps > params.maxIterations/2) + { + const int topSize = start - position; + const int bottomSize = position - end; + if (topSize <= bottomSize) + { + const int newStart = position + params.initialStepSize * (params.maxIterations/4); + start = std::min(newStart, maxPositionLimit); + end = start - params.initialStepSize * (params.maxIterations/2); + } + else + { + const int newEnd = position - params.initialStepSize * (params.maxIterations/4); + end = std::max(newEnd, minPositionLimit); + start = end + params.initialStepSize * (params.maxIterations/2); + } + } + requestedPosition = start; + qCDebug(KSTARS_EKOS_FOCUS) << QString("LinearFocuser: initialPosition %1 end %2 steps %3 sized %4") + .arg(start).arg(end).arg(start-end/params.initialStepSize).arg(params.initialStepSize); +} + +int LinearFocusAlgorithm::newMeasurement(int position, double value) +{ + ++numSteps; + qCDebug(KSTARS_EKOS_FOCUS) << QString("LinearFocuser: step %1, newMeasurement(%2, %3)").arg(numSteps).arg(position).arg(value); + + // Not sure how to get a general value for this. Skip this check? + constexpr int LINEAR_POSITION_TOLERANCE = 25; + if (abs(position - requestedPosition) > LINEAR_POSITION_TOLERANCE) + { + qCDebug(KSTARS_EKOS_FOCUS) << QString("LinearFocuser: error didn't get the requested position"); + return requestedPosition; + } + // Have we already found a solution? + if (focusSolution != -1) + { + doneString = i18n("Called newMeasurement after a solution was found."); + qCDebug(KSTARS_EKOS_FOCUS) << QString("LinearFocuser: error %1").arg(doneString); + return -1; + } + + // Store the sample values. + values.push_back(value); + positions.push_back(position); + + if (!minimumFound) + { + // The first pass. We're looking for the minimum of a v-curve. + // Check all possible indices to see if we have a V-curve minimum position, and, + // if so, which is the best. + int bestScore = -1; + int bestIndex = -1; + // Is this the last sample before we bump into a limit. + bool lastSample = requestedPosition - stepSize < minPositionLimit; + for (int i = 0; i < values.size(); ++i) { + const int score = evaluateMinimum(i, lastSample); + if (score > 0) { + minimumFound = true; + if (score > bestScore) { + bestScore = score; + bestIndex = i; + } + } + } + if (minimumFound) + { + qCDebug(KSTARS_EKOS_FOCUS) << QString("LinearFocuser: Minimum found at index %1").arg(bestIndex); + return setupSecondPass(bestIndex); + } + } + else + { + // We previously found the v-curve minimum. We're now in a 2nd pass looking to recreate it. + if (value < minValue * (1.0 + params.focusTolerance)) + { + focusSolution = position; + minimumReFound = true; + done = true; + doneString = i18n("Solution found."); + qCDebug(KSTARS_EKOS_FOCUS) << QString("LinearFocuser: solution at position %1 value %2 (best %3)").arg(position).arg(value).arg(minValue); + return -1; + } + else + { + qCDebug(KSTARS_EKOS_FOCUS) << QString("LinearFocuser: %1 %2 not a solution, not < %3").arg(position).arg(value).arg(minValue * (1.0+params.focusTolerance)); + if (gettingWorse()) + { + // Doesn't look like we'll find something close to the min. Retry the 2nd pass. + qCDebug(KSTARS_EKOS_FOCUS) << QString("LinearFocuser: getting worse, re-running 2nd pass"); + return setupSecondPass(firstPassMinIndex); + } + } + } + + if (numSteps == params.maxIterations - 1) + { + // If we're close to exceeding the iteration limit, retry this pass near the old minimum position. + const int minIndex = static_cast(std::min_element(values.begin(), values.end()) - values.begin()); + return setupSecondPass(minIndex, 0.5); + } + else if (numSteps > params.maxIterations) + { + // Fail. Exceeded our alloted number of iterations. + searchFailed = true; + done = true; + doneString = i18n("Too many steps."); + qCDebug(KSTARS_EKOS_FOCUS) << QString("LinearFocuser: error %1").arg(doneString); + return -1; + } + + // Setup the next sample. + requestedPosition = requestedPosition - stepSize; + + // Make sure the next sample is within bounds. + if (requestedPosition < minPositionLimit) + { + // The position is too low. Pick the min value and go to (or retry) a 2nd iteration. + const int minIndex = static_cast(std::min_element(values.begin(), values.end()) - values.begin()); + qCDebug(KSTARS_EKOS_FOCUS) << QString("LinearFocuser: reached end without Vmin. Restarting %1 pos %2 value %3") + .arg(minIndex).arg(positions[minIndex]).arg(values[minIndex]); + return setupSecondPass(minIndex); + } + qCDebug(KSTARS_EKOS_FOCUS) << QString("LinearFocuser: requesting position %1").arg(requestedPosition); + return requestedPosition; +} + +int LinearFocusAlgorithm::setupSecondPass(int minIndex, double margin) +{ + firstPassMinIndex = minIndex; + minimumFound = true; + + int bestPosition = positions[minIndex]; + minValue = values[minIndex]; + // Arbitrarily go back "margin" steps above the best position. + // Could be a problem if backlash were worse than that many steps. + requestedPosition = std::min(static_cast(bestPosition + stepSize * margin), maxPositionLimit); + stepSize = params.initialStepSize / 2; + qCDebug(KSTARS_EKOS_FOCUS) << QString("LinearFocuser: 2ndPass starting at %1 step %2").arg(requestedPosition).arg(stepSize); + return requestedPosition; +} + +// Given the values in the two vectors (values & positions), Evaluates whether index +// is the position of a "V". If a negative number is returned, the answer is no. +// A positive return value is a score. Best score wins. +// +// Implemented as simple heuristic. A V point must have: +// - at least 2 points on both sides that have higher values +// - for both sides: # higher values - # lower values > 2 +// Return value is the number of higher values minus the number of lower values +// counted on both sides. +// Note: could have just returned positive for only the minimum value, but this +// allows for others to be possible minima as well, if the min is too close to the edge. + +int LinearFocusAlgorithm::evaluateMinimum(int index, bool lastSample) +{ + const double indexValue = values[index]; + const int threshold = 2; + + if (index < threshold) + return -1; + if (!lastSample && index >= values.size() - threshold) + return -1; + + // Left means lower focus position (focused in), right means higher position. + int right_num_higher = 0, right_num_lower = 0; + int left_num_higher = 0, left_num_lower = 0; + for (int i = 0; i < index; ++i) + { + if (values[i] >= indexValue) ++right_num_higher; + else ++right_num_lower; + } + for (int i = index+1; i < values.size(); ++i) + { + if (values[i] >= indexValue) ++left_num_higher; + else ++left_num_lower; + } + const int right_difference = right_num_higher - right_num_lower; + const int left_difference = left_num_higher - left_num_lower; + if (right_difference >= threshold && left_difference >= threshold) + return right_difference + left_difference; + return -1; +} + +// Return true if there are "streak" consecutive values which are successively worse. +bool LinearFocusAlgorithm::gettingWorse() +{ + // Must have this many consecutive values getting worse. + constexpr int streak = 3; + const int length = values.size(); + if (length < streak+1) + return false; + for (int i = length-1; i >= length-streak; --i) + if (values[i] <= values[i-1]) + return false; + return true; +} + +} +