mx/smargopolo
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

includes FPRX, FPRY, FPRZ orientation tweaks for both nudgeSH and nudgeO

Browse Source
This commit is contained in:
Wayne Glettig
2022-04-21 12:11:42 +02:00
parent e2ac16840c
commit 3e2e80c2e5
1 changed files with 17 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
catkin_ws/src/smargon/src/nudge.cpp
View File

@@ -29,8 +29,10 @@ double OMEGA, CHI, PHI;
double OX, OY, OZ;
double NX, NY, NZ;
//Orientation tweak parameters:
double offsetOMEGA = 0.0;
double FPRX=0.0, FPRY=0.0, FPRZ=0.0;
//double FPRX=-90.0, FPRY=0.0, FPRZ=13.0; //Vespa orientation
// Commonly used functions ////////////////////////////////////////////////////
int calculateSH () {
@@ -65,16 +67,27 @@ int calculateSH () {
int calculateO () {
double oldOX, oldOY, oldOZ, omega;
double oldOX, oldOY, oldOZ, omega, offsetOmega, fprx, fpry, fprz;
double nudgeOX, nudgeOY, nudgeOZ;
//Get existing SH values:
oldOX = OX; oldOY = OY; oldOZ = OZ;
//calculate the nudge vector [NX,NY,NZ] in SH coordinates:
omega = OMEGA/180*M_PI;
nudgeOX = NY*cos(omega) - NZ*sin(omega);
nudgeOY = NY*sin(omega) + NZ*cos(omega);
nudgeOZ = NX;
offsetOmega = offsetOMEGA/180*M_PI;
fprx = FPRX/180*M_PI;
fpry = FPRY/180*M_PI;
fprz = FPRZ/180*M_PI;
//This is the simplified formula without fprx,fpry,frpz, offsetOmega
//nudgeOX = NY*cos(omega) - NZ*sin(omega);
//nudgeOY = NY*sin(omega) + NZ*cos(omega);
//nudgeOZ = NX;
//This is the formula that takes in account fprx,fpry,frpz and offsetOmega
nudgeOX = NX*(sin(omega + offsetOmega)*sin(fpry) + cos(omega + offsetOmega)*cos(fpry)*sin(fprz)) + NY*(cos(omega + offsetOmega)*cos(fprx)*cos(fprz) - sin(omega + offsetOmega)*cos(fpry)*sin(fprx) + cos(omega + offsetOmega)*sin(fprx)*sin(fpry)*sin(fprz))+ NZ*(cos(omega + offsetOmega)*cos(fprx)*sin(fpry)*sin(fprz) - cos(omega + offsetOmega)*cos(fprz)*sin(fprx) - sin(omega + offsetOmega)*cos(fprx)*cos(fpry));
nudgeOY = NX*(sin(omega + offsetOmega)*cos(fpry)*sin(fprz) - cos(omega + offsetOmega)*sin(fpry)) + NY*(cos(omega + offsetOmega)*cos(fpry)*sin(fprx) + sin(omega + offsetOmega)*cos(fprx)*cos(fprz) + sin(omega + offsetOmega)*sin(fprx)*sin(fpry)*sin(fprz))+ NZ*(cos(omega + offsetOmega)*cos(fprx)*cos(fpry) - sin(omega + offsetOmega)*cos(fprz)*sin(fprx) + sin(omega + offsetOmega)*cos(fprx)*sin(fpry)*sin(fprz));
nudgeOZ = NX*cos(fpry)*cos(fprz) + NY*(cos(fprz)*sin(fprx)*sin(fpry) - cos(fprx)*sin(fprz)) + NZ*(sin(fprx)*sin(fprz) + cos(fprx)*cos(fprz)*sin(fpry));
//add the nudge vecor to the existing SH position
OX = oldOX + nudgeOX;