Commit ec7d81a0 authored by Alessandro Abrami's avatar Alessandro Abrami
Browse files

2021/09/09:

	BRANCH(withFilters): Codice per PRESTO, verificato Presto/Andor vs dati: OK. (in attesa dati Fava)
parent b78c9550
......@@ -3,3 +3,4 @@ MSG.txt
ken-pespccdstage-srv_mag_pesp.txt
pespccdstage-srv_mag_pesp.txt
simul
src/Stage2Filter.cpp.save
......@@ -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 -----*/
......
......@@ -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;
......
......@@ -46,6 +46,14 @@
<status abstract="false" inherited="false" concrete="true" concreteHere="true"/>
<DefaultPropValue>-1</DefaultPropValue>
</deviceProperties>
<deviceProperties name="FilterSelectorDevice" description="selector device proxy&#xA;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"/>
......
......@@ -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);
}
//--------------------------------------------------------
......
......@@ -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);
......
......@@ -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;
}
......
#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
#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