322 lines
14 KiB
C++
322 lines
14 KiB
C++
#include <stdio.h>
|
|
|
|
//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.
|
|
bool extend_to_90=false, extend_to_0=false;
|
|
if (largestCHI < 90) extend_to_90 = true;
|
|
if (smallestCHI > 0) extend_to_0 = true;
|
|
|
|
|
|
// go through the LUT again and :
|
|
// duplicate entries with largest CHI values but change CHI to 90deg
|
|
// 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_90 && (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] = 90.;
|
|
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 (void) {
|
|
|
|
//Get OMEGA & CHI values from the input message
|
|
//double CHI, OMEGA;
|
|
//OMEGA = input_msg.position[3];
|
|
//CHI = input_msg.position[4];
|
|
OMEGA = 0;
|
|
CHI = 0;
|
|
printf("OMEGA: %lf, CHI: %lf\n", OMEGA, CHI);
|
|
|
|
// For the Bilinear Interpollation, we need the four Edge points (Q11, Q12, Q21, Q22)
|
|
// which are the closest points in the LUT
|
|
double OMEGA_1, OMEGA_2, CHI_1, CHI_2;
|
|
int iQ11=-1, iQ12=-1, iQ21=-1, iQ22=-1;
|
|
|
|
|
|
//Run through LUT to find closest Points Q11:
|
|
//
|
|
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]);
|
|
//printf("OMEGA: %lf, CHI: %lf\n",LUT[i][0],LUT[i][1]);
|
|
//printf("dist_i: %lf, dist_iQ22: %lf\n", dist_i, dist_iQ22);
|
|
if (dist_i <= dist_iQ22) iQ22 = i; //Set this point as the new Q22
|
|
}
|
|
}
|
|
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]);
|
|
/* 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.");
|
|
load_calibration_table();
|
|
extend_chi();
|
|
extend_omega();
|
|
|
|
|
|
corr_type_2();
|
|
|
|
return 0;
|
|
}
|