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