diff --git a/.gitignore b/.gitignore
index bb28ed2a500d1a66dcd5533dda30739567cfdffb..89fc1c383e6c16c1b753c2f79fd0382343436026 100644
--- a/.gitignore
+++ b/.gitignore
@@ -3,3 +3,4 @@ MSG.txt
ken-pespccdstage-srv_mag_pesp.txt
pespccdstage-srv_mag_pesp.txt
simul
+src/Stage2Filter.cpp.save
diff --git a/src/PespCCDStage.cpp b/src/PespCCDStage.cpp
index aaf8b9b47fbe5247d7abcd1d6065f4975894bcdd..a764e1aa14bdfe24df6a29162c9a4b221394f3eb 100644
--- a/src/PespCCDStage.cpp
+++ b/src/PespCCDStage.cpp
@@ -354,7 +354,12 @@ typedef struct {
o(1)= yagCenterNL[3];
y[1] = o;
- pstage = new PespStage_ns::PrestoStage(rot, q, offset);
+ itpp::vec __cr;
+ __cr.set_size(2,false);
+ __cr(0) = rotationCenterXZ[0];
+ __cr(1) = rotationCenterXZ[1];
+
+ pstage = new PespStage_ns::PrestoStage(__cr, rot, q, offset, rpQpDistance);
if (pstage) {
*attr_StageType_read = Tango::string_dup("Presto");
((PespStage_ns::PrestoStage *)pstage)->setyagsNL(y);
@@ -380,26 +385,6 @@ typedef struct {
pstage = NULL;
*attr_StageType_read = Tango::string_dup("Error");
}
-
- //===============================================================
- // - - - - FILTERS
- // 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 )
- #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;
-
- //===============================================================
} else {
pstage = NULL;
string s("Error :?");
@@ -407,6 +392,23 @@ typedef struct {
s.append("?");
*attr_StageType_read = Tango::string_dup(s.c_str());
}
+
+ //===============================================================
+ // - - - - FILTERS
+ // FF
+ //
+ //p_stage2flt = new Stage2Filter(std::string dev, axis_nick, itpp::vec R /*pto in xOz*/, double df);
+
+ if (pstage != NULL){
+ if (filterRpDistance > 0 )
+
+ p_stage2flt = new PespStage_ns::Stage2Filter(this, filterSelectorDevice[0], filterSelectorDevice[1], filterRpDistance);
+ else
+ p_stage2flt = NULL;
+
+ }
+ //===============================================================
+
if (pstage == NULL){
init_ok = false;
}
@@ -652,6 +654,8 @@ void PespCCDStage::get_device_property()
dev_prop.push_back(Tango::DbDatum("YagsCenterOffsets"));
dev_prop.push_back(Tango::DbDatum("RotationCenterXZ"));
dev_prop.push_back(Tango::DbDatum("FilterRpDistance"));
+ dev_prop.push_back(Tango::DbDatum("FilterSelectorDevice"));
+ dev_prop.push_back(Tango::DbDatum("RpQpDistance"));
// is there at least one property to be read ?
if (dev_prop.size()>0)
@@ -786,6 +790,28 @@ void PespCCDStage::get_device_property()
// And try to extract FilterRpDistance value from database
if (dev_prop[i].is_empty()==false) dev_prop[i] >> filterRpDistance;
+ // Try to initialize FilterSelectorDevice from class property
+ cl_prop = ds_class->get_class_property(dev_prop[++i].name);
+ if (cl_prop.is_empty()==false) cl_prop >> filterSelectorDevice;
+ else {
+ // Try to initialize FilterSelectorDevice from default device value
+ def_prop = ds_class->get_default_device_property(dev_prop[i].name);
+ if (def_prop.is_empty()==false) def_prop >> filterSelectorDevice;
+ }
+ // And try to extract FilterSelectorDevice value from database
+ if (dev_prop[i].is_empty()==false) dev_prop[i] >> filterSelectorDevice;
+
+ // Try to initialize RpQpDistance from class property
+ cl_prop = ds_class->get_class_property(dev_prop[++i].name);
+ if (cl_prop.is_empty()==false) cl_prop >> rpQpDistance;
+ else {
+ // Try to initialize RpQpDistance from default device value
+ def_prop = ds_class->get_default_device_property(dev_prop[i].name);
+ if (def_prop.is_empty()==false) def_prop >> rpQpDistance;
+ }
+ // And try to extract RpQpDistance value from database
+ if (dev_prop[i].is_empty()==false) dev_prop[i] >> rpQpDistance;
+
}
/*----- PROTECTED REGION ID(PespCCDStage::get_device_property_after) ENABLED START -----*/
diff --git a/src/PespCCDStage.h b/src/PespCCDStage.h
index 20ad96be17778f5d00c389338fcf6a36c2090258..6b41f5ca649fe243ed1cc3d364b05c9085420d20 100644
--- a/src/PespCCDStage.h
+++ b/src/PespCCDStage.h
@@ -155,6 +155,11 @@ public:
vector<Tango::DevDouble> rotationCenterXZ;
// FilterRpDistance: Distance between filter axis and rotation point Rp (near ccd)
Tango::DevDouble filterRpDistance;
+ // FilterSelectorDevice: selector device proxy
+ // continuos position attribute
+ vector<string> filterSelectorDevice;
+ // RpQpDistance: Distanza R` (o R2) - Q` (lungo asse l)
+ Tango::DevDouble rpQpDistance;
bool mandatoryNotDefined;
diff --git a/src/PespCCDStage.xmi b/src/PespCCDStage.xmi
index 0afac97c8a83dbe2f241dece11cbce84fac95a55..f3ed463957a64b24376fef82b40b6a90e6ea525e 100644
--- a/src/PespCCDStage.xmi
+++ b/src/PespCCDStage.xmi
@@ -46,6 +46,14 @@
<status abstract="false" inherited="false" concrete="true" concreteHere="true"/>
<DefaultPropValue>-1</DefaultPropValue>
</deviceProperties>
+ <deviceProperties name="FilterSelectorDevice" description="selector device proxy
continuos position attribute">
+ <type xsi:type="pogoDsl:StringVectorType"/>
+ <status abstract="false" inherited="false" concrete="true" concreteHere="true"/>
+ </deviceProperties>
+ <deviceProperties name="RpQpDistance" description="Distanza R` (o R2) - Q` (lungo asse l)">
+ <type xsi:type="pogoDsl:DoubleType"/>
+ <status abstract="false" inherited="false" concrete="true" concreteHere="true"/>
+ </deviceProperties>
<commands name="State" description="This command gets the device state (stored in its device_state data member) and returns it to the caller." execMethod="dev_state" displayLevel="OPERATOR" polledPeriod="0">
<argin description="none">
<type xsi:type="pogoDsl:VoidType"/>
diff --git a/src/PespCCDStageClass.cpp b/src/PespCCDStageClass.cpp
index 7515b5d8b0385a4b1ee67991d4cf1f1a98347f0c..e11bd897181873dc54e649d7e4d3f453515d4141 100644
--- a/src/PespCCDStageClass.cpp
+++ b/src/PespCCDStageClass.cpp
@@ -414,6 +414,32 @@ void PespCCDStageClass::set_default_property()
}
else
add_wiz_dev_prop(prop_name, prop_desc);
+ prop_name = "FilterSelectorDevice";
+ prop_desc = "selector device proxy\ncontinuos position attribute";
+ prop_def = "";
+ vect_data.clear();
+ if (prop_def.length()>0)
+ {
+ Tango::DbDatum data(prop_name);
+ data << vect_data ;
+ dev_def_prop.push_back(data);
+ add_wiz_dev_prop(prop_name, prop_desc, prop_def);
+ }
+ else
+ add_wiz_dev_prop(prop_name, prop_desc);
+ prop_name = "RpQpDistance";
+ prop_desc = "Distanza R` (o R2) - Q` (lungo asse l)";
+ prop_def = "";
+ vect_data.clear();
+ if (prop_def.length()>0)
+ {
+ Tango::DbDatum data(prop_name);
+ data << vect_data ;
+ dev_def_prop.push_back(data);
+ add_wiz_dev_prop(prop_name, prop_desc, prop_def);
+ }
+ else
+ add_wiz_dev_prop(prop_name, prop_desc);
}
//--------------------------------------------------------
diff --git a/src/PespStage.h b/src/PespStage.h
index 5926f494c08ab777d9f5d62d08ffcc863433c29f..4ca22cb5726d4957749538657bc965b2ccef27c8 100644
--- a/src/PespStage.h
+++ b/src/PespStage.h
@@ -13,15 +13,43 @@
* PespStage class description:
*
*/
+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
+
+ std::cout << ">>>>>------- " << __func__ << " rettap= " << rettap;
+ std::cout << std::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
{
+typedef enum {
+ UNKNOWN,
+ PRESTO,
+ TARDI
+} stageType_t;
+
//=======================================================================
//
//
class PespStage {
protected:
+ stageType_t _stagetype;
itpp::vec _chamber;
double _pitch;
@@ -38,6 +66,12 @@ public:
void set_chamber(itpp::vec chamber) { _chamber = chamber;};
void set_pitch(double pitch) { _pitch = pitch;};
+ itpp::mat get_rot() { return _rot; };
+ itpp::vec get_q() { return _q; };
+
+ itpp::vec get_cr() { return _cr; };
+ stageType_t get_stagetype() { return _stagetype; };
+
virtual itpp::vec calculateXZ2NL(itpp::vec p)=0;
virtual itpp::vec calculateNL2XZ(itpp::vec ccd)=0;
@@ -70,8 +104,10 @@ class PrestoStage : public PespStage {
private:
std::vector<itpp::vec> _yagsNL;
int _k;
+
+ double _rpQpDistance;
public:
- PrestoStage(itpp::mat rot, itpp::vec q, itpp::vec offset) : PespStage(rot, q, offset) { _yagsNL.resize(2); };
+ PrestoStage(itpp::vec __cr, itpp::mat rot, itpp::vec q, itpp::vec offset, double rpQpDistance) : PespStage(rot, q, offset) { _stagetype = PRESTO; _yagsNL.resize(2); _rpQpDistance = rpQpDistance; set_rotcenter(__cr); };
itpp::vec calculateXZ2NL(itpp::vec p);
itpp::vec calculateNL2XZ(itpp::vec ccd);
@@ -79,6 +115,7 @@ public:
itpp::vec calculateBR2XZ(itpp::vec p);
itpp::vec calculateXZ2BR(itpp::vec Focus);
+ void set_rotcenter(itpp::vec cr); // in xOz
void setyagsNL(std::vector<itpp::vec> y) { _yagsNL = y; };
void set_yag_NL(itpp::vec d) { _offset = d; };
@@ -90,6 +127,8 @@ public:
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)
+
+ itpp::vec R2(itpp::vec Focus);
};
//=======================================================================
@@ -101,7 +140,7 @@ private:
double _d;
int _k;
public:
- TardiStage(itpp::vec __cr, itpp::mat rot, itpp::vec q, itpp::vec offset) : PespStage(rot, q, offset) { set_rotcenter(__cr); _d = 0; };
+ TardiStage(itpp::vec __cr, itpp::mat rot, itpp::vec q, itpp::vec offset) : PespStage(rot, q, offset) { _stagetype = TARDI; set_rotcenter(__cr); _d = 0; };
itpp::vec NL2XZ(itpp::vec p);
itpp::vec XZ2NL(itpp::vec p);
diff --git a/src/PrestoStage.cpp b/src/PrestoStage.cpp
index e3d8857786e339186888a7d2c1dcc4c4fbbc3432..b45e506e4ef060b1b7d4d15679fec7dfae045fa4 100644
--- a/src/PrestoStage.cpp
+++ b/src/PrestoStage.cpp
@@ -4,6 +4,20 @@
namespace PespStage_ns
{
+/**********************************************************
+*
+*
+**********************************************************/
+void PrestoStage::set_rotcenter(itpp::vec cr){
+ #ifdef _DBG_ROTCENTER
+ std::cout << "PrestoStage::set_rotcenter(" << cr(0) << "," << cr(1) << ") in xOz" << std::endl;
+ #endif
+
+ _cr = cr;
+
+}
+
+
//BBB
itpp::vec PrestoStage::calculateNL2XZ(itpp::vec ccd){
@@ -194,17 +208,12 @@ double m, x1, z1, x2, z2;
return ris;
}
-/**********************************************************
-*
-* retta parametrica dell'asse del tubo/naso
-*
-**********************************************************/
-itpp::mat PrestoStage::rettapTubo_xOz(itpp::vec Focus){
-
- // da Focus a P (Q') P, Q' ed R gia' sulla stessa retta.
+itpp::vec PrestoStage::R2(itpp::vec Focus){
+ // da Focus a Q' (asse/centro croce) a R2 a retta tubo = R-R2
+ // da Focus Q' ... centro/asse della croce ce porta il prisma
#ifdef _DBG_PRESTO_FUNCS
- std::cout << "------- " << __func__ << " ";
+ std::cout << "------- " << __func__ << " START";
std::cout << std::endl;
#endif
@@ -218,23 +227,39 @@ itpp::mat PrestoStage::rettapTubo_xOz(itpp::vec Focus){
std::cout << std::endl;
#endif
- // da Q' a R'
+ itpp::vec r2(2); //in nQl
+ r2(0) = qp(0); //stessa coordinata n
+ r2(1) = qp(1) + _rpQpDistance; // coordinata l
+
+ itpp::vec r2_xOz(2); //in xOz
+ r2_xOz = _rot * r2 + _q;
+
+ #ifdef _DBG_PRESTO_FUNCS
+ std::cout << "------- " << __func__ << " ";
+ std::cout << "R2=" << r2 << " [nQl]";
+ std::cout << " .. " << r2_xOz << " [xOz]";
+ std::cout << std::endl;
+ #endif
+
+ return r2_xOz;
+}
- 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 parametrica dell'asse del tubo/naso
+*
+**********************************************************/
+itpp::mat PrestoStage::rettapTubo_xOz(itpp::vec Focus){
- // retta R - P(Q')
+ itpp::vec r2_xOz(2);
- itpp::mat rettap_rrp(2,2);
- rettap_rrp = rettap_xOz(rp);
+ r2_xOz = R2(Focus);
- // return retta
+ itpp::mat rettap_rr2(2,2);
+ rettap_rr2 = rettap_xOz(r2_xOz);
- return rettap_rrp;
+ return rettap_rr2;
}
diff --git a/src/Stage2Filter-correctPRESTO.cpp b/src/Stage2Filter-correctPRESTO.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2c125af51ec57781e2f40cb5e4fa85c810b4c8bf
--- /dev/null
+++ b/src/Stage2Filter-correctPRESTO.cpp
@@ -0,0 +1,153 @@
+#include "Stage2Filter.h"
+
+#include "PespCCDStage.h"
+
+
+namespace PespStage_ns
+{
+
+double Stage2Filter::correct_position_PRESTO(itpp::vec Focus){
+
+ double newpos = nan("");
+ PespStage_ns::PrestoStage* presto = (PespStage_ns::PrestoStage*)_ccdstage->pstage;
+
+
+ itpp::vec r2_xOz(2); //in xOz
+ r2_xOz = presto->R2(Focus);
+
+ #ifdef _DBG_STAGE2FILTER
+ std::cout << "------- " << __func__ << " ";
+ std::cout << "R2=" << r2_xOz << " [xOz]";
+ std::cout << std::endl;
+ #endif
+
+ itpp::mat asse_tubo = presto->rettapTubo_xOz(Focus); // asse R->R2
+
+ #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: R2=" << (asse_tubo * vt) << endl;
+ }
+ if (0){
+ 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
+ double dbase = presto->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 = R2]";
+ 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 = presto->rettap_orto(asse_tubo, i1);
+
+ // da Focus a retta fascio
+ itpp::mat retta_fascio = presto->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 = presto->intercetta(retta_fascio, asse_filter);
+
+ // D2 = distanza tra asse tubo e I2, ovvero distanza tra I1 e I2
+ double d2 = presto->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
+ //!!
+ //!! pero' d2, distanza e' sempre positiva,
+ //!! mentre devo spostare in positivo o negativo
+ //!! ovvero devo capire se I2 sta' al di la' o al di qua' dell'asse tubo
+ //!! I2(x) - I1(x) < 0 allora d2<0
+ //
+ if ( (i2(0) - i1(0)) < 0 ) d2 *= (-1.0);
+ newpos = _sel.nominal_pos[_sel.selector].pos + d2;
+
+ #ifdef _DBG_STAGE2FILTER
+ cout << ">>>>> " << __func__ << " newpos=" << newpos << endl;
+ #endif
+
+ #ifdef _DBG_STAGE2FILTER
+ cout << __func__ << " ... search within discrete positions ...." << endl;
+ #endif
+
+ //
+ // 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 << " on selector #:" << _sel.selector << endl;
+ #endif
+ }
+
+
+ #ifdef _DBG_STAGE2FILTER
+ std::cout << std::endl;
+ std::cout << "------- " << __func__ << " END";
+ std::cout << std::endl;
+ std::cout << std::endl;
+ std::cout << std::endl;
+ #endif
+ return newpos;
+}
+
+
+} // namespace
diff --git a/src/Stage2Filter-correctTARDI.cpp b/src/Stage2Filter-correctTARDI.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3c2b2571dbfc6075d6ebd40e81ad334b6d19dab7
--- /dev/null
+++ b/src/Stage2Filter-correctTARDI.cpp
@@ -0,0 +1,148 @@
+#include "Stage2Filter.h"
+
+#include "PespCCDStage.h"
+
+namespace PespStage_ns
+{
+
+
+double Stage2Filter::correct_position_TARDI(itpp::vec Focus){
+
+ double newpos = nan("");
+
+ // da Focus ad asse tubo
+
+ // retta R-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
+ //
+ // NOTA: Per Tardi va bene Q' (secondo punto di rotazione del tubo oltre a R)
+ // Per Presto Q' non va bene, bisogna calcolare R' (ovvero il
+ // secondo punto di rotazione del tubo oltre a R): questo e'
+ // fatto all'interno di rettapTubo_xOz(Focus), ovvero retta R-R'
+ //
+ //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 = R' ... =Q' per Tardi]";
+ 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
+ //!!
+ //!! pero' d2, distanza e' sempre positiva,
+ //!! mentre devo spostare in positivo o negativo
+ //!! ovvero devo capire se I2 sta' al di la' o al di qua' dell'asse tubo
+ //!! I2(x) - I1(x) < 0 allora d2<0
+ //
+ if ( (i2(0) - i1(0)) < 0 ) d2 *= (-1.0);
+ newpos = _sel.nominal_pos[_sel.selector].pos + d2;
+
+ #ifdef _DBG_STAGE2FILTER
+ cout << ">>>>> " << __func__ << " newpos=" << newpos << endl;
+ #endif
+
+ #ifdef _DBG_STAGE2FILTER
+ cout << __func__ << " ... search within discrete positions ...." << endl;
+ #endif
+
+ //
+ // 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 << " on selector #:" << _sel.selector << endl;
+ #endif
+ }
+
+ return newpos;
+}
+
+
+} // namespace
diff --git a/src/Stage2Filter.cpp b/src/Stage2Filter.cpp
index 33ce271c37b1d87f3e68301c48788ac2e4ab7fdb..1586050f08c2214d93a8d106aab9e921d83006c5 100644
--- a/src/Stage2Filter.cpp
+++ b/src/Stage2Filter.cpp
@@ -2,32 +2,19 @@
#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){
+//Stage2Filter::Stage2Filter(PespCCDStage_ns::PespCCDStage *ccdstage, std::string dev, std::string axis_nick, itpp::vec R /*pto in xOz*/, double df){
+Stage2Filter::Stage2Filter(PespCCDStage_ns::PespCCDStage *ccdstage, std::string dev, std::string axis_nick, double df){
+
+ itpp::vec R(2); /*pto in xOz*/
+ R = ccdstage->pstage->get_cr();
+
#ifdef _DBG_STAGE2FILTER
+ std::cout << std::endl;
+ std::cout << std::endl;
std::cout << " ------:: " << __func__ << " :: ctor(";
std::cout << "dev=" << dev << " ";
std::cout << "axis_nick=" << axis_nick << " ";
@@ -169,162 +156,51 @@ bool Stage2Filter::correct_position(itpp::vec Focus){
bool retval = false;
#ifdef _DBG_STAGE2FILTER
- cout << "Entering " << __func__ << " ..." << endl;
+ std::cout << std::endl;
+ cout << "Entering " << __func__;
+ cout << " for stage: [" << _ccdstage->pstage->get_stagetype();
+ cout << "]: " << _ccdstage->stageType;
+ cout << " ..." << endl;
cout << ">>>>> " << __func__ << " Focus=" << Focus << endl;
#endif
- // da Focus ad asse tubo
-
- // retta R-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
- //
- // NOTA: Per Tardi va bene Q' (secondo punto di rotazione del tubo oltre a R)
- // Per Presto Q' non va bene, bisogna calcolare R' (ovvero il
- // secondo punto di rotazione del tubo oltre a R): questo e'
- // fatto all'interno di rettapTubo_xOz(Focus), ovvero retta R-R'
- //
- //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 = R' ... =Q' per Tardi]";
- 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
- //!!
- //!! pero' d2, distanza e' sempre positiva,
- //!! mentre devo spostare in positivo o negativo
- //!! ovvero devo capire se I2 sta' al di la' o al di qua' dell'asse tubo
- //!! I2(x) - I1(x) < 0 allora d2<0
- //
- if ( (i2(0) - i1(0)) < 0 ) d2 *= (-1.0);
- double newpos = _sel.nominal_pos[_sel.selector].pos + d2;
-
- #ifdef _DBG_STAGE2FILTER
- cout << ">>>>> " << __func__ << " newpos=" << newpos << endl;
- #endif
-
- #ifdef _DBG_STAGE2FILTER
- cout << __func__ << " ... search within discrete positions ...." << endl;
- #endif
-
- //
- // 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 << " on selector #:" << _sel.selector << endl;
- #endif
+ double newpos = nan("");
+
+ switch (_ccdstage->pstage->get_stagetype()){
+ case PRESTO:
+ newpos = correct_position_PRESTO(Focus);
+ break;
+ case TARDI:
+ newpos = correct_position_TARDI(Focus);
+ break;
+ default:
+ break;
+ } //end of switch (_ccdstage->pstage->get_stagetype())
+
+ if (!isnan(newpos)){
+ Tango::DeviceAttribute attr;
+ attr.set_name(_sel.move_attr);
+
+ attr << newpos;
+
+ 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
+ }
}
- Tango::DeviceAttribute attr;
- attr.set_name(_sel.move_attr);
-
- attr << newpos;
-
- 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;
+ std::cout << std::endl;
#endif
return retval;
diff --git a/src/Stage2Filter.h b/src/Stage2Filter.h
index 17f2151a39c302eacedfafee4e7e5409eb4ab8c9..d4e7438f4adc45e75e3c5f04dcfe8975b70365f1 100644
--- a/src/Stage2Filter.h
+++ b/src/Stage2Filter.h
@@ -51,7 +51,8 @@ private:
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);
+ //Stage2Filter(PespCCDStage_ns::PespCCDStage *ccdstage, std::string dev, std::string axis_nick, itpp::vec R /*pto in xOz*/, double df);
+ Stage2Filter(PespCCDStage_ns::PespCCDStage *ccdstage, std::string dev, std::string axis_nick, double df);
bool enable() { return _enable; };
void enable(bool v) { _enable = v; };
@@ -59,6 +60,8 @@ public:
bool update();
bool correct_position(itpp::vec Focus);
+ double correct_position_PRESTO(itpp::vec Focus);
+ double correct_position_TARDI(itpp::vec Focus);
Tango::DevShort currSelector() { return _sel.selector; };
Tango::DevState stateSelector() { return _sel.state; };