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

@@ -2,3 +2,4 @@ MYNOTES.txt
 MSG.txt
 ken-pespccdstage-srv_mag_pesp.txt
 pespccdstage-srv_mag_pesp.txt
+simul

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

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;
 	}
 }
 //-----------------------------------------------------------------------------

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

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);
 

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

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;
 }
 
 

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
 
 /**********************************************************
 *