385 lines
16 KiB
C++
385 lines
16 KiB
C++
#include <stdio.h>
|
|
|
|
|
|
#include <iostream>
|
|
//Chrono can be used to measure the time of certain events in [us]:
|
|
#include <chrono>
|
|
// auto start = std::chrono::high_resolution_clock::now();
|
|
// //do stuff here//
|
|
// auto stop = std::chrono::high_resolution_clock::now();
|
|
// auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop-start);
|
|
// std::cout << duration.count() << std::endl;
|
|
|
|
|
|
//Global Variables
|
|
double CHI=0, OMEGA=0;
|
|
|
|
//LUT Global Variables:
|
|
#define LUT_MAX_LINES 10000
|
|
double LUT[LUT_MAX_LINES][6]; //[Line_No] [OMEGA, CHI, PHI, X_corr, Y_corr, Z_corr]
|
|
int LUT_nelems = 0;
|
|
|
|
// Commonly used functions ////////////////////////////////////////////////////
|
|
int load_calibration_table(void) {
|
|
//Define variables
|
|
char filename[32]= "LUT.csv";
|
|
FILE * pFile;
|
|
char mode[4]="r"; //mode read
|
|
char header_str[1000]; //string buffer
|
|
int lineno = 0;
|
|
//Open file:
|
|
pFile = fopen(filename, mode);
|
|
//Check if file can be opened:
|
|
if (pFile==NULL){ //Error loading file
|
|
printf("LUT: cannot open file: %s\n", filename);
|
|
return 1;
|
|
} else { //Ok, file open
|
|
//Read first line in with the headers
|
|
fscanf (pFile, "%s\n", header_str);
|
|
printf("LUT Header: %s\n", header_str);
|
|
//start reading line by line:
|
|
while (!feof(pFile)) {
|
|
//populate LUT
|
|
fscanf (pFile, "%lf,%lf,%lf,%lf,%lf\n", &LUT[lineno][1], //CHI
|
|
&LUT[lineno][0], //OMEGA
|
|
&LUT[lineno][3], //X_corr
|
|
&LUT[lineno][4], //Y_corr
|
|
&LUT[lineno][5]);//Z_corr
|
|
printf ("LUT[%05d]: %lf,%lf,%lf,%lf,%lf\n", lineno,
|
|
LUT[lineno][1],
|
|
LUT[lineno][0],
|
|
LUT[lineno][3],
|
|
LUT[lineno][4],
|
|
LUT[lineno][5]);
|
|
lineno++;
|
|
if (lineno >= LUT_MAX_LINES) { //if the file is too large for the internal storage
|
|
printf("LUT file has too many lines! LUT_MAX_LINES is %d.\n", LUT_MAX_LINES);
|
|
fclose(pFile);
|
|
return 1;
|
|
}
|
|
}
|
|
LUT_nelems = lineno; //update global variable how many lines the LUT has.
|
|
printf("LUT: %d lines read.\n", LUT_nelems);
|
|
fclose(pFile);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int extend_chi() {
|
|
// find largest and smalles values of CHI
|
|
double largestCHI = -720;
|
|
double smallestCHI = +720;
|
|
for (int i=0; i<LUT_nelems; i++){
|
|
if (LUT[i][1] > largestCHI ) largestCHI = LUT[i][1]; //scan to find largest CHI
|
|
if (LUT[i][1] < smallestCHI) smallestCHI = LUT[i][1]; //scan to find smallest CHI
|
|
}
|
|
//if largest and smallest values of CHI don't touch the edge, extend them.
|
|
//Why 95deg? That's because of the boundary condition at 90deg,
|
|
//and how it find the four corners later on. With 95 we have clear workspace coverage.
|
|
bool extend_to_95=false, extend_to_0=false;
|
|
if (largestCHI < 95) extend_to_95 = true;
|
|
if (smallestCHI > 0) extend_to_0 = true;
|
|
|
|
|
|
// go through the LUT again and :
|
|
// duplicate entries with largest CHI values but change CHI to 95deg
|
|
// duplicate entries with smallest CHI values but change CHI to 0deg
|
|
int LUT_nelems_before_extending; //need to remember the size of the LUT before extending.
|
|
LUT_nelems_before_extending =LUT_nelems;
|
|
for (int i=0; i<LUT_nelems_before_extending; i++){
|
|
if (extend_to_95 && (LUT[i][1] == largestCHI)) {
|
|
//check first if we have space in the LUT:
|
|
if (LUT_nelems + 1 >= LUT_MAX_LINES) {
|
|
printf("ERROR during Extending, no more space in LUT");
|
|
return 1;
|
|
}
|
|
LUT[LUT_nelems][0] = LUT[i][0];
|
|
LUT[LUT_nelems][1] = 95.0;
|
|
LUT[LUT_nelems][2] = LUT[i][2];
|
|
LUT[LUT_nelems][3] = LUT[i][3];
|
|
LUT[LUT_nelems][4] = LUT[i][4];
|
|
LUT[LUT_nelems][5] = LUT[i][5];
|
|
LUT_nelems++;
|
|
}
|
|
if (extend_to_0 && LUT[i][1] == smallestCHI) {
|
|
//check first if we have space in the LUT:
|
|
if (LUT_nelems + 1 >= LUT_MAX_LINES) {
|
|
printf("ERROR during Extending, no more space in LUT");
|
|
return 1;
|
|
}
|
|
LUT[LUT_nelems][0] = LUT[i][0];
|
|
LUT[LUT_nelems][1] = 0.;
|
|
LUT[LUT_nelems][2] = LUT[i][2];
|
|
LUT[LUT_nelems][3] = LUT[i][3];
|
|
LUT[LUT_nelems][4] = LUT[i][4];
|
|
LUT[LUT_nelems][5] = LUT[i][5];
|
|
LUT_nelems++;
|
|
}
|
|
}
|
|
printf("LUT: Extended to %d, added %d elems. (CHI Edge-Extend)\n", LUT_nelems, LUT_nelems-LUT_nelems_before_extending);
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
int extend_omega() {
|
|
// find largest and smalles values of OMEGA
|
|
double largestOMEGA = -720;
|
|
double smallestOMEGA = +720;
|
|
for (int i=0; i<LUT_nelems; i++){
|
|
if (LUT[i][0] > largestOMEGA) largestOMEGA = LUT[i][0]; //scan to find largest OMEGA
|
|
if (LUT[i][0] < smallestOMEGA) smallestOMEGA = LUT[i][0]; //scan to find smallest OMEGA
|
|
}
|
|
// go through the LUT again and :
|
|
// duplicate entries with largest omega values but change OMEGA to OMEGA-360deg
|
|
// duplicate entries with smallest omega values but change OMEGA to OMEGA+360deg
|
|
int LUT_nelems_before_extending; //need to remember the size of the LUT before extending.
|
|
LUT_nelems_before_extending =LUT_nelems;
|
|
for (int i=0; i<LUT_nelems_before_extending; i++){
|
|
if (LUT[i][0] == largestOMEGA) {
|
|
//check first if we have space in the LUT:
|
|
if (LUT_nelems + 1 >= LUT_MAX_LINES) {
|
|
printf("ERROR during Extending, no more space in LUT");
|
|
return 1;
|
|
}
|
|
LUT[LUT_nelems][0] = LUT[i][0] - 360;
|
|
LUT[LUT_nelems][1] = LUT[i][1];
|
|
LUT[LUT_nelems][2] = LUT[i][2];
|
|
LUT[LUT_nelems][3] = LUT[i][3];
|
|
LUT[LUT_nelems][4] = LUT[i][4];
|
|
LUT[LUT_nelems][5] = LUT[i][5];
|
|
LUT_nelems++;
|
|
}
|
|
if (LUT[i][0] == smallestOMEGA) {
|
|
//check first if we have space in the LUT:
|
|
if (LUT_nelems + 1 >= LUT_MAX_LINES) {
|
|
printf("ERROR during Extending, no more space in LUT");
|
|
return 1;
|
|
}
|
|
LUT[LUT_nelems][0] = LUT[i][0] + 360;
|
|
LUT[LUT_nelems][1] = LUT[i][1];
|
|
LUT[LUT_nelems][2] = LUT[i][2];
|
|
LUT[LUT_nelems][3] = LUT[i][3];
|
|
LUT[LUT_nelems][4] = LUT[i][4];
|
|
LUT[LUT_nelems][5] = LUT[i][5];
|
|
LUT_nelems++;
|
|
}
|
|
}
|
|
printf("LUT: Extended to %d, added %d elems. (OMEGA Wrap-Over)\n", LUT_nelems, LUT_nelems-LUT_nelems_before_extending);
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
int corr_type_2 (double OMEGA, double CHI) {
|
|
|
|
//Get OMEGA & CHI values from the input message
|
|
//double CHI, OMEGA;
|
|
//OMEGA = input_msg.position[3];
|
|
//CHI = input_msg.position[4];
|
|
printf("OMEGA: %lf, CHI: %lf\n", OMEGA, CHI);
|
|
|
|
auto start = std::chrono::high_resolution_clock::now();
|
|
// For the Bilinear Interpollation, we need the four Edge points (Q11, Q12, Q21, Q22)
|
|
// which are the closest points in the LUT
|
|
int iQ11=-1, iQ12=-1, iQ21=-1, iQ22=-1;
|
|
|
|
//Run through LUT and find closest four corner Points Q11, Q12, Q21, Q22:
|
|
//
|
|
for (int i=0; i<LUT_nelems; i++){
|
|
if (LUT[i][0]<=OMEGA && LUT[i][1]<=CHI) { //Q11: [0]<OMEGA && [1]<CHI
|
|
if (iQ11==-1) iQ11=i;
|
|
double dist_i, dist_iQ11;
|
|
dist_i = (OMEGA-LUT[i] [0])*(OMEGA-LUT[i] [0]) + (CHI-LUT[i] [1])*(CHI-LUT[i] [1]);
|
|
dist_iQ11 = (OMEGA-LUT[iQ11][0])*(OMEGA-LUT[iQ11][0]) + (CHI-LUT[iQ11][1])*(CHI-LUT[iQ11][1]);
|
|
if (dist_i <= dist_iQ11) iQ11 = i; //Set this point as the new Q11
|
|
}
|
|
if (LUT[i][0]<=OMEGA && LUT[i][1]>CHI) { //Q12: [0]<OMEGA && [1]>CHI
|
|
if (iQ12==-1) iQ12=i;
|
|
double dist_i, dist_iQ12;
|
|
dist_i = (OMEGA-LUT[i] [0])*(OMEGA-LUT[i] [0]) + (CHI-LUT[i] [1])*(CHI-LUT[i] [1]);
|
|
dist_iQ12 = (OMEGA-LUT[iQ12][0])*(OMEGA-LUT[iQ12][0]) + (CHI-LUT[iQ12][1])*(CHI-LUT[iQ12][1]);
|
|
if (dist_i <= dist_iQ12) iQ12 = i; //Set this point as the new Q12
|
|
}
|
|
if (LUT[i][0]>OMEGA && LUT[i][1]<=CHI) { //Q21: [0]>OMEGA && [1]<CHI
|
|
if (iQ21==-1) iQ21=i;
|
|
double dist_i, dist_iQ21;
|
|
dist_i = (OMEGA-LUT[i] [0])*(OMEGA-LUT[i] [0]) + (CHI-LUT[i] [1])*(CHI-LUT[i] [1]);
|
|
dist_iQ21 = (OMEGA-LUT[iQ21][0])*(OMEGA-LUT[iQ21][0]) + (CHI-LUT[iQ21][1])*(CHI-LUT[iQ21][1]);
|
|
if (dist_i <= dist_iQ21) iQ21 = i; //Set this point as the new Q21
|
|
}
|
|
if (LUT[i][0]>OMEGA && LUT[i][1]>CHI) { //Q22: [0]>OMEGA && [1]>CHI
|
|
if (iQ22==-1) iQ22=i;
|
|
double dist_i, dist_iQ22;
|
|
dist_i = (OMEGA-LUT[i] [0])*(OMEGA-LUT[i] [0]) + (CHI-LUT[i] [1])*(CHI-LUT[i] [1]);
|
|
dist_iQ22 = (OMEGA-LUT[iQ22][0])*(OMEGA-LUT[iQ22][0]) + (CHI-LUT[iQ22][1])*(CHI-LUT[iQ22][1]);
|
|
if (dist_i <= dist_iQ22) iQ22 = i; //Set this point as the new Q22
|
|
}
|
|
}
|
|
|
|
// Bilinear Interpolation within Q11, Q12, Q21, Q22.
|
|
// let's use x & y for shorter formulas
|
|
double x, x1, x2, y, y1, y2;
|
|
double fQ11_BX, fQ21_BX, fQ12_BX, fQ22_BX;
|
|
double fQ11_BY, fQ21_BY, fQ12_BY, fQ22_BY;
|
|
double fQ11_BZ, fQ21_BZ, fQ12_BZ, fQ22_BZ;
|
|
double BX_corr, BY_corr, BZ_corr;
|
|
// Gather data:
|
|
x=OMEGA;
|
|
y=CHI;
|
|
x1=LUT[iQ11][0];
|
|
x2=LUT[iQ21][0];
|
|
y1=LUT[iQ11][1];
|
|
y2=LUT[iQ12][1];
|
|
fQ11_BX = LUT[iQ11][3]; fQ11_BY = LUT[iQ11][4]; fQ11_BZ = LUT[iQ11][5];
|
|
fQ12_BX = LUT[iQ12][3]; fQ12_BY = LUT[iQ12][4]; fQ12_BZ = LUT[iQ12][5];
|
|
fQ21_BX = LUT[iQ21][3]; fQ21_BY = LUT[iQ21][4]; fQ21_BZ = LUT[iQ21][5];
|
|
fQ22_BX = LUT[iQ22][3]; fQ22_BY = LUT[iQ22][4]; fQ22_BZ = LUT[iQ22][5];
|
|
// Calculate:
|
|
BX_corr = (fQ11_BX*(x2-x)*(y2-y) + fQ21_BX*(x-x1)*(y2-y) + fQ12_BX*(x2-x)*(y-y1) + fQ22_BX*(x-x1)*(y-y1))
|
|
/((x2-x1)*(y2-y1));
|
|
BY_corr = (fQ11_BY*(x2-x)*(y2-y) + fQ21_BY*(x-x1)*(y2-y) + fQ12_BY*(x2-x)*(y-y1) + fQ22_BY*(x-x1)*(y-y1))
|
|
/((x2-x1)*(y2-y1));
|
|
BZ_corr = (fQ11_BZ*(x2-x)*(y2-y) + fQ21_BZ*(x-x1)*(y2-y) + fQ12_BZ*(x2-x)*(y-y1) + fQ22_BZ*(x-x1)*(y-y1))
|
|
/((x2-x1)*(y2-y1));
|
|
|
|
auto stop = std::chrono::high_resolution_clock::now();
|
|
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop-start);
|
|
std::cout << "Interpollation took " << duration.count() << "[us]" << std::endl;
|
|
// Print output
|
|
printf ("Q11: LUT[%05d]: OMEGA=%lf, CHI=%lf, X_corr=%lf, Y_corr=%lf, Z_corr=%lf\n",
|
|
iQ11,
|
|
LUT[iQ11][0],
|
|
LUT[iQ11][1],
|
|
LUT[iQ11][3],
|
|
LUT[iQ11][4],
|
|
LUT[iQ11][5]);
|
|
printf ("Q12: LUT[%05d]: OMEGA=%lf, CHI=%lf, X_corr=%lf, Y_corr=%lf, Z_corr=%lf\n",
|
|
iQ12,
|
|
LUT[iQ12][0],
|
|
LUT[iQ12][1],
|
|
LUT[iQ12][3],
|
|
LUT[iQ12][4],
|
|
LUT[iQ12][5]);
|
|
printf ("Q21: LUT[%05d]: OMEGA=%lf, CHI=%lf, X_corr=%lf, Y_corr=%lf, Z_corr=%lf\n",
|
|
iQ21,
|
|
LUT[iQ21][0],
|
|
LUT[iQ21][1],
|
|
LUT[iQ21][3],
|
|
LUT[iQ21][4],
|
|
LUT[iQ21][5]);
|
|
printf ("Q22: LUT[%05d]: OMEGA=%lf, CHI=%lf, X_corr=%lf, Y_corr=%lf, Z_corr=%lf\n",
|
|
iQ22,
|
|
LUT[iQ22][0],
|
|
LUT[iQ22][1],
|
|
LUT[iQ22][3],
|
|
LUT[iQ22][4],
|
|
LUT[iQ22][5]);
|
|
|
|
printf ("BX_corr: %lf\n", BX_corr);
|
|
printf ("BY_corr: %lf\n", BY_corr);
|
|
printf ("BZ_corr: %lf\n", BZ_corr);
|
|
|
|
/* if (iQ11 == -1 && iQ12 == -1) {
|
|
OMEGA = OMEGA +360;
|
|
for (int i=0; i<LUT_nelems; i++){
|
|
if (LUT[i][0]<OMEGA && LUT[i][1]<CHI) { //Q11: [0]<OMEGA && [1]<CHI
|
|
if (iQ11==-1) iQ11=i;
|
|
double dist_i, dist_iQ11;
|
|
dist_i = (OMEGA-LUT[i] [0])*(OMEGA-LUT[i] [0]) + (CHI-LUT[i] [1])*(CHI-LUT[i] [1]);
|
|
dist_iQ11 = (OMEGA-LUT[iQ11][0])*(OMEGA-LUT[iQ11][0]) + (CHI-LUT[iQ11][1])*(CHI-LUT[iQ11][1]);
|
|
if (dist_i <= dist_iQ11) iQ11 = i; //Set this point as the new Q11
|
|
}
|
|
if (LUT[i][0]<OMEGA && LUT[i][1]>CHI) { //Q12: [0]<OMEGA && [1]>CHI
|
|
if (iQ12==-1) iQ12=i;
|
|
double dist_i, dist_iQ12;
|
|
dist_i = (OMEGA-LUT[i] [0])*(OMEGA-LUT[i] [0]) + (CHI-LUT[i] [1])*(CHI-LUT[i] [1]);
|
|
dist_iQ12 = (OMEGA-LUT[iQ12][0])*(OMEGA-LUT[iQ12][0]) + (CHI-LUT[iQ12][1])*(CHI-LUT[iQ12][1]);
|
|
if (dist_i <= dist_iQ12) iQ12 = i; //Set this point as the new Q12
|
|
}
|
|
}
|
|
|
|
printf ("Q11: LUT[%05d]: OMEGA=%lf, CHI=%lf, X_corr=%lf, Y_corr=%lf, Z_corr=%lf\n",
|
|
iQ11,
|
|
LUT[iQ11][0],
|
|
LUT[iQ11][1],
|
|
LUT[iQ11][3],
|
|
LUT[iQ11][4],
|
|
LUT[iQ11][5]);
|
|
printf ("Q12: LUT[%05d]: OMEGA=%lf, CHI=%lf, X_corr=%lf, Y_corr=%lf, Z_corr=%lf\n",
|
|
iQ12,
|
|
LUT[iQ12][0],
|
|
LUT[iQ12][1],
|
|
LUT[iQ12][3],
|
|
LUT[iQ12][4],
|
|
LUT[iQ12][5]);
|
|
}
|
|
*/
|
|
return 0;
|
|
|
|
//
|
|
//
|
|
//
|
|
/* Do linear interpolation based on LUT
|
|
double CHI_0, CHI_1, BX_0, BX_1, BY_0, BY_1, BZ_0, BZ_1;
|
|
for (int i=0; i<CHI_LUT_nelems-1; i++){ //for CHI values between 1.5 & 88.5 deg
|
|
if ((CHI >= CHI_LUT[i][0]) && (CHI < CHI_LUT[i+1][0])) {
|
|
CHI_0 = CHI_LUT[i][0]; CHI_1 = CHI_LUT[i+1][0];
|
|
BX_0 = CHI_LUT[i][1]; BX_1 = CHI_LUT[i+1][1];
|
|
BY_0 = CHI_LUT[i][2]; BY_1 = CHI_LUT[i+1][2];
|
|
BZ_0 = CHI_LUT[i][3]; BZ_1 = CHI_LUT[i+1][3];
|
|
}
|
|
}
|
|
BX_corr_CHI = -(BX_0 +(CHI-CHI_0)/(CHI_1-CHI_0)*(BX_1-BX_0));
|
|
BY_corr_CHI = -(BY_0 +(CHI-CHI_0)/(CHI_1-CHI_0)*(BY_1-BY_0));
|
|
BZ_corr_CHI = -(BZ_0 +(CHI-CHI_0)/(CHI_1-CHI_0)*(BZ_1-BZ_0));
|
|
|
|
if (CHI < CHI_LUT[0][0]) { //for CHI values below 1.5deg
|
|
BX_corr_CHI = CHI_LUT[0][1];
|
|
BY_corr_CHI = CHI_LUT[0][2];
|
|
BZ_corr_CHI = CHI_LUT[0][3];
|
|
}
|
|
if (CHI > CHI_LUT[CHI_LUT_nelems-1][0]) { //for CHI values above 88.5deg
|
|
BX_corr_CHI = CHI_LUT[CHI_LUT_nelems-1][1];
|
|
BY_corr_CHI = CHI_LUT[CHI_LUT_nelems-1][2];
|
|
BZ_corr_CHI = CHI_LUT[CHI_LUT_nelems-1][3];
|
|
|
|
}
|
|
*/
|
|
|
|
}
|
|
|
|
|
|
|
|
int main( int argc, char *argv[] )
|
|
{
|
|
printf("Loading LUT.");
|
|
auto start = std::chrono::high_resolution_clock::now();
|
|
load_calibration_table();
|
|
auto stop = std::chrono::high_resolution_clock::now();
|
|
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop-start);
|
|
std::cout << "LUT Reading took " << duration.count() << "[us]" << std::endl;
|
|
|
|
start = std::chrono::high_resolution_clock::now();
|
|
extend_chi();
|
|
stop = std::chrono::high_resolution_clock::now();
|
|
duration = std::chrono::duration_cast<std::chrono::microseconds>(stop-start);
|
|
std::cout << "CHI extension took " << duration.count() << "[us]" << std::endl;
|
|
|
|
start = std::chrono::high_resolution_clock::now();
|
|
extend_omega();
|
|
duration = std::chrono::duration_cast<std::chrono::microseconds>(stop-start);
|
|
stop = std::chrono::high_resolution_clock::now();
|
|
std::cout << "OMEGA wraparound took " << duration.count() << "[us]" << std::endl;
|
|
|
|
for (int i=0;i<100;i++) {
|
|
for (int j=0;j<100;j++) {
|
|
double OMEGA, CHI;
|
|
OMEGA = (360./100.*(double)i);
|
|
CHI = (90./100.*(double)j);
|
|
corr_type_2(OMEGA, CHI);
|
|
}
|
|
}
|
|
return 0;
|
|
}
|