From d99838dd41520c3b5de2c87eedfe253b54b54e26 Mon Sep 17 00:00:00 2001
From: Alessandro Abrami <alessandro.abrami@elettra.eu>
Date: Wed, 8 Sep 2021 15:55:11 +0200
Subject: [PATCH] 2021/09/08: BRANCH(withFilters): modificato codice per
gestire i casi in cui la quota calcolata cade fuori dai range del selector:
al caso prende i limiti del range corrente.
---
src/Stage2Filter.cpp | 42 +++++++++---------------------------------
1 file changed, 9 insertions(+), 33 deletions(-)
diff --git a/src/Stage2Filter.cpp b/src/Stage2Filter.cpp
index ca37098..e062b99 100644
--- a/src/Stage2Filter.cpp
+++ b/src/Stage2Filter.cpp
@@ -283,42 +283,18 @@ bool Stage2Filter::correct_position(itpp::vec Focus){
#ifdef _DBG_STAGE2FILTER
cout << __func__ << " ... search within discrete positions ...." << endl;
#endif
- vector<nominal_pos_t>::iterator it;
- bool newpos_in_discrete = false;
- for ( it = _sel.nominal_pos.begin(); it != _sel.nominal_pos.end(); ++it){
- #ifdef _DBG_STAGE2FILTER
- std::cout << " ------:: " << __func__ << " ";
- std::cout << (*it).pos << " " << (*it).delta;
- std::cout << std::endl;
- #endif
-
- if ( fabs(newpos - (*it).pos) <= (*it).delta ) {
- #ifdef _DBG_STAGE2FILTER
- cout << __func__ << " ... FOUND discrete position .... for " << newpos << endl;
- #endif
-
- newpos_in_discrete = true;
- break;
- }
- }
- if ( !newpos_in_discrete ) {
- #ifdef _DBG_STAGE2FILTER
- cout << __func__ << " ... search approx discrete position ...." << endl;
- #endif
-
- vector<double> dist;
- vector<double> target;
- for ( it = _sel.nominal_pos.begin(); it != _sel.nominal_pos.end(); ++it){
- dist.push_back(fabs(newpos - (*it).pos));
- target.push_back((*it).pos);
- }
-
- int minElementIndex = std::min_element(dist.begin(),dist.end()) - dist.begin();
- newpos = target[minElementIndex];
+ //
+ // Codice di sotto: cosi' non si 'esce' dal selector corrente:
+ //
+ if ( fabs(d2) > fabs(_sel.nominal_pos[_sel.selector].delta) ) {
+ double v1 = fabs(newpos - (_sel.nominal_pos[_sel.selector].pos + _sel.nominal_pos[_sel.selector].delta) );
+ double v2 = fabs(newpos - (_sel.nominal_pos[_sel.selector].pos - _sel.nominal_pos[_sel.selector].delta) );
+ if ( v1 < v2 ) newpos = _sel.nominal_pos[_sel.selector].pos + 0.99 * _sel.nominal_pos[_sel.selector].delta;
+ else newpos = _sel.nominal_pos[_sel.selector].pos - 0.99 * _sel.nominal_pos[_sel.selector].delta;
#ifdef _DBG_STAGE2FILTER
- cout << __func__ << " ... NEW approx discrete position ....: " << newpos << endl;
+ cout << __func__ << " ... NEW approx discrete position ....: " << newpos << " on selector #:" << _sel.selector << endl;
#endif
}
--
GitLab