Commit 207b57f6 authored by Alessandro Abrami's avatar Alessandro Abrami
Browse files

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.
parent fc2567a6
......@@ -2,3 +2,4 @@ MYNOTES.txt
MSG.txt
ken-pespccdstage-srv_mag_pesp.txt
pespccdstage-srv_mag_pesp.txt
simul
......@@ -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
......
......@@ -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;
}
}
//-----------------------------------------------------------------------------
......
......@@ -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
......@@ -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);
......
......@@ -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
......
......@@ -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;
}
......
......@@ -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
/**********************************************************
*
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment