From 8f9b3a59be4b0d230f45bf82ee3dc655e69a532c Mon Sep 17 00:00:00 2001
From: Alessandro Abrami <alessandro.abrami@elettra.eu>
Date: Tue, 8 Jun 2021 08:39:29 +0200
Subject: [PATCH] 2021/06/08: BRANCH(withFilters): primo funzionamento ok.
---
.gitignore | 2 +
Makefile | 3 +
src/PespCCDStage.cpp | 224 ++++++++++++++++++++++-----------
src/PespCCDStage.h | 7 ++
src/PespStage.cpp | 93 ++++++++++++++
src/PespStage.h | 21 +++-
src/PrestoStage.cpp | 9 ++
src/Stage2Filter.cpp | 292 +++++++++++++++++++++++++++++++++++++++++++
src/Stage2Filter.h | 66 ++++++++++
src/TardiStage.cpp | 70 ++++++++++-
10 files changed, 707 insertions(+), 80 deletions(-)
create mode 100644 src/PespStage.cpp
create mode 100644 src/Stage2Filter.cpp
create mode 100644 src/Stage2Filter.h
diff --git a/.gitignore b/.gitignore
index 32ea799..a47eb36 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,2 +1,4 @@
MYNOTES.txt
MSG.txt
+ken-pespccdstage-srv_mag_pesp.txt
+pespccdstage-srv_mag_pesp.txt
diff --git a/Makefile b/Makefile
index 526f64b..1561c85 100644
--- a/Makefile
+++ b/Makefile
@@ -25,6 +25,9 @@ CXXFLAGS += -D_DBG_PARAMETERS
#CXXFLAGS += -D_WITHOUT_DIPLACEMENT
+CXXFLAGS += -D_DBG_STAGE2FILTER
+CXXFLAGS += -D_DBG_TARDI_FUNCS
+
##----------------------------------------------------------------------------
#
#
diff --git a/src/PespCCDStage.cpp b/src/PespCCDStage.cpp
index 909f63b..49143d0 100644
--- a/src/PespCCDStage.cpp
+++ b/src/PespCCDStage.cpp
@@ -182,6 +182,11 @@ void PespCCDStage::delete_device()
pstage = NULL;
}
+ if (p_stage2flt) {
+ delete p_stage2flt;
+ p_stage2flt = NULL;
+ }
+
CORBA::string_free(attr_StageType_read[0]);
@@ -371,6 +376,14 @@ typedef struct {
pstage = NULL;
*attr_StageType_read = Tango::string_dup("Error");
}
+
+ //===============================================================
+ // - - - - FILTERS
+ //
+ //p_stage2flt = new Stage2Filter(std::string dev, axis_nick, itpp::vec R /*pto in xOz*/, double df);
+
+ p_stage2flt = new PespStage_ns::Stage2Filter(this, "tango://srv-tango-padres-01:20000/pos_mag/mover1d/flt_mrc_pos_mag.01", "X", __cr, double(40));
+ //===============================================================
} else {
pstage = NULL;
string s("Error :?");
@@ -576,7 +589,6 @@ typedef struct {
first_write_YagSelector = true;
first_write_BeamOnYagDisplacement = true;
-
if (init_ok){
set_state(Tango::ON);
set_status("Ready.");
@@ -896,27 +908,34 @@ void PespCCDStage::write_FocusX(Tango::WAttribute &attr)
if (get_state() == Tango::FAULT) return;
+ itpp::vec newXZ(2);
try {
- itpp::vec newccd;
- INFO_STREAM << "PespCCDStage::write_FocusX("<<w_val <<")"<< endl;
+ itpp::vec newccd;
+ INFO_STREAM << "PespCCDStage::write_FocusX("<<w_val <<")"<< endl;
- itpp::vec newXZ(2);
- newXZ(0)=w_val;
- newXZ(1)=(*attr_FocusZ_read);
- newccd=pstage->calculateXZ2NL(newXZ);
+ newXZ(0)=w_val;
+ newXZ(1)=(*attr_FocusZ_read);
+ newccd=pstage->calculateXZ2NL(newXZ);
//AAA
- #ifndef _DONT_WRITECENTERX
- double v = newccd(0);
- Tango::DeviceAttribute daN(ccdstageN.move_attr, v);
- ccdstageN.dp->write_attribute(daN);
- v = newccd(1);
- Tango::DeviceAttribute daL(ccdstageL.move_attr, v);
- ccdstageL.dp->write_attribute(daL);
- #endif
+ #ifndef _DONT_WRITECENTERX
+ double v = newccd(0);
+ Tango::DeviceAttribute daN(ccdstageN.move_attr, v);
+ ccdstageN.dp->write_attribute(daN);
+ v = newccd(1);
+ Tango::DeviceAttribute daL(ccdstageL.move_attr, v);
+ ccdstageL.dp->write_attribute(daL);
+ #endif
+
+ } catch(...) {
+ ERROR_STREAM << "Write attribute FAILED !!!" << endl;
+ }
+
+ //FF
+ itpp::vec Focus = newXZ;
+ if (!p_stage2flt->correct_position(Focus)){
+ INFO_STREAM << "PespCCDStage::write_FocusX() error on filters"<< endl;
+ }
- } catch(...) {
- ERROR_STREAM << "Write attribute FAILED !!!" << endl;
- }
/*----- PROTECTED REGION END -----*/ // PespCCDStage::write_FocusX
}
@@ -956,27 +975,33 @@ void PespCCDStage::write_FocusZ(Tango::WAttribute &attr)
if (get_state() == Tango::FAULT) return;
+ itpp::vec newXZ(2);
try {
- itpp::vec newccd;
- INFO_STREAM << "PespCCDStage::write_FocusZ("<<w_val <<")"<< endl;
+ itpp::vec newccd;
+ INFO_STREAM << "PespCCDStage::write_FocusZ("<<w_val <<")"<< endl;
- itpp::vec newXZ(2);
- newXZ(0)=(*attr_FocusX_read);
- newXZ(1)=w_val;
+ newXZ(0)=(*attr_FocusX_read);
+ newXZ(1)=w_val;
- newccd=pstage->calculateXZ2NL(newXZ);
+ newccd=pstage->calculateXZ2NL(newXZ);
//AAA
- double v = newccd(0);
- Tango::DeviceAttribute daN(ccdstageN.move_attr, v);
- ccdstageN.dp->write_attribute(daN);
- v = newccd(1);
- Tango::DeviceAttribute daL(ccdstageL.move_attr, v);
- ccdstageL.dp->write_attribute(daL);
+ double v = newccd(0);
+ Tango::DeviceAttribute daN(ccdstageN.move_attr, v);
+ ccdstageN.dp->write_attribute(daN);
+ v = newccd(1);
+ Tango::DeviceAttribute daL(ccdstageL.move_attr, v);
+ ccdstageL.dp->write_attribute(daL);
- } catch(...) {
- ERROR_STREAM << "Write attribute FAILED !!!" << endl;
- }
+ } catch(...) {
+ ERROR_STREAM << "Write attribute FAILED !!!" << endl;
+ }
+
+ //FF
+ itpp::vec Focus = newXZ;
+ if (!p_stage2flt->correct_position(Focus)){
+ INFO_STREAM << "PespCCDStage::write_FocusZ() error on filters"<< endl;
+ }
/*----- PROTECTED REGION END -----*/ // PespCCDStage::write_FocusZ
}
@@ -1016,33 +1041,39 @@ void PespCCDStage::write_FocusDistance(Tango::WAttribute &attr)
if (get_state() == Tango::FAULT) return;
+ itpp::vec newXZ(2);
try {
- itpp::vec newccd;
- INFO_STREAM << "PespCCDStage::write_FocusDistance("<<w_val <<")"<< endl;
+ itpp::vec newccd;
+ INFO_STREAM << "PespCCDStage::write_FocusDistance("<<w_val <<")"<< endl;
- itpp::vec newXZ(2);
- itpp::vec newBR(2);
+ itpp::vec newBR(2);
- newBR(0)=(*attr_FocusAngle_read);
- newBR(1)=w_val;
+ newBR(0)=(*attr_FocusAngle_read);
+ newBR(1)=w_val;
- newXZ=pstage->calculateBR2XZ(newBR);
- newccd=pstage->calculateXZ2NL(newXZ);
+ newXZ=pstage->calculateBR2XZ(newBR);
+ newccd=pstage->calculateXZ2NL(newXZ);
//AAA
- #ifndef _DONT_WRITE_BR
- double v = newccd(0);
- Tango::DeviceAttribute daN(ccdstageN.move_attr, v);
- ccdstageN.dp->write_attribute(daN);
- v = newccd(1);
- Tango::DeviceAttribute daL(ccdstageL.move_attr, v);
- ccdstageL.dp->write_attribute(daL);
- #endif
+ #ifndef _DONT_WRITE_BR
+ double v = newccd(0);
+ Tango::DeviceAttribute daN(ccdstageN.move_attr, v);
+ ccdstageN.dp->write_attribute(daN);
+ v = newccd(1);
+ Tango::DeviceAttribute daL(ccdstageL.move_attr, v);
+ ccdstageL.dp->write_attribute(daL);
+ #endif
- } catch(...) {
- ERROR_STREAM << "Write attribute FAILED !!!" << endl;
- }
+ } catch(...) {
+ ERROR_STREAM << "Write attribute FAILED !!!" << endl;
+ }
+
+ //FF
+ itpp::vec Focus = newXZ;
+ if (!p_stage2flt->correct_position(Focus)){
+ INFO_STREAM << "PespCCDStage::write_FocusDistance() error on filters"<< endl;
+ }
/*----- PROTECTED REGION END -----*/ // PespCCDStage::write_FocusDistance
}
@@ -1082,32 +1113,38 @@ void PespCCDStage::write_FocusAngle(Tango::WAttribute &attr)
if (get_state() == Tango::FAULT) return;
+ itpp::vec newXZ(2);
try {
- itpp::vec newccd;
- INFO_STREAM << "PespCCDStage::write_FocusAngle("<<w_val <<")"<< endl;
+ itpp::vec newccd;
+ INFO_STREAM << "PespCCDStage::write_FocusAngle("<<w_val <<")"<< endl;
- itpp::vec newXZ(2);
- itpp::vec newBR(2);
+ itpp::vec newBR(2);
- newBR(0)=w_val;
- newBR(1)=(*attr_FocusDistance_read);
+ newBR(0)=w_val;
+ newBR(1)=(*attr_FocusDistance_read);
- newXZ=pstage->calculateBR2XZ(newBR);
- newccd=pstage->calculateXZ2NL(newXZ);
+ newXZ=pstage->calculateBR2XZ(newBR);
+ newccd=pstage->calculateXZ2NL(newXZ);
//AAA
- #ifndef _DONT_WRITE_BR
- double v = newccd(0);
- Tango::DeviceAttribute daN(ccdstageN.move_attr, v);
- ccdstageN.dp->write_attribute(daN);
- v = newccd(1);
- Tango::DeviceAttribute daL(ccdstageL.move_attr, v);
- ccdstageL.dp->write_attribute(daL);
- #endif
+ #ifndef _DONT_WRITE_BR
+ double v = newccd(0);
+ Tango::DeviceAttribute daN(ccdstageN.move_attr, v);
+ ccdstageN.dp->write_attribute(daN);
+ v = newccd(1);
+ Tango::DeviceAttribute daL(ccdstageL.move_attr, v);
+ ccdstageL.dp->write_attribute(daL);
+ #endif
- } catch(...) {
- ERROR_STREAM << "Write attribute FAILED !!!" << endl;
- }
+ } catch(...) {
+ ERROR_STREAM << "Write attribute FAILED !!!" << endl;
+ }
+
+ //FF
+ itpp::vec Focus = newXZ;
+ if (!p_stage2flt->correct_position(Focus)){
+ INFO_STREAM << "PespCCDStage::write_FocusDistance() error on filters"<< endl;
+ }
/*----- PROTECTED REGION END -----*/ // PespCCDStage::write_FocusAngle
}
@@ -1200,6 +1237,20 @@ void PespCCDStage::write_BeamOnYagDisplacement(Tango::WAttribute &attr)
}
first_write_BeamOnYagDisplacement = false;
+
+ //
+ // il Focus in XZ non e' cambiato, ma e' cambiato lo stage sottostante (a causa del nuovo displacement)
+ // pertanto e' cambiata anche la posizione del tubo/naso
+ // quindi richiedo il cambiamento anche se con lo stesso Focus.
+
+ //FF
+ itpp::vec Focus = oldXZ;
+ if (!p_stage2flt->correct_position(Focus)){
+ INFO_STREAM << "PespCCDStage::write_FocusDistance() error on filters"<< endl;
+ }
+
+
+
/*----- PROTECTED REGION END -----*/ // PespCCDStage::write_BeamOnYagDisplacement
}
//--------------------------------------------------------
@@ -1348,6 +1399,19 @@ void PespCCDStage::write_YagSelector(Tango::WAttribute &attr)
first_write_YagSelector = false;
+ //
+ // il Focus in XZ non e' cambiato, ma e' cambiato lo stage sottostante (a causa del nuovo yag)
+ // pertanto e' cambiata anche la posizione del tubo/naso
+ // quindi richiedo il cambiamento anche se con lo stesso Focus.
+
+ //FF
+ if (attr_UpdateOnSelector && !first_write_YagSelector){
+ itpp::vec Focus = oldXZ;
+ if (!p_stage2flt->correct_position(Focus)){
+ INFO_STREAM << "PespCCDStage::write_FocusDistance() error on filters"<< endl;
+ }
+ }
+
/*----- PROTECTED REGION END -----*/ // PespCCDStage::write_YagSelector
}
//--------------------------------------------------------
@@ -1481,6 +1545,12 @@ Tango::DevBoolean PespCCDStage::set_angle_distance(const Tango::DevVarDoubleArra
ERROR_STREAM << "PespCCDStage::set_angle_distance Write CCDStageAxisL attribute FAILED !!!" << endl;
}
+ //FF
+ itpp::vec Focus = newXZ;
+ if (!p_stage2flt->correct_position(Focus)){
+ INFO_STREAM << "PespCCDStage::write_FocusDistance() error on filters"<< endl;
+ }
+
argout = true;
/*----- PROTECTED REGION END -----*/ // PespCCDStage::set_angle_distance
@@ -1684,7 +1754,9 @@ void PespCCDStage::updateattributes(){
*attr_FocusDistance_read = br[1];
-
+ if (!p_stage2flt->update()) {
+ ERROR_STREAM << "Read filters FAILED !!!" << endl;
+ }
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
@@ -1768,5 +1840,13 @@ void PespCCDStage::updatestatus(){
}
+//FF
+//
+double PespCCDStage::filter_distance(){ // filter distance from R
+
+ return 10;
+}
+
+
/*----- PROTECTED REGION END -----*/ // PespCCDStage::namespace_ending
} // namespace
diff --git a/src/PespCCDStage.h b/src/PespCCDStage.h
index c3fdcc4..c7d77bf 100644
--- a/src/PespCCDStage.h
+++ b/src/PespCCDStage.h
@@ -60,6 +60,8 @@ typedef struct {
} mover_t;
+#include "Stage2Filter.h"
+
/*----- PROTECTED REGION END -----*/ // PespCCDStage.h
@@ -82,6 +84,8 @@ class PespCCDStage : public TANGO_BASE_CLASS
/*----- PROTECTED REGION ID(PespCCDStage::Data Members) ENABLED START -----*/
// Add your own data members
+private:
+ PespStage_ns::Stage2Filter* p_stage2flt;
public:
mover_t ccdstageN;
mover_t ccdstageL;
@@ -399,6 +403,9 @@ protected :
void updateattributes();
void updatestatus();
+public:
+ double filter_distance(); // filter distance from R
+
/*----- PROTECTED REGION END -----*/ // PespCCDStage::Additional Method prototypes
};
diff --git a/src/PespStage.cpp b/src/PespStage.cpp
new file mode 100644
index 0000000..37f213e
--- /dev/null
+++ b/src/PespStage.cpp
@@ -0,0 +1,93 @@
+#include "PespStage.h"
+
+namespace PespStage_ns
+{
+
+/********************************************************************
+*
+* retta parametrica ortogonale a 'rtta' e passante per il punto 'pto'
+*
+********************************************************************/
+itpp::mat PespStage::rettap_orto(itpp::mat rtta, itpp::vec pto) {
+ itpp::mat rmat(2,2);
+
+ double m = sqrt( pow(rtta(1,1),2) + pow(rtta(0,1),2) );
+
+ rmat(0,0) = pto(0);
+ rmat(1,0) = pto(1);
+ rmat(0,1) = -rtta(1,1)/m;
+ rmat(1,1) = rtta(0,1)/m;
+
+ return rmat;
+}
+
+/**********************************************************
+*
+* retta parametrica da O (0,0) a P in xOz
+*
+**********************************************************/
+itpp::mat PespStage::rettap_P_xOz(itpp::vec p) {
+ itpp::mat rmat(2,2);
+
+ rmat(0,0) = 0;
+ rmat(1,0) = 0;
+ rmat(0,1) = p(0);
+ rmat(1,1) = p(1);
+
+ return rmat;
+}
+
+/**********************************************************
+*
+* distanza di riferimento, ovvero tra i due punti che hanno generato la retta
+*
+**********************************************************/
+double PespStage::rettap_dBase(itpp::mat retta) {
+
+ //retta(0,0) ... primo punto asse x
+ //retta(1,0) ... primo punto asse z
+ //retta(0,1) ... distanza in x tra i due pti
+ //retta(1,1) ... distanza in z tra i due pti
+
+ double dbase = sqrt( pow(retta(1,1),2) + pow(retta(0,1),2) );
+
+ return dbase;
+}
+
+/**********************************************************
+*
+* intercetta tra due rette parametriche
+*
+**********************************************************/
+itpp::vec PespStage::intercetta(itpp::mat rettap_1, itpp::mat rettap_2){
+
+ itpp::vec r(2);
+
+ itpp::mat m = inv(rettap_2) * rettap_1;
+
+ if (m(0,1) != 0.0 ){
+ double t1 = (1 - m(0,0))/m(0,1);
+ itpp::vec t(2);
+ t(0) = 1;
+ t(1) = t1;
+ r = rettap_1 * t;
+ } else {
+ r(0) = nan("");
+ r(1) = nan("");
+ }
+
+ return r;
+}
+
+/**********************************************************
+*
+* distanza tra due pti
+*
+**********************************************************/
+double PespStage::d_p1p2(itpp::vec p1, itpp::vec p2){
+
+ return sqrt(pow(p1(0) - p2(0),2) + pow(p1(1) - p2(1),2));
+}
+
+
+} // End of namespace
diff --git a/src/PespStage.h b/src/PespStage.h
index a48f102..cd64936 100644
--- a/src/PespStage.h
+++ b/src/PespStage.h
@@ -47,6 +47,18 @@ public:
double get_displacement() { return displacement;};
void set_displacement(double v) { displacement = v;};
+ itpp::mat rettap_orto(itpp::mat rtta, itpp::vec pto);
+
+ virtual itpp::mat rettapTubo_xOz(itpp::vec Focus) = 0; // retta parametrica dell'asse del tubo/naso: da R a Focus
+
+ itpp::mat rettap_P_xOz(itpp::vec p);
+
+ double rettap_dBase(itpp::mat retta); // distanza di riferimento, ovvero tra i due punti che hanno generato la retta
+
+ itpp::vec intercetta(itpp::mat rettap_1, itpp::mat rettap_2);
+
+ double d_p1p2(itpp::vec p1, itpp::vec p2); //distanza tra due pti
+
};
//=======================================================================
@@ -70,6 +82,9 @@ public:
itpp::vec get_yag_NL() { return _offset; };
void select_yag(int k) { _k = k; _offset = _yagsNL[k]; };
int get_select_yag() { return _k; };
+
+ itpp::mat rettapTubo_xOz(itpp::vec Focus); // retta parametrica dell'asse del tubo/naso
+
};
//=======================================================================
@@ -89,8 +104,6 @@ public:
itpp::mat rettap_nQl(itpp::vec qp);
itpp::mat rettap_xOz(itpp::vec p);
- itpp::mat rettap_orto(itpp::mat rtta, itpp::vec pto);
-
itpp::vec pp2p(itpp::vec pp);
void set_rotcenter(itpp::vec cr); // in xOz
@@ -105,6 +118,10 @@ public:
itpp::vec calculateBR2XZ(itpp::vec p);
itpp::vec calculateXZ2BR(itpp::vec Focus);
+ itpp::vec Focus2Qp(itpp::vec Focus); // calc Q' da Focus (with displacement)
+
+ itpp::mat rettapTubo_xOz(itpp::vec Focus); // retta parametrica dell'asse del tubo/naso
+
};
} // End of namespace
diff --git a/src/PrestoStage.cpp b/src/PrestoStage.cpp
index da34050..0e4a9a8 100644
--- a/src/PrestoStage.cpp
+++ b/src/PrestoStage.cpp
@@ -194,6 +194,15 @@ double m, x1, z1, x2, z2;
return ris;
}
+/**********************************************************
+*
+* retta parametrica dell'asse del tubo/naso
+*
+**********************************************************/
+itpp::mat PrestoStage::rettapTubo_xOz(itpp::vec Focus){
+}
+
+
/*----- PROTECTED REGION END -----*/ // PespCCDStage::namespace_ending
} // namespace
diff --git a/src/Stage2Filter.cpp b/src/Stage2Filter.cpp
new file mode 100644
index 0000000..e313789
--- /dev/null
+++ b/src/Stage2Filter.cpp
@@ -0,0 +1,292 @@
+#include "Stage2Filter.h"
+
+#include "PespCCDStage.h"
+
+typedef struct {
+ double a; //x
+ double b; //y
+ double c; //z
+ double d; //cost.
+} rettan_t;
+
+static rettan_t retta_p2n(itpp::mat rettap){
+ rettan_t rettan;
+ rettan.b = 0; // siamo nel piano xz
+
+ cout << ">>>>>------- " << __func__ << " rettap= " << rettap;
+ cout << endl;
+
+ rettan.a = 1;
+ rettan.c = -(rettap(0,1) / rettap(1,1));
+ rettan.d = -rettap(0,0)+ (rettap(0,1) / rettap(1,1)) * rettap(1,0);
+
+ return rettan;
+}
+
+namespace PespStage_ns
+{
+
+Stage2Filter::Stage2Filter(PespCCDStage_ns::PespCCDStage *ccdstage, std::string dev, std::string axis_nick, itpp::vec R /*pto in xOz*/, double df){
+ #ifdef _DBG_STAGE2FILTER
+ std::cout << " ------:: " << __func__ << " :: ctor(";
+ std::cout << "dev=" << dev << " ";
+ std::cout << "axis_nick=" << axis_nick << " ";
+ std::cout << "R=" << R << " ";
+ std::cout << "df=" << df << " ";
+ std::cout << ")" << std::endl;
+ #endif
+
+ _ccdstage = ccdstage;
+ _sel.dev = dev;
+ _sel.axis_nickName = axis_nick;
+ _sel.move_attr = axis_nick;
+ _sel.configured = false;
+ _R = R;
+ _df = df;
+
+ _enable = false;
+
+ /* check if device is responding */
+ try {
+ _sel.dp = new Tango::DeviceProxy(_sel.dev);
+
+ #ifdef _DBG_STAGE2FILTER
+ std::cout << "Connected to device '" + _sel.dev + "'"<< std::endl;
+ #endif
+
+ float pingtime;
+ pingtime = _sel.dp->ping();
+ }
+ catch(Tango::DevFailed e) {
+ #ifdef _DBG_STAGE2FILTER
+ std::cout << "Failed to connect to device '" + _sel.dev + "'"<< std::endl;
+ #endif
+ _sel.dp = NULL;
+ return;
+ }
+
+ // get number of selectors
+ //
+
+ _sel.no_of_instruments = -1;
+
+ Tango::DeviceData dd = _sel.dp->command_inout("GetInstrumentList");
+ vector<string> vs;
+ dd >> vs;
+
+ _sel.no_of_instruments = vs.size();
+
+ vector<string>::iterator it;
+
+ #ifdef _DBG_STAGE2FILTER
+ std::cout << "len(GetInstrumentList)=" << _sel.no_of_instruments << std::endl;
+ for (it = vs.begin(); it != vs.end(); ++it)
+ std::cout << "(GetInstrumentList)=" << *it << std::endl;
+ #endif
+
+ // get nominal pos
+ //
+ Tango::DeviceData din;
+ Tango::DevVarLongStringArray vlsa;
+ vector<double> vd;
+
+ vlsa.lvalue.length(1);
+ vlsa.svalue.length(1);
+ vlsa.svalue[0] = CORBA::string_dup(_sel.axis_nickName.c_str());
+
+
+ for (int i = 0; i < _sel.no_of_instruments; i++){
+ vlsa.lvalue[0] = i;
+ din << vlsa;
+ dd = _sel.dp->command_inout("GetTPos", din);
+ cout << dd << endl;
+ dd >> vd;
+ nominal_pos_t np;
+ if ( vd.size() == 2 ){
+ np.pos = vd[0];
+ np.delta = vd[1];
+ } else {
+ np.pos = nan("");
+ np.delta = nan("");
+ }
+ _sel.nominal_pos.push_back(np);
+ }
+
+ #ifdef _DBG_STAGE2FILTER
+ {
+ vector<nominal_pos_t>::iterator it;
+ for ( it = _sel.nominal_pos.begin(); it != _sel.nominal_pos.end(); ++it){
+ std::cout << " ------:: " << __func__ << " ";
+ std::cout << (*it).pos << " " << (*it).delta;
+ std::cout << std::endl;
+ }
+
+ }
+ #endif
+
+ _sel.configured = true;
+
+ bool up = update();
+
+ #ifdef _DBG_STAGE2FILTER
+ std::cout << " ------:: " << __func__ << " ";
+ if (up) {
+ std::cout << "selector=" << _sel.selector;
+ std::cout << std::endl;
+ } else {
+ cout << "problem on selector!" << endl;
+ }
+ #endif
+
+
+}
+
+bool Stage2Filter::update(){
+
+ Tango::DeviceAttribute da;
+ Tango::DevShort selector = 0;
+
+ if (_sel.dp!= NULL) try {
+ da = _sel.dp->read_attribute("Selector");
+ da >> selector;
+ _sel.selector = selector;
+ _sel.state = _sel.dp->state();
+
+ } catch(...) {
+ cout << "Read attribute Selector FAILED !!!" << endl;
+ return false;
+ } else {
+ _sel.state = Tango::UNKNOWN;
+ return false;
+ }
+
+ return true;
+}
+
+
+bool Stage2Filter::correct_position(itpp::vec Focus){
+
+ #ifdef _DBG_STAGE2FILTER
+ cout << "Entering " << __func__ << " ..." << endl;
+ cout << ">>>>> " << __func__ << " Focus=" << Focus << endl;
+ #endif
+
+ // da Focus ad asse tubo
+
+ // retta R-Focus
+ itpp::mat asse_tubo = _ccdstage->pstage->rettapTubo_xOz(Focus);
+
+ #ifdef _DBG_STAGE2FILTER
+ cout << ">>>>> " << __func__ << " asse_tubo=" << asse_tubo << endl;
+ {
+ itpp::vec vt(2);
+ vt(0) = 1;
+ vt(1) = 0;
+ cout << ">>>>> " << __func__ << " asse_tubo: R=" << (asse_tubo * vt) << endl;
+ vt(1) = 1;
+ cout << ">>>>> " << __func__ << " asse_tubo: Q'=" << (asse_tubo * vt) << endl;
+ }
+ {
+ rettan_t r = retta_p2n(asse_tubo);
+ cout << ">>>>>------- " << __func__ << " asse_tubo= ";
+ cout << r.a << "*x + ";
+ cout << r.c << "*z + ";
+ cout << r.d ;
+ cout << endl;
+ }
+ #endif
+
+ // da retta asse tubo e
+ // df (da Q', da R la distanza CAMBIA!)
+ // => punto di intersezione (I1) asse filtro con asse tubo
+ //double df = _ccdstage->filter_distance();
+ double dbase = _ccdstage->pstage->rettap_dBase(asse_tubo);
+
+ double t = 1 - _df / dbase ;
+
+ #ifdef _DBG_STAGE2FILTER
+ cout << ">>>>> " << __func__ << " df=" << _df << " ";
+ cout << "dbase=" << dbase << " ";
+ cout << "t=1-df/dbase=" << t << " ";
+ cout << " [1 = Q']";
+ cout << endl;
+ #endif
+
+ itpp::vec vt(2);
+ vt(0) = 1;
+ vt(1) = t;
+
+ itpp::vec i1(2);
+ i1 = asse_tubo * vt;
+
+ #ifdef _DBG_STAGE2FILTER
+ cout << ">>>>> " << __func__ << " i1=" << i1 << " ";
+ cout << endl;
+ #endif
+
+ // retta orto asse tubo e passante per intersezione I1
+ itpp::mat asse_filter = _ccdstage->pstage->rettap_orto(asse_tubo, i1);
+
+ // da Focus a retta fascio
+ itpp::mat retta_fascio = _ccdstage->pstage->rettap_P_xOz(Focus);
+
+ #ifdef _DBG_STAGE2FILTER
+ cout << ">>>>> " << __func__ << " retta_fascio=" << retta_fascio << endl;
+ {
+ itpp::vec vt(2);
+ vt(0) = 1;
+ vt(1) = 0;
+ cout << ">>>>> " << __func__ << " retta_fascio: O=" << (retta_fascio * vt) << endl;
+ vt(1) = 1;
+ cout << ">>>>> " << __func__ << " retta_fascio: Focus=" << (retta_fascio * vt) << endl;
+ }
+ {
+ rettan_t r = retta_p2n(retta_fascio);
+ cout << ">>>>>------- " << __func__ << " retta_fascio= ";
+ cout << r.a << "*x + ";
+ cout << r.c << "*z + ";
+ cout << r.d ;
+ cout << endl;
+ }
+ #endif
+
+ // I2 = intersezione retta fascio e retta orto
+ itpp::vec i2 = _ccdstage->pstage->intercetta(retta_fascio, asse_filter);
+
+ // D2 = distanza tra asse tubo e I2, ovvero distanza tra I1 e I2
+ double d2 = _ccdstage->pstage->d_p1p2(i1, i2);
+
+ #ifdef _DBG_STAGE2FILTER
+ cout << ">>>>> " << __func__ << " sel_pos=" << _sel.nominal_pos[_sel.selector].pos << endl;
+ cout << ">>>>> " << __func__ << " d2=" << d2 << endl;
+ #endif
+
+
+ // sposto di D2 la posizione del filtro
+ // la direzione di d2 e' da asse tubo verso x,z >0,>0
+ // mentre l'asse filtro e inverso
+ //
+ double newpos = _sel.nominal_pos[_sel.selector].pos - d2;
+
+ #ifdef _DBG_STAGE2FILTER
+ cout << ">>>>> " << __func__ << " newpos=" << 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;
+
+
+ #ifdef _DBG_STAGE2FILTER
+ cout << "Leaving " << __func__ << " ..." << endl;
+ #endif
+
+ return true;
+}
+
+
+} // namespace
diff --git a/src/Stage2Filter.h b/src/Stage2Filter.h
new file mode 100644
index 0000000..aaa573b
--- /dev/null
+++ b/src/Stage2Filter.h
@@ -0,0 +1,66 @@
+#ifndef __STAGE2FILTER__
+#define __STAGE2FILTER__
+
+#include <tango.h>
+
+#include <itpp/itbase.h>
+#include <itpp/itcomm.h>
+#include <math.h>
+#include <vector>
+
+#define PI 3.14159265
+
+namespace PespCCDStage_ns {
+class PespCCDStage;
+}
+
+namespace PespStage_ns
+{
+
+
+typedef struct {
+ double pos;
+ double delta;
+} nominal_pos_t;
+
+typedef struct {
+ string dev;
+ string axis_nickName;
+ bool configured;
+ std::string move_attr;
+ Tango::DeviceProxy *dp;
+ Tango::DevState state;
+ Tango::DevShort selector;
+ int no_of_instruments;
+ double pos;
+ long tmo;
+ vector<nominal_pos_t> nominal_pos;
+} selector_t;
+
+
+//=======================================================================
+//
+//
+class Stage2Filter {
+
+private:
+ selector_t _sel;
+ itpp::vec _R;
+ double _df;
+ bool _enable;
+ PespCCDStage_ns::PespCCDStage* _ccdstage;
+
+public:
+ Stage2Filter(PespCCDStage_ns::PespCCDStage *ccdstage, std::string dev, std::string axis_nick, itpp::vec R /*pto in xOz*/, double df);
+
+ bool enable() { return _enable; };
+ void enable(bool v) { _enable = v; };
+
+ bool update();
+
+ bool correct_position(itpp::vec Focus);
+
+};
+
+} // namespace
+#endif /* __STAGE2FILTER__ */
diff --git a/src/TardiStage.cpp b/src/TardiStage.cpp
index 1d0c7de..0712f3b 100644
--- a/src/TardiStage.cpp
+++ b/src/TardiStage.cpp
@@ -129,6 +129,8 @@ itpp::mat TardiStage::rettap_nQl(itpp::vec qp){
}
+#if 0
+//spostata in
/**********************************************************
*
* retta parametrica ortogonale a rtta e passante per il puntp pto
@@ -146,6 +148,7 @@ itpp::mat TardiStage::rettap_orto(itpp::mat rtta, itpp::vec pto){
return rmat;
}
+#endif
/**********************************************************
@@ -344,9 +347,8 @@ itpp::vec r; //retta per centro yag e centro specchio/chamber in x,z
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
-itpp::vec TardiStage::calculateXZ2NL(itpp::vec p){
-itpp::vec ris(2);
-itpp::mat tmat(2,2);
+itpp::vec TardiStage::Focus2Qp(itpp::vec Focus){
+ itpp::mat tmat(2,2);
// ccd coord. dello stage nel suo sitema di rif.: n,l
// offset dello yag rispetto 0,0 di rif n,l
@@ -358,6 +360,16 @@ itpp::mat tmat(2,2);
// ris coord ccd coord[n,l]
//
+ #ifdef _DBG_TARDI_FUNCS
+ std::cout << "------- " << __func__ << " ";
+ #ifndef _WITHOUT_DIPLACEMENT
+ std::cout << " WITH displacement";
+ #else
+ std::cout << " WITHOUT displacement";
+ #endif
+ std::cout << std::endl;
+ #endif
+
itpp::vec _p(2);
#ifndef _WITHOUT_DIPLACEMENT
@@ -372,14 +384,14 @@ itpp::mat tmat(2,2);
// trovo qp....
if (displacement == 0.0 ){
- _p = p;
+ _p = Focus;
} else {
- itpp::vec pp(p);
+ itpp::vec pp(Focus);
_p = pp2p(pp);
}
#else /* _WITHOUT_DIPLACEMENT */
- _p = p;
+ _p = Focus;
#endif /* _WITHOUT_DIPLACEMENT */
@@ -405,6 +417,16 @@ itpp::mat tmat(2,2);
itpp::vec qp(2);
qp = rettap_xOz(_p) * vt;
+ return qp;
+}
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+itpp::vec TardiStage::calculateXZ2NL(itpp::vec p){
+ itpp::vec ris(2);
+ itpp::vec qp(2);
+
+ qp = Focus2Qp(p);
+
ris = XZ2NL(qp);
#if _DBG_CALCXZ2NL
@@ -455,5 +477,41 @@ double m, x1, z1, x2, z2;
}
+/**********************************************************
+*
+* retta parametrica dell'asse del tubo/naso
+*
+**********************************************************/
+itpp::mat TardiStage::rettapTubo_xOz(itpp::vec Focus){
+
+ // da Focus a P (Q') P, Q' ed R gia' sulla stessa retta.
+
+ #ifdef _DBG_TARDI_FUNCS
+ std::cout << "------- " << __func__ << " ";
+ std::cout << std::endl;
+ #endif
+
+ itpp::vec qp(2);
+ qp = Focus2Qp(Focus);
+
+ #ifdef _DBG_TARDI_FUNCS
+ std::cout << "------- " << __func__ << " ";
+ std::cout << "Focus=" << Focus << " ";
+ std::cout << "Q'=" << qp << " ";
+ std::cout << std::endl;
+ #endif
+
+ // retta R - P(Q')
+
+ itpp::mat rettap_rqp(2,2);
+ rettap_rqp = rettap_xOz(qp);
+
+ // return retta
+
+ return rettap_rqp;
+}
+
+
+
/*----- PROTECTED REGION END -----*/ // PespCCDStage::namespace_ending
} // namespace
--
GitLab