Commit 56387966 authored by Jean-Luc Pons's avatar Jean-Luc Pons

Added polynomial for Q,S and multi BPM geometry handling

parent 84cd7b48
...@@ -184,10 +184,10 @@ double *DDThread::getQ(int *length) { ...@@ -184,10 +184,10 @@ double *DDThread::getQ(int *length) {
GET_VALUES(q); GET_VALUES(q);
} }
double *DDThread::getH(int *length) { double *DDThread::getH(int *length) {
GET_VALUES(x); GET_VALUES(h);
} }
double *DDThread::getV(int *length) { double *DDThread::getV(int *length) {
GET_VALUES(z); GET_VALUES(v);
} }
// ---------------------------------------------------------------------------------------- // ----------------------------------------------------------------------------------------
......
This diff is collapsed.
...@@ -55,8 +55,8 @@ typedef struct { ...@@ -55,8 +55,8 @@ typedef struct {
double id; double id;
double qd; double qd;
double sum; double sum;
double x; double h;
double z; double v;
double q; double q;
} BP; } BP;
...@@ -71,6 +71,21 @@ typedef struct { ...@@ -71,6 +71,21 @@ typedef struct {
} ADC; } ADC;
// Polynomial item
typedef struct {
int nX; // power of X
int nY; // power of Y
double c; // Coefficient
} POLY;
// Maximum number of POLYNOMIAL SET
#define MAX_POLYNOMIAL_SET 5
// Maximum polynomial power
#define MAX_POLYNOMIAL_POWER 8
// --------------------------------------------------------------------------- // ---------------------------------------------------------------------------
// MCI stuff // MCI stuff
// --------------------------------------------------------------------------- // ---------------------------------------------------------------------------
...@@ -109,9 +124,6 @@ class LiberaSparkSRBPM; ...@@ -109,9 +124,6 @@ class LiberaSparkSRBPM;
#include <IQThread.h> #include <IQThread.h>
#include <ADCThread.h> #include <ADCThread.h>
// Maximum number of POLYNOMIAL SET
#define MAX_POLYNOMIAL_SET 5
/*----- PROTECTED REGION END -----*/ // LiberaSparkSRBPM.h /*----- PROTECTED REGION END -----*/ // LiberaSparkSRBPM.h
/** /**
...@@ -180,9 +192,6 @@ public: ...@@ -180,9 +192,6 @@ public:
isig::SignalSourceSharedPtr tbtdecSignal; isig::SignalSourceSharedPtr tbtdecSignal;
isig::SignalSourceSharedPtr tbtiqdecSignal; isig::SignalSourceSharedPtr tbtiqdecSignal;
uint32_t KX;
uint32_t KY;
double A0Temp; double A0Temp;
double A1Temp; double A1Temp;
...@@ -204,8 +213,11 @@ private: ...@@ -204,8 +213,11 @@ private:
PosCalculation *posCalculation; PosCalculation *posCalculation;
vector<double> hBpmCoefs[MAX_POLYNOMIAL_SET]; vector<POLY> hBpmCoefs[MAX_POLYNOMIAL_SET];
vector<double> vBpmCoefs[MAX_POLYNOMIAL_SET]; vector<POLY> vBpmCoefs[MAX_POLYNOMIAL_SET];
vector<POLY> qBpmCoefs[MAX_POLYNOMIAL_SET];
vector<POLY> sBpmCoefs[MAX_POLYNOMIAL_SET];
int nbPolynomialSet; int nbPolynomialSet;
void StartSA(); void StartSA();
...@@ -216,6 +228,8 @@ private: ...@@ -216,6 +228,8 @@ private:
void RestartADC(); void RestartADC();
void LoadPolynomial(); void LoadPolynomial();
void Disconnect(); void Disconnect();
void parsePolynomial(string polyName,vector<string>& in,vector<POLY>& poly);
void split(vector<string> &tokens, const string &text, char sep);
template<typename lType,typename tType> template<typename lType,typename tType>
void READ_ATTR(Tango::Attribute &attr,tType *value,string name,double conv = 0.0); void READ_ATTR(Tango::Attribute &attr,tType *value,string name,double conv = 0.0);
...@@ -239,6 +253,14 @@ public: ...@@ -239,6 +253,14 @@ public:
Tango::DevLong aGCMinADC; Tango::DevLong aGCMinADC;
// AGCMaxADC: Maximum ADC value for Auto Gain Control // AGCMaxADC: Maximum ADC value for Auto Gain Control
Tango::DevLong aGCMaxADC; Tango::DevLong aGCMaxADC;
// KH: Horizontal K value (in nm)
Tango::DevDouble kH;
// KV: Vertical K value (in nm)
Tango::DevDouble kV;
// BPM_Set_List: List of BPM set name
vector<string> bPM_Set_List;
// BPM_Geometry: BPM geometry (Large or Small)
string bPM_Geometry;
// Attribute data members // Attribute data members
public: public:
......
...@@ -13,6 +13,22 @@ ...@@ -13,6 +13,22 @@
<type xsi:type="pogoDsl:IntType"/> <type xsi:type="pogoDsl:IntType"/>
<status abstract="false" inherited="false" concrete="true" concreteHere="true"/> <status abstract="false" inherited="false" concrete="true" concreteHere="true"/>
</deviceProperties> </deviceProperties>
<deviceProperties name="KH" description="Horizontal K value (in nm)">
<type xsi:type="pogoDsl:DoubleType"/>
<status abstract="false" inherited="false" concrete="true" concreteHere="true"/>
</deviceProperties>
<deviceProperties name="KV" description="Vertical K value (in nm)">
<type xsi:type="pogoDsl:DoubleType"/>
<status abstract="false" inherited="false" concrete="true" concreteHere="true"/>
</deviceProperties>
<deviceProperties name="BPM_Set_List" description="List of BPM set name">
<type xsi:type="pogoDsl:StringVectorType"/>
<status abstract="false" inherited="false" concrete="true" concreteHere="true"/>
</deviceProperties>
<deviceProperties name="BPM_Geometry" description="BPM geometry (Large or Small)">
<type xsi:type="pogoDsl:StringType"/>
<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="1000" isDynamic="false"> <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="1000" isDynamic="false">
<argin description="none"> <argin description="none">
<type xsi:type="pogoDsl:VoidType"/> <type xsi:type="pogoDsl:VoidType"/>
......
...@@ -283,6 +283,58 @@ void LiberaSparkSRBPMClass::set_default_property() ...@@ -283,6 +283,58 @@ void LiberaSparkSRBPMClass::set_default_property()
} }
else else
add_wiz_dev_prop(prop_name, prop_desc); add_wiz_dev_prop(prop_name, prop_desc);
prop_name = "KH";
prop_desc = "Horizontal K value (in nm)";
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 = "KV";
prop_desc = "Vertical K value (in nm)";
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 = "BPM_Set_List";
prop_desc = "List of BPM set name";
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 = "BPM_Geometry";
prop_desc = "BPM geometry (Large or Small)";
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);
} }
//-------------------------------------------------------- //--------------------------------------------------------
......
...@@ -23,43 +23,24 @@ namespace LiberaSparkSRBPM_ns { ...@@ -23,43 +23,24 @@ namespace LiberaSparkSRBPM_ns {
// -------------------------------------------------------------------------------------- // --------------------------------------------------------------------------------------
bool PosCalculation::CheckPolynomial(vector<double> &coefs,string &error) { #define SEARCH_MAX(poly) \
for(int i=0;i<(int)poly.size();i++) { \
double delta; if(poly[i].nX>maxXPow) maxXPow = poly[i].nX; \
delta = 1 + 4.0 * 2.0 * (double)coefs.size(); if(poly[i].nY>maxYPow) maxYPow = poly[i].nY; \
unsigned int order = (unsigned int)((sqrt(delta)-1) / 2.0);
// Check order
if( order > MAX_ORDER ) {
error = "Polynomial order too large (MAX=" + std::to_string(MAX_ORDER) + ")";
return false;
}
int nbCoef = ((order-1)*order)/2 + order;
if( nbCoef != (int)coefs.size() ) {
error = "Wrong coefficient number for polynomial, got " + std::to_string(coefs.size()) +
", " + std::to_string(nbCoef) + " expected.";
return false;
}
return true;
} }
void PosCalculation::SetPolynomial(vector<double> &hCoefs,vector<double> &vCoefs) { void PosCalculation::SetPolynomial(vector<POLY>& hPoly,vector<POLY>& vPoly,vector<POLY>& qPoly,vector<POLY>& sPoly) {
hPolyCoefs = hCoefs;
vPolyCoefs = vCoefs;
// Compute polynomial orders n (v and h orders are n+1)
// i[0] * X^0Y^0 + i[1] * X^0Y^1 + ... + i[n] * X0Y^n + i[n+1] * X^1Y^0 + ... + i[2n] * X^1Y^n + i[2n+1] * X^2Y^0 + ...
double delta; this->hPoly = hPoly;
delta = 1 + 4.0 * 2.0 * (double)hPolyCoefs.size(); this->vPoly = vPoly;
hOrder = (unsigned int)((sqrt(delta)-1) / 2.0); this->qPoly = qPoly;
delta = 1 + 4.0 * 2.0 * (double)vPolyCoefs.size(); this->sPoly = sPoly;
vOrder = (unsigned int)((sqrt(delta)-1) / 2.0); maxXPow = 0;
maxYPow = 0;
SEARCH_MAX(hPoly);
SEARCH_MAX(vPoly);
SEARCH_MAX(qPoly);
SEARCH_MAX(sPoly);
} }
...@@ -99,8 +80,6 @@ PosCalculation::PosCalculation(LiberaSparkSRBPM *ds) { ...@@ -99,8 +80,6 @@ PosCalculation::PosCalculation(LiberaSparkSRBPM *ds) {
kB=1.0; kB=1.0;
kC=1.0; kC=1.0;
kD=1.0; kD=1.0;
hOrder = 0;
vOrder = 0;
// Rotation matrix // Rotation matrix
tiltAngle = 0.0; tiltAngle = 0.0;
r11 = 1.0; r12 = 0.0; r11 = 1.0; r12 = 0.0;
...@@ -129,15 +108,19 @@ void PosCalculation::ComputeDDPosition(BP &p) { ...@@ -129,15 +108,19 @@ void PosCalculation::ComputeDDPosition(BP &p) {
} }
// -------------------------------------------------------------------------------------- // --------------------------------------------------------------------------------------
#define EVAL_POLY(poly) \
sum = 0.0; \
for (size_t j(0); j < poly.size(); j++) \
sum += px[poly[j].nX] * py[poly[j].nY] * poly[j].c;
void PosCalculation::ComputePosition(BP &p) { void PosCalculation::ComputePosition(BP &p) {
// Inputs are in nm // Inputs are in nm
double KX = ds->KX; double KH = ds->kH;
double KZ = ds->KY; double KV = ds->kV;
double OX = (ds->attr_HOffset_read[0] + ds->attr_HOffsetFine_read[0])*1e9; double KQ = (ds->kH + ds->kV) / 2.0;
double OZ = (ds->attr_VOffset_read[0] + ds->attr_VOffsetFine_read[0])*1e9; double OH = (ds->attr_HOffset_read[0] + ds->attr_HOffsetFine_read[0])*1e9;
double OQ = ds->attr_VOffset_read[0]*1e9; double OV = (ds->attr_VOffset_read[0] + ds->attr_VOffsetFine_read[0])*1e9;
p.va *= kA; p.va *= kA;
p.vb *= kB; p.vb *= kB;
...@@ -152,8 +135,8 @@ void PosCalculation::ComputePosition(BP &p) { ...@@ -152,8 +135,8 @@ void PosCalculation::ComputePosition(BP &p) {
p.sum = a + b + c + d; p.sum = a + b + c + d;
if (p.sum < sumSAThreshold) { if (p.sum < sumSAThreshold) {
p.x = NAN; p.h = NAN;
p.z = NAN; p.v = NAN;
p.q = NAN; p.q = NAN;
return; return;
} }
...@@ -164,62 +147,44 @@ void PosCalculation::ComputePosition(BP &p) { ...@@ -164,62 +147,44 @@ void PosCalculation::ComputePosition(BP &p) {
case DOS_CALC: { case DOS_CALC: {
// Pos in m // Pos in m
p.x = (KX * (((a + d) - (b + c)) / p.sum) + OX)*1e-9; p.h = (KH * (((a + d) - (b + c)) / p.sum) + OH)*1e-9;
p.z = (KZ * (((a + b) - (c + d)) / p.sum) + OZ)*1e-9; p.v = (KV * (((a + b) - (c + d)) / p.sum) + OV)*1e-9;
p.q = (KX * (((a + c) - (b + d)) / p.sum) + OQ)*1e-9; p.q = (KQ * (((a + c) - (b + d)) / p.sum) )*1e-9;
} }
break; break;
// Compute position according to polynomial transform // Compute position according to polynomial transform
case POLYNOMIAL_CALC: { case POLYNOMIAL_CALC: {
unsigned int maxOrder = (hOrder > vOrder) ? hOrder : vOrder;
if(maxOrder==0) {
// No polynomial diefined
p.x = NAN;
p.z = NAN;
p.q = NAN;
return;
}
double iS = 1.0 / (a + b + c + d); double iS = 1.0 / (a + b + c + d);
double X = (a - b - c + d) * iS; double X = (a - b - c + d) * iS;
double Y = (a + b - c - d) * iS; double Y = (a + b - c - d) * iS;
double q = (a - b + c - d) * iS;
double sum; double sum;
double powx = 1.0; double powx = 1.0;
double powy = 1.0; double powy = 1.0;
for (size_t j(0); j < maxOrder; j++) { for (int j(0); j <= maxXPow; j++) {
px[j] = powx; px[j] = powx;
py[j] = powy;
powx *= X; powx *= X;
}
for (int j(0); j <= maxYPow; j++) {
py[j] = powy;
powy *= Y; powy *= Y;
} }
sum = 0.0; EVAL_POLY(hPoly);
unsigned int cf = 0; p.h = (1e6*sum + OH)*1e-9;
for (size_t x(0); x < hOrder; x++) {
for (size_t y(0); y < hOrder - x; y++) { EVAL_POLY(vPoly);
sum += px[x] * py[y] * hPolyCoefs[cf]; p.v = (1e6*sum + OV)*1e-9;
cf++;
} EVAL_POLY(qPoly);
} p.q = KQ*(q-sum)*1e-9;
p.x = (sum + OX)*1e-9;
sum = 0.0;
cf = 0;
for (size_t x(0); x < vOrder; x++) {
for (size_t y(0); y < vOrder - x; y++) {
sum += px[x] * py[y] * vPolyCoefs[cf];
cf++;
}
}
p.z = (sum + OZ)*1e-9;
// TODO polynomial for Q EVAL_POLY(sPoly);
p.q = (KX * (((p.va + p.vc) - (p.vb + p.vd)) / p.sum) + OQ)*1e-9; p.sum = p.sum / sum;
} }
break; break;
...@@ -227,10 +192,10 @@ void PosCalculation::ComputePosition(BP &p) { ...@@ -227,10 +192,10 @@ void PosCalculation::ComputePosition(BP &p) {
} }
// Tilt rotation // Tilt rotation
double rx = r11 * p.x + r12 * p.z; double rh = r11 * p.h + r12 * p.v;
double rz = r21 * p.x + r22 * p.z; double rv = r21 * p.h + r22 * p.v;
p.x = rx; p.h = rh;
p.z = rz; p.v = rv;
} }
......
...@@ -21,10 +21,9 @@ ...@@ -21,10 +21,9 @@
#define DOS_CALC 0 #define DOS_CALC 0
#define POLYNOMIAL_CALC 1 #define POLYNOMIAL_CALC 1
#define MAX_ORDER 15 // Maximum polynomial order
namespace LiberaSparkSRBPM_ns { namespace LiberaSparkSRBPM_ns {
class PosCalculation { class PosCalculation {
public: public:
...@@ -35,8 +34,7 @@ public: ...@@ -35,8 +34,7 @@ public:
void SetAlgorithm(int algo); void SetAlgorithm(int algo);
int GetAlgorithm(); int GetAlgorithm();
void SetTiltAngle(double angle); void SetTiltAngle(double angle);
bool CheckPolynomial(vector<double> &hCoefs,string &error); void SetPolynomial(vector<POLY>& hPoly,vector<POLY>& vPoly,vector<POLY>& qPoly,vector<POLY>& sPoly);
void SetPolynomial(vector<double> &hCoefs,vector<double> &vCoefs);
double kA; double kA;
double kB; double kB;
...@@ -51,12 +49,15 @@ private: ...@@ -51,12 +49,15 @@ private:
int calculationAlgorithm; int calculationAlgorithm;
// Polynomial stuff // Polynomial stuff
vector<double> hPolyCoefs; vector<POLY> hPoly;
vector<double> vPolyCoefs; vector<POLY> vPoly;
unsigned int hOrder; vector<POLY> qPoly;
unsigned int vOrder; vector<POLY> sPoly;
double px[MAX_ORDER];
double py[MAX_ORDER]; double px[MAX_POLYNOMIAL_POWER];
double py[MAX_POLYNOMIAL_POWER];
int maxXPow;
int maxYPow;
double r11,r12,r21,r22; double r11,r12,r21,r22;
......
...@@ -44,8 +44,8 @@ SAThread::SAThread(LiberaSparkSRBPM *libera, PosCalculation *pc, omni_mutex &m): ...@@ -44,8 +44,8 @@ SAThread::SAThread(LiberaSparkSRBPM *libera, PosCalculation *pc, omni_mutex &m):
currentPos.vc = NAN; currentPos.vc = NAN;
currentPos.vd = NAN; currentPos.vd = NAN;
currentPos.sum = NAN; currentPos.sum = NAN;
currentPos.x = NAN; currentPos.h = NAN;
currentPos.z = NAN; currentPos.v = NAN;
currentPos.q = NAN; currentPos.q = NAN;
HStdDev = NAN; HStdDev = NAN;
VStdDev = NAN; VStdDev = NAN;
...@@ -175,11 +175,11 @@ void SAThread::computeAveraging() { ...@@ -175,11 +175,11 @@ void SAThread::computeAveraging() {
double sumx = 0; double sumx = 0;
double sumz = 0; double sumz = 0;
for(int i=0;i<length;i++) { for(int i=0;i<length;i++) {
sumx += saValues[hSize-(i+1)].x; sumx += saValues[hSize-(i+1)].h;
sumz += saValues[hSize-(i+1)].z; sumz += saValues[hSize-(i+1)].v;
} }
currentPos.x = sumx / (double)length; currentPos.h = sumx / (double)length;
currentPos.z = sumz / (double)length; currentPos.v = sumz / (double)length;
} }
...@@ -200,10 +200,10 @@ void SAThread::computeStats() { ...@@ -200,10 +200,10 @@ void SAThread::computeStats() {
double sumx2 = 0; double sumx2 = 0;
double sumz2 = 0; double sumz2 = 0;
for(int i=0;i<length;i++) { for(int i=0;i<length;i++) {
sumx += saValues[hSize-(i+1)].x; sumx += saValues[hSize-(i+1)].h;
sumz += saValues[hSize-(i+1)].z; sumz += saValues[hSize-(i+1)].v;
sumx2 += SQUARE(saValues[hSize-(i+1)].x); sumx2 += SQUARE(saValues[hSize-(i+1)].h);
sumz2 += SQUARE(saValues[hSize-(i+1)].z); sumz2 += SQUARE(saValues[hSize-(i+1)].v);
} }
double avgx = sumx / (double)length; double avgx = sumx / (double)length;
...@@ -241,13 +241,13 @@ else \ ...@@ -241,13 +241,13 @@ else \
double SAThread::GetX() { double SAThread::GetX() {
GET_VAL(currentPos.x); GET_VAL(currentPos.h);
} }
double SAThread::GetZ() { double SAThread::GetZ() {
GET_VAL(currentPos.z); GET_VAL(currentPos.v);
} }
...@@ -381,11 +381,11 @@ void SAThread::getSumHistory(double *hist,int *length) { ...@@ -381,11 +381,11 @@ void SAThread::getSumHistory(double *hist,int *length) {
} }
void SAThread::getXHistory(double *hist,int *length) { void SAThread::getXHistory(double *hist,int *length) {
GET_HIST(x); GET_HIST(h);
} }
void SAThread::getZHistory(double *hist,int *length) { void SAThread::getZHistory(double *hist,int *length) {
GET_HIST(z); GET_HIST(v);
} }
void SAThread::getQHistory(double *hist,int *length) { void SAThread::getQHistory(double *hist,int *length) {
......
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