From 207b57f620429b997e146c80dddc9c324542730a Mon Sep 17 00:00:00 2001
From: Alessandro Abrami <alessandro.abrami@elettra.eu>
Date: Wed, 8 Sep 2021 15:23:02 +0200
Subject: [PATCH] 2021/09/08: BRANCH(withFilters): aggiunto codice per
gestire i casi in cui la quota calcolata cade fuori dai range del selector:
ancora da migliorare.
---
.gitignore | 1 +
Makefile | 2 ++
src/PespCCDStage.cpp | 18 ++++++++++---
src/PespStage.cpp | 16 ++++++++++++
src/PespStage.h | 7 +++--
src/PrestoStage.cpp | 62 ++++++++++++++++++++++++++++++++++++++++++++
src/Stage2Filter.cpp | 60 +++++++++++++++++++++++++++++++++++++++---
src/TardiStage.cpp | 2 +-
8 files changed, 159 insertions(+), 9 deletions(-)
diff --git a/.gitignore b/.gitignore
index a47eb36..bb28ed2 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@ MYNOTES.txt
MSG.txt
ken-pespccdstage-srv_mag_pesp.txt
pespccdstage-srv_mag_pesp.txt
+simul
diff --git a/Makefile b/Makefile
index 1561c85..7079713 100644
--- a/Makefile
+++ b/Makefile
@@ -3,6 +3,8 @@ NAME_SRV = pespccdstage-srv
CXXFLAGS =
LDFLAGS = -litpp
+CXXFLAGS += -D_SIMUL_
+
#CXXFLAGS += -D_DBG_INIT
CXXFLAGS += -D_DBG_PARAMETERS
#CXXFLAGS += -D_DBG_TARDISTAGE
diff --git a/src/PespCCDStage.cpp b/src/PespCCDStage.cpp
index ba2809d..5726885 100644
--- a/src/PespCCDStage.cpp
+++ b/src/PespCCDStage.cpp
@@ -385,9 +385,16 @@ typedef struct {
// FF
//
//p_stage2flt = new Stage2Filter(std::string dev, axis_nick, itpp::vec R /*pto in xOz*/, double df);
+ #ifdef _DBG_STAGE2FILTER
+ cout << "- - - - - - - !!!! filter target hardcoded !!!! - - - - -" << endl;
+ #endif
if (filterRpDistance > 0 )
- p_stage2flt = new PespStage_ns::Stage2Filter(this, "tango://srv-tango-padres-01:20000/pos_mag/mover1d/flt_mrc_pos_mag.01", "X", __cr, filterRpDistance);
+ #ifdef _SIMUL_
+ p_stage2flt = new PespStage_ns::Stage2Filter(this, "tango://srv-tango-ctrl-01:20000/sim-pos_mag/mover1d/flt_mrc_pos_mag.01", "X", __cr, filterRpDistance);
+ #else
+ p_stage2flt = new PespStage_ns::Stage2Filter(this, "tango://srv-tango-padres-01:20000/pos_mag/mover1d/flt_mrc_pos_mag.01", "X", __cr, filterRpDistance);
+ #endif
else
p_stage2flt = NULL;
//===============================================================
@@ -1817,8 +1824,13 @@ void PespCCDStage::updateattributes(){
*attr_FocusDistance_read = br[1];
- if (!p_stage2flt->update()) {
- ERROR_STREAM << "Read filters FAILED !!!" << endl;
+ if (p_stage2flt != NULL) {
+ if (!p_stage2flt->update()) {
+ ERROR_STREAM << "Read filters FAILED !!!" << endl;
+ }
+ } else {
+ cout << "p_stage2flt NULL !!!" << endl;
+ ERROR_STREAM << "p_stage2flt NULL !!!" << endl;
}
}
//-----------------------------------------------------------------------------
diff --git a/src/PespStage.cpp b/src/PespStage.cpp
index 37f213e..79f445c 100644
--- a/src/PespStage.cpp
+++ b/src/PespStage.cpp
@@ -89,5 +89,21 @@ double PespStage::d_p1p2(itpp::vec p1, itpp::vec p2){
return sqrt(pow(p1(0) - p2(0),2) + pow(p1(1) - p2(1),2));
}
+/**********************************************************
+*
+* retta parametrica da R (cr) a Q' in xOz
+*
+**********************************************************/
+itpp::mat PespStage::rettap_xOz(itpp::vec qp){
+ itpp::mat rmat(2,2);
+
+ rmat(0,0) = _cr(0);
+ rmat(1,0) = _cr(1);
+ rmat(0,1) = qp(0) - _cr(0);
+ rmat(1,1) = qp(1) - _cr(1);
+
+ return rmat;
+}
+
} // End of namespace
diff --git a/src/PespStage.h b/src/PespStage.h
index cd64936..5926f49 100644
--- a/src/PespStage.h
+++ b/src/PespStage.h
@@ -28,6 +28,7 @@ protected:
itpp::mat _rot;
itpp::vec _q;
itpp::vec _offset;
+ itpp::vec _cr; // R in xOz
double displacement;
@@ -48,6 +49,7 @@ public:
void set_displacement(double v) { displacement = v;};
itpp::mat rettap_orto(itpp::mat rtta, itpp::vec pto);
+ itpp::mat rettap_xOz(itpp::vec p);
virtual itpp::mat rettapTubo_xOz(itpp::vec Focus) = 0; // retta parametrica dell'asse del tubo/naso: da R a Focus
@@ -77,6 +79,7 @@ public:
itpp::vec calculateBR2XZ(itpp::vec p);
itpp::vec calculateXZ2BR(itpp::vec Focus);
+
void setyagsNL(std::vector<itpp::vec> y) { _yagsNL = y; };
void set_yag_NL(itpp::vec d) { _offset = d; };
itpp::vec get_yag_NL() { return _offset; };
@@ -85,6 +88,8 @@ public:
itpp::mat rettapTubo_xOz(itpp::vec Focus); // retta parametrica dell'asse del tubo/naso
+ itpp::vec Focus2Qp(itpp::vec Focus); // calc Q' da Focus (with displacement)
+ itpp::vec Focus2Qp_nQl(itpp::vec Focus); // calc Q' da Focus (with displacement)
};
//=======================================================================
@@ -92,7 +97,6 @@ public:
//
class TardiStage : public PespStage {
private:
- itpp::vec _cr; // in xOz
itpp::vec _crnl; // in nQl
double _d;
int _k;
@@ -102,7 +106,6 @@ public:
itpp::vec NL2XZ(itpp::vec p);
itpp::vec XZ2NL(itpp::vec p);
itpp::mat rettap_nQl(itpp::vec qp);
- itpp::mat rettap_xOz(itpp::vec p);
itpp::vec pp2p(itpp::vec pp);
diff --git a/src/PrestoStage.cpp b/src/PrestoStage.cpp
index 0e4a9a8..e3d8857 100644
--- a/src/PrestoStage.cpp
+++ b/src/PrestoStage.cpp
@@ -200,8 +200,70 @@ double m, x1, z1, x2, z2;
*
**********************************************************/
itpp::mat PrestoStage::rettapTubo_xOz(itpp::vec Focus){
+
+ // da Focus a P (Q') P, Q' ed R gia' sulla stessa retta.
+
+ #ifdef _DBG_PRESTO_FUNCS
+ std::cout << "------- " << __func__ << " ";
+ std::cout << std::endl;
+ #endif
+
+ itpp::vec qp(2);
+ qp = Focus2Qp_nQl(Focus);
+
+ #ifdef _DBG_PRESTO_FUNCS
+ std::cout << "------- " << __func__ << " ";
+ std::cout << "Focus=" << Focus << " [xOz] ";
+ std::cout << "Q'=" << qp << " [nQl]";
+ std::cout << std::endl;
+ #endif
+
+ // da Q' a R'
+
+ itpp::vec rp_nQl(_offset);
+ rp_nQl(0) += 100; // N <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
+ // rp_nQl(1) = ; // L stesso di P (non P')
+
+ itpp::vec rp(2);
+ rp = _rot * (qp + rp_nQl) + _q ;
+
+ // retta R - P(Q')
+
+ itpp::mat rettap_rrp(2,2);
+ rettap_rrp = rettap_xOz(rp);
+
+ // return retta
+
+ return rettap_rrp;
+
+}
+
+
+/**********************************************************
+*
+* calc Q' da Focus (with displacement)
+*
+**********************************************************/
+itpp::vec PrestoStage::Focus2Qp(itpp::vec Focus){
+itpp::vec qp(2);
+
+ itpp::vec newoffset(_offset);
+
+ #ifndef _WITHOUT_DIPLACEMENT
+ // SE si vuole introdurre il "displacement" sullo yag,
+ // allora gli offset (in particolare la coord n)
+ // deve essere modificata prima di fare il calcolo du cui sotto
+
+ newoffset(0) += -displacement;
+ #endif
+ qp = Focus - _rot * newoffset;
+
+ return qp;
}
+itpp::vec PrestoStage::Focus2Qp_nQl(itpp::vec Focus){
+ return calculateXZ2NL(Focus);
+}
/*----- PROTECTED REGION END -----*/ // PespCCDStage::namespace_ending
diff --git a/src/Stage2Filter.cpp b/src/Stage2Filter.cpp
index a78a861..ca37098 100644
--- a/src/Stage2Filter.cpp
+++ b/src/Stage2Filter.cpp
@@ -166,6 +166,8 @@ bool Stage2Filter::update(){
bool Stage2Filter::correct_position(itpp::vec Focus){
+ bool retval = false;
+
#ifdef _DBG_STAGE2FILTER
cout << "Entering " << __func__ << " ..." << endl;
cout << ">>>>> " << __func__ << " Focus=" << Focus << endl;
@@ -278,20 +280,72 @@ bool Stage2Filter::correct_position(itpp::vec Focus){
cout << ">>>>> " << __func__ << " newpos=" << newpos << endl;
#endif
+ #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];
+
+ #ifdef _DBG_STAGE2FILTER
+ cout << __func__ << " ... NEW approx discrete position ....: " << newpos << endl;
+ #endif
+ }
+
Tango::DeviceAttribute attr;
attr.set_name(_sel.move_attr);
attr << newpos;
- if (_sel.dp != NULL) _sel.dp->write_attribute(attr);
- else return false;
+ try {
+ if (_sel.dp != NULL) {
+ _sel.dp->write_attribute(attr);
+ retval = true;
+ }
+ } catch(Tango::DevFailed e) {
+ retval = false;
+
+ #ifdef _DBG_STAGE2FILTER
+ cout << "Failed to set attribute!" << endl;
+ #endif
+ }
#ifdef _DBG_STAGE2FILTER
cout << "Leaving " << __func__ << " ..." << endl;
#endif
- return true;
+ return retval;
}
diff --git a/src/TardiStage.cpp b/src/TardiStage.cpp
index 0712f3b..1894431 100644
--- a/src/TardiStage.cpp
+++ b/src/TardiStage.cpp
@@ -148,7 +148,6 @@ itpp::mat TardiStage::rettap_orto(itpp::mat rtta, itpp::vec pto){
return rmat;
}
-#endif
/**********************************************************
@@ -166,6 +165,7 @@ itpp::mat TardiStage::rettap_xOz(itpp::vec qp){
return rmat;
}
+#endif
/**********************************************************
*
--
GitLab