Skip to content

L
LiberaSparkSRBPM
Commit 56387966 authored by Jean-Luc Pons's avatar Jean-Luc Pons
Browse files
Options

Added polynomial for Q,S and multi BPM geometry handling

parent 84cd7b48
Hide whitespace changes
Inline Side-by-side
Showing with 348 additions and 155 deletions
DDThread.cpp
View file @ 56387966
......@@ -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);
}
// ----------------------------------------------------------------------------------------
......
LiberaSparkSRBPM.cpp
View file @ 56387966
......@@ -40,6 +40,7 @@
#include <LiberaSparkSRBPM.h>
#include <LiberaSparkSRBPMClass.h>
#include "LiberaSparkSRBPM.h"
#define KILL_THREAD(th) {if(th){ th->exitThread=true;th->wakeup();th->join(NULL);th=NULL;}}
......@@ -421,7 +422,26 @@ void LiberaSparkSRBPM::init_device()
attr_CalibCoeffsC_read = new Tango::DevDouble[32];
attr_CalibCoeffsD_read = new Tango::DevDouble[32];
/*----- PROTECTED REGION ID(LiberaSparkSRBPM::init_device) ENABLED START -----*/
if( kH==0 ) {
Tango::Except::throw_exception(
"ErrorInit",
"KH property not defined",
"LiberaSparkSRBPM::init_device");
}
if( kV==0 ) {
Tango::Except::throw_exception(
"ErrorInit",
"KV property not defined",
"LiberaSparkSRBPM::init_device");
}
// Connect to MCI and create signal handles
try {
......@@ -452,11 +472,14 @@ void LiberaSparkSRBPM::init_device()
mci::Node adcNode = m_root.GetNode(mci::Tokenize("application.signals.adc"));
adcSignal = mci::CreateRemoteSignal(adcNode);
// Get KX and KY
// Set KX and KY
uint32_t iValue;
mci::Node kxNode = m_root.GetNode(mci::Tokenize("application.fa_dsp.position.Kx"));
kxNode.Get(KX);
iValue = (uint32_t)kH;
kxNode.Set(iValue);
mci::Node kyNode = m_root.GetNode(mci::Tokenize("application.fa_dsp.position.Kx"));
kyNode.Get(KY);
iValue = (uint32_t)kV;
kyNode.Get(iValue);
} catch (istd::Exception e) {
......@@ -499,8 +522,8 @@ void LiberaSparkSRBPM::init_device()
self = new Tango::DeviceProxy(get_name());
attr_AGC_Enable_read[0] = false;
attr_KH_read[0] = KX;
attr_KV_read[0] = KY;
attr_KH_read[0] = kH;
attr_KV_read[0] = kV;
LoadPolynomial();
......@@ -524,6 +547,8 @@ void LiberaSparkSRBPM::get_device_property()
// Initialize property data members
aGCMinADC = 3000;
aGCMaxADC = 7000;
kH = 0;
kV = 0;
/*----- PROTECTED REGION END -----*/ // LiberaSparkSRBPM::get_device_property_before
......@@ -532,6 +557,10 @@ void LiberaSparkSRBPM::get_device_property()
Tango::DbData dev_prop;
dev_prop.push_back(Tango::DbDatum("AGCMinADC"));
dev_prop.push_back(Tango::DbDatum("AGCMaxADC"));
dev_prop.push_back(Tango::DbDatum("KH"));
dev_prop.push_back(Tango::DbDatum("KV"));
dev_prop.push_back(Tango::DbDatum("BPM_Set_List"));
dev_prop.push_back(Tango::DbDatum("BPM_Geometry"));
// is there at least one property to be read ?
if (dev_prop.size()>0)
......@@ -568,6 +597,50 @@ void LiberaSparkSRBPM::get_device_property()
// And try to extract AGCMaxADC value from database
if (dev_prop[i].is_empty()==false) dev_prop[i] >> aGCMaxADC;
// Try to initialize KH from class property
cl_prop = ds_class->get_class_property(dev_prop[++i].name);
if (cl_prop.is_empty()==false) cl_prop >> kH;
else {
// Try to initialize KH from default device value
def_prop = ds_class->get_default_device_property(dev_prop[i].name);
if (def_prop.is_empty()==false) def_prop >> kH;
}
// And try to extract KH value from database
if (dev_prop[i].is_empty()==false) dev_prop[i] >> kH;
// Try to initialize KV from class property
cl_prop = ds_class->get_class_property(dev_prop[++i].name);
if (cl_prop.is_empty()==false) cl_prop >> kV;
else {
// Try to initialize KV from default device value
def_prop = ds_class->get_default_device_property(dev_prop[i].name);
if (def_prop.is_empty()==false) def_prop >> kV;
}
// And try to extract KV value from database
if (dev_prop[i].is_empty()==false) dev_prop[i] >> kV;
// Try to initialize BPM_Set_List from class property
cl_prop = ds_class->get_class_property(dev_prop[++i].name);
if (cl_prop.is_empty()==false) cl_prop >> bPM_Set_List;
else {
// Try to initialize BPM_Set_List from default device value
def_prop = ds_class->get_default_device_property(dev_prop[i].name);
if (def_prop.is_empty()==false) def_prop >> bPM_Set_List;
}
// And try to extract BPM_Set_List value from database
if (dev_prop[i].is_empty()==false) dev_prop[i] >> bPM_Set_List;
// Try to initialize BPM_Geometry from class property
cl_prop = ds_class->get_class_property(dev_prop[++i].name);
if (cl_prop.is_empty()==false) cl_prop >> bPM_Geometry;
else {
// Try to initialize BPM_Geometry from default device value
def_prop = ds_class->get_default_device_property(dev_prop[i].name);
if (def_prop.is_empty()==false) def_prop >> bPM_Geometry;
}
// And try to extract BPM_Geometry value from database
if (dev_prop[i].is_empty()==false) dev_prop[i] >> bPM_Geometry;
}
/*----- PROTECTED REGION ID(LiberaSparkSRBPM::get_device_property_after) ENABLED START -----*/
......@@ -638,7 +711,7 @@ void LiberaSparkSRBPM::read_DeviceType(Tango::Attribute &attr)
DEBUG_STREAM << "LiberaSparkSRBPM::read_DeviceType(Tango::Attribute &attr) entering... " << endl;
/*----- PROTECTED REGION ID(LiberaSparkSRBPM::read_DeviceType) ENABLED START -----*/
attr_DeviceType_read[0] = "LiberaSpark";
attr_DeviceType_read[0] = (char *)"LiberaSpark";
attr.set_value(attr_DeviceType_read);
/*----- PROTECTED REGION END -----*/ // LiberaSparkSRBPM::read_DeviceType
......@@ -2334,6 +2407,12 @@ void LiberaSparkSRBPM::write_CalcAlgorithm(Tango::WAttribute &attr)
attr.get_write_value(w_val);
/*----- PROTECTED REGION ID(LiberaSparkSRBPM::write_CalcAlgorithm) ENABLED START -----*/
if(nbPolynomialSet==0 && w_val==CalcAlgorithmEnum::_POLYNOMIAL)
Tango::Except::throw_exception(
"ErrorWrite",
"No polynomial defined",
"LiberaSparkSRBPM::write_CalcAlgorithm");
posCalculation->SetAlgorithm(w_val);
/*----- PROTECTED REGION END -----*/ // LiberaSparkSRBPM::write_CalcAlgorithm
......@@ -2382,7 +2461,7 @@ void LiberaSparkSRBPM::write_Polynomial(Tango::WAttribute &attr)
}
attr_Polynomial_read[0] = (PolynomialEnum)v;
posCalculation->SetPolynomial(hBpmCoefs[v],vBpmCoefs[v]);
posCalculation->SetPolynomial(hBpmCoefs[v],vBpmCoefs[v],qBpmCoefs[v],sBpmCoefs[v]);
/*----- PROTECTED REGION END -----*/ // LiberaSparkSRBPM::write_Polynomial
}
......@@ -3615,28 +3694,25 @@ void LiberaSparkSRBPM::LoadPolynomial() {
Tango::Database *db = Tango::Util::instance()->get_database();
// Retrieve list of sets from DB
Tango::DbData db_data;
db_data.push_back(Tango::DbDatum("BPM_Set_List"));
db->get_class_property("LiberaSparkSRBPM", db_data);
if (db_data[0].is_empty()) {
cout << "No polynomial defined" << endl;
return;
}
vector<string> setNames;
db_data[0] >> setNames;
nbPolynomialSet = (int)setNames.size();
nbPolynomialSet = (int)bPM_Set_List.size();
if(nbPolynomialSet==0) {
cout << "No polynomial defined" << endl;
return;
}
if(bPM_Geometry.length()==0) {
Tango::Except::throw_exception(
"ErrorInit",
"BPM_Geometry not defined",
"LiberaSparkSRBPM::LoadPolynomial");
}
// Update "Polynomial" attribute enum label
Tango::Attribute &pAtt = get_device_attr()->get_attr_by_name("Polynomial");
Tango::MultiAttrProp<Tango::DevEnum> presetProps;
pAtt.get_properties(presetProps);
presetProps.enum_labels = setNames;
presetProps.enum_labels = bPM_Set_List;
pAtt.set_properties(presetProps);
// Load coeficients
......@@ -3645,14 +3721,21 @@ void LiberaSparkSRBPM::LoadPolynomial() {
string hBpmSetName;
string vBpmSetName;
hBpmSetName = "H_BPM_Coef_" + setNames[i];
vBpmSetName = "V_BPM_Coef_" + setNames[i];
string qBpmSetName;
string sBpmSetName;
hBpmSetName = "H_BPM_Coef_" + bPM_Geometry + "_" + bPM_Set_List[i];
vBpmSetName = "V_BPM_Coef_" + bPM_Geometry + "_" + bPM_Set_List[i];
qBpmSetName = "Q_BPM_Coef_" + bPM_Geometry + "_" + bPM_Set_List[i];
sBpmSetName = "S_BPM_Coef_" + bPM_Geometry + "_" + bPM_Set_List[i];
// Retreive BPM coef from the class
Tango::DbData db_data;
db_data.push_back(Tango::DbDatum(hBpmSetName));
db_data.push_back(Tango::DbDatum(vBpmSetName));
db_data.push_back(Tango::DbDatum(qBpmSetName));
db_data.push_back(Tango::DbDatum(sBpmSetName));
db->get_class_property("LiberaSparkSRBPM", db_data);
if (db_data[0].is_empty()) {
Tango::Except::throw_exception(
"ErrorInit",
......@@ -3662,34 +3745,88 @@ void LiberaSparkSRBPM::LoadPolynomial() {
if (db_data[1].is_empty()) {
Tango::Except::throw_exception(
"ErrorInit",
vBpmSetName + "not defined for class LiberaBrilliance",
vBpmSetName + "not defined for class LiberaSparkSRBPM",
"LiberaSparkSRBPM::LoadPolynomial");
}
db_data[0] >> hBpmCoefs[i];
db_data[1] >> vBpmCoefs[i];
string errStr;
if( !posCalculation->CheckPolynomial(hBpmCoefs[i],errStr) ) {
if (db_data[2].is_empty()) {
Tango::Except::throw_exception(
"ErrorInit",
hBpmSetName + ": " + errStr,
qBpmSetName + "not defined for class LiberaSparkSRBPM",
"LiberaSparkSRBPM::LoadPolynomial");
}
if( !posCalculation->CheckPolynomial(vBpmCoefs[i],errStr) ) {
if (db_data[3].is_empty()) {
Tango::Except::throw_exception(
"ErrorInit",
vBpmSetName + ": " + errStr,
sBpmSetName + "not defined for class LiberaSparkSRBPM",
"LiberaSparkSRBPM::LoadPolynomial");
}
vector<string> strs;
db_data[0] >> strs;
parsePolynomial(hBpmSetName,strs,hBpmCoefs[i]);
db_data[1] >> strs;
parsePolynomial(vBpmSetName,strs,vBpmCoefs[i]);
db_data[2] >> strs;
parsePolynomial(qBpmSetName,strs,qBpmCoefs[i]);
db_data[3] >> strs;
parsePolynomial(sBpmSetName,strs,sBpmCoefs[i]);
}
// Set default
posCalculation->SetPolynomial(hBpmCoefs[0], vBpmCoefs[0]);
posCalculation->SetPolynomial(hBpmCoefs[0], vBpmCoefs[0], qBpmCoefs[0], sBpmCoefs[0]);
}
void LiberaSparkSRBPM::parsePolynomial(string polyName,vector<string>& in,vector<POLY>& poly) {
for(int i=0;i<(int)in.size();i++) {
vector<string> fields;
split(fields,in[i],',');
if(fields.size()!=3)
Tango::Except::throw_exception(
"ErrorInit",
polyName + ", invalid definition: pH,pV,coef expected",
"LiberaSparkSRBPM::parsePolynomial");
POLY p;
p.nX = std::stoi(fields[0]);
if(p.nX>=MAX_POLYNOMIAL_POWER)
Tango::Except::throw_exception(
"ErrorInit",
polyName + ", invalid definition: power of X too large",
"LiberaSparkSRBPM::parsePolynomial");
p.nY = std::stoi(fields[1]);
if(p.nY>=MAX_POLYNOMIAL_POWER)
Tango::Except::throw_exception(
"ErrorInit",
polyName + ", invalid definition: power of Y too large",
"LiberaSparkSRBPM::parsePolynomial");
p.c = std::stod(fields[2]);
poly.push_back(p);
}
}
void LiberaSparkSRBPM::split(vector<string> &tokens, const string &text, char sep) {
size_t start = 0, end = 0;
tokens.clear();
while ((end = text.find(sep, start)) != string::npos) {
tokens.push_back(text.substr(start, end - start));
start = end + 1;
}
tokens.push_back(text.substr(start));
}
......
LiberaSparkSRBPM.h
View file @ 56387966
......@@ -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:
......
LiberaSparkSRBPM.xmi
View file @ 56387966
......@@ -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"/>
......
LiberaSparkSRBPMClass.cpp
View file @ 56387966
......@@ -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);
}
//--------------------------------------------------------
......
PosCalculation.cpp
View file @ 56387966
......@@ -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;