Euler angles and angular rates wrt orbit reference frame
mhmodayman opened this issue · 6 comments
Hello Eric
I am using socket programming to interface with your simulator. Your simulator is the TX side.
I am struggling to find how can I report using socket:
1- euler angles of the main body with respect to orbit reference frame
2- angular rates of the main body with respect to orbit reference frame
3- I want to confirm if orbit reference frame is the same as LVLH in your simulator
4- I am not sure what does CLN mean, and how is it related to attitude calculations
This is the Inp_IPC file
<<<<<<<<<<<<<<< 42: InterProcess Comm Configuration File >>>>>>>>>>>>>>>>
1 ! Number of Sockets
********************************** IPC 0 *****************************
TX ! IPC Mode (OFF,TX,RX,TXRX,ACS,WRITEFILE,READFILE)
0 ! AC.ID for ACS mode
"State00.42" ! File name for WRITE or READ
SERVER ! Socket Role (SERVER,CLIENT,GMSEC_CLIENT)
localhost 10001 ! Server Host Name, Port
TRUE ! Allow Blocking (i.e. wait on RX)
TRUE ! Echo to stdout
3 ! Number of TX prefixes
"SC" ! Prefix 0
"Orb" ! Prefix 1
"World" ! Prefix 2
and this is the output I am getting as of now
TIME 2023-091-00:00:02.000000000
SC[0].PosR = 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00
SC[0].VelR = 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00
SC[0].svb = 1.597590108428e-01 1.018398953365e-01 -9.818888400284e-01
SC[0].bvb = 0.000000000000e+00 0.000000000000e+00 -0.000000000000e+00
SC[0].Hvb = 7.088104198251e-05 -1.046206690504e-03 3.568393131993e-05
SC[0].AC.ParmLoadEnabled = 0
SC[0].AC.ParmDumpEnabled = 0
SC[0].B[0].wn = 7.875671331390e-05 -1.162451878338e-03 1.189464377331e-04
SC[0].B[0].qn = 2.062720814948e-01 6.768945928731e-01 -2.076632127589e-01 -6.753825054009e-01
SC[0].GN.Pos = 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00
SC[0].GN.PosRate = 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00
SC[0].GN.Ang = 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00
SC[0].GN.AngRate = 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00
Orb[0].PosN = 6.978128628534e+06 8.452597279137e+03 1.253149080861e+04
Orb[0].VelN = -1.637145984590e+01 4.226292029335e+03 6.265735604229e+03
World[3].PosH = -1.467010361715e+11 -2.856012611759e+10 0.000000000000e+00
World[3].eph.PosN = -1.467010361715e+11 -2.856012611759e+10 0.000000000000e+00
World[3].eph.VelN = 5.208518237659e+03 -2.935490158302e+04 -0.000000000000e+00
World[10].PosH = -1.469742185582e+11 -2.826368103144e+10 3.650135433286e+07
World[10].eph.PosN = -2.731823867180e+08 2.574636440388e+08 1.514084210472e+08
World[10].eph.VelN = -7.076089727499e+02 -6.008000724794e+02 -2.750860587448e+02
[EOF]
Hi Mahmoud,
Let's do the easy ones first:
Yes, L is the LVLH, which some folks call the orbit frame. If you're comparing with other references, watch for permutations of axes; conventions vary.
CLN is the direction cosine matrix (C) from the N frame to the L frame (or, of L in N, different way of describing the same rotation).
You want Euler angles with respect to L. You can build those from SC[0].B[0].CN, SC[0].CLN, and the functions MxMT and C2A.
For rates relative to L, use SC[0].B[0].wn and SC[0].wln. wln is expressed in N and B.wn is expressed in B, so you'll need some conversions before you subtract.
Now the hard part. Once you have created new variables to hold the quantities you want, you need to get them into the socket traffic. Read Database/Readme.txt. Look for lines in 42types.h with the string [=] in them, and compare to your socket message. Annotate your new variables accordingly. In the Database folder, run "python HeadersToJson.py" and "python JsonToIPC.py". make clean, make. If all goes well, your new variables will now appear in your socket message.
Someday I'll finish automating this process, so all you'll have to do is make the annotation in the header file and remake.
Regards,
-Eric (he/him)
Hi Eric,
I find these variables are already in the 42types.h
SC[0].B[0].CN
SC[0].CLN
SC[0].B[0].wn
SC[0].wln
so I think I don't need to run "python HeadersToJson.py" and "python JsonToIPC.py", right?
I understood (from Nomenclature.pdf) that for Euler angles with respect to L, I can use the following, right?
double CBL[3][3], Roll, Pitch, Yaw;
MxMT(SC[0].CLN, SC[0].B[0].CN, CBL);
C2A(321,CBL,&Yaw,&Pitch,&Roll); % results in euler angles represented in L frame.
But I am not sure where should I add these lines (to define them).
and where should I add them to be sent over socket (I assume they should be in SimWriteToSocket.c).
and (from mathkit.c) I understood that for angular rates of B with respect to L, I can use the following, right?
double wbn_L[3]; % angular velocity of B in N (expressed in L)
double wln_L[3]; % Angular velocity of L in N (expressed in L)
double wbl_L[3]; % Angular velocity of B in L (expressed in L)
double CNL[3][3]; % DCM of N in L (or from L to N)
MxV(CBL, SC[0].B[0].wn, wbn_L); % CBL that I calculated above for Euler angles
MT(SC[0].CLN, CNL);
MxV(CNL, SC[0].wln, wln_L);
wbl_L[0] = wln_L[0] - wbn_L[0];
wbl_L[1] = wln_L[1] - wbn_L[1];
wbl_L[2] = wln_L[2] - wbn_L[2];
But I am not sure where should I add these lines (to define them).
and where should I add them to be sent over socket (I assume they should be in SimWriteToSocket.c).
Hi Mahmoud,
There are a couple of ways to go about this. One, you could add your lines directly into SimWriteToSocket.c. This keeps all your changes local, which is nice. The downside is that SimWriteToSocket.c, and all the files in the Source/IPC folder, get rewritten by the python scripts. If you never run the python scripts, no problem. If you do, you'll lose any hand-written code you've added.
The other way works within the paradigm that uses the python scripts to autogenerate all the IPC code, so it's preferable if you're going to be adding more things in the future (and for any third party reading this thread later).
First, add your new variables to the SC struct definition in 42types.h. So, for example, you'll have a SC[0].wbl_L. Annotate the definition with the [~=~] string. Then run the python scripts, make clean, remake. Your new variable will then appear in SimWriteToSocket.c along with all the others.
You still need to actually compute your new variable. You have choices on where to do this; I have no strong suggestions. I might put a function in 42ipc.c, or in 42report.c. Just somewhere convenient that's after all your inputs are freshly computed, and before the socket gets written to. See 42exec.c:SimStep to see the order things get called in.
Regards,
-Eric (he/him)
Hi Mahmoud,
You have some transposition errors in the code you quoted above, which means I should explain some nomenclature a little more carefully.
A rotation from frame A to frame B is represented by the direction cosine matrix CBA or the quaternion qba. The order of frames matters. CAB is the transpose of CBA, and represents the opposite rotation (from B to A).
MxM is a 3x3 matrix product. MTxM transposes the first matrix, and MxMT transposes the second matrix. These are for convenience.
To compose a rotation from two other rotations, we multiply. For instance, CBL = CBN*CNL. Note how the N is "in the middle" so it "cancels".
So MxMT(SC[0].B[0].CN,SC[0].CLN,CBL) is correct. Check your other constructions with this in mind.
Also, wbl_L should be wbn_L - wln_L.
Regards,
-Eric (he/him)
Hi Eric
I updated the functions as below
void calcEulerInLFrame()
{
double CBL[3][3];
MxMT(SC[0].B[0].CN, SC[0].CLN, CBL);
C2A(321,CBL, &SC[0].Yaw_L, &SC[0].Pitch_L, &SC[0].Roll_L);
}
void calcEulerRatesInLFrame()
{
double CBL[3][3];
MxMT(SC[0].B[0].CN, SC[0].CLN, CBL);
MxV(CBL, SC[0].B[0].wn, SC[0].wbn_L);
MT(SC[0].CLN, SC[0].CNL);
MxV(SC[0].CNL, SC[0].wln, SC[0].wln_L);
SC[0].wbl_L[0] = SC[0].wbn_L[0] - SC[0].wln_L[0];
SC[0].wbl_L[1] = SC[0].wbn_L[1] - SC[0].wln_L[1];
SC[0].wbl_L[2] = SC[0].wbn_L[2] - SC[0].wln_L[2];
}
The reason I wanted to have rates expressed in L frame, is that I wanted to see their initial values.
As you can see, in 42/Tx/SC_Simple.txt, I have angular rates wrt L frame as (0.08, -0.02, 0.03)
*************************** Initial Attitude ***************************
LAL ! Ang Vel wrt [NL], Att [QA] wrt [NLF]
0.08 -0.02 0.03 ! Ang Vel (deg/sec)
0.0 0.0 0.0 1.0 ! Quaternion
0.0 0.0 0.0 123 ! Angles (deg) & Euler Sequence
*************************** Dynamics Flags ***************************
But, from socket I get different initial values.
SC[0].wbl_L = 1.124118660126e-03 -5.782522778693e-04 -1.424477398272e-04
Shouldn't values be like the following?
SC[0].wbl_L = 0.08 -0.02 0.03
Hi Mahmoud,
The input file takes in numbers in deg/sec. The outputs are in rad/sec. There's more going on, though.
Angular rates are the rate of some frame with respect to some frame. So far so good. But we have to store the components of that vector expressed in some frame. If notation fatigue just set in, you're not alone. The variable names I give call out the "of" and the "with respect to", but leave the "expressed in" implicit. Sorry about any confusion that comes out of that.
B.wn is the angular rate of B with respect to N, expressed in B.
SC.wln is the angular rate of L with respect to N, expressed in N.
Look in the variable declarations in, for example, 42types.h, for hints about what frame a vector is expressed in.
Try:
void calcEulerRatesInLFrame()
{
double CBL[3][3];
MxMT(SC[0].B[0].CN, SC[0].CLN, CBL);
MTxV(CBL, SC[0].B[0].wn, SC[0].wbn_L);
MxV(SC[0].CLN, SC[0].wln, SC[0].wln_L);
SC[0].wbl_L[0] = SC[0].wbn_L[0] - SC[0].wln_L[0];
SC[0].wbl_L[1] = SC[0].wbn_L[1] - SC[0].wln_L[1];
SC[0].wbl_L[2] = SC[0].wbn_L[2] - SC[0].wln_L[2];
}
Note, however, that the numbers in the input file are meant to be expressed in the B[0] frame, not in the L (or N) frame. So if you're checking the inputs, you probably want to express SC[0].wln in the SC[0].B[0] frame, and perform the subtraction there.
Regards,
-Eric (he/him)