-- ' 11/12/2012 17:25:38 -- ' 02/09/2012 19:26:00 -- ' 14/08/2012 18:16:34 -------------------------------------------------------------------------------- -- (c) Empresarios Agrupados - EcosimPro Simulation Source Code -- FILE NAME: Functions (derived from FLIGHT_Common.el and FLIGHT_CompFrame.el) -- DESCRIPTION: Defines common enumerative types, constants, global variables, port types and functions for SATELLITE library components (derived from flight dynamics components) -- NOTES: -- AUTHOR: Ramon Perez Vara and Borja Garcia Gutierrez, C.Koppel Kci@KopooS.com -- CREATION DATE: 4/01/2001 updated 2011, 2012, 2013 --Enums, Ports and Functions list -- Enum ECI, Pol, Orb, OrbPointingReference, Sat, FluidAxis, AttitudeMeasure -- Ports Forces, State, InteractionsFluids -- Functions basic: rad; degfull; deg; CrossMatrix; XcrossY; XscalarY; Norm; Normalise; Normalise; aXb; aXbYc; Integer; JulianDay; -- SkewSymmetricLowTriangle; CrossMatrixEnum; -- CheckUnitQ; CheckChoiceQuater; QMatrix; QMatrixHat; Q1productQ2; Qinverse; -- Function Coordinate frame change and Quaternions : CoordAtoB_BrotationA; CoordArotationB; CoordAtoB_BeulerA; ChangeSatToNum; -- CoordAqaterBmatrix; CoordAcardanBmatrix; CoordECIeulerOrbQuater; CoordAqaterBeuler; CoordOrbCardanSatQuater; CoordAquaterBcardan; -- CoordPolEulerECI; CoordRVECItoEulerAngles; CoordAqaterBmatrixEnum; -- Functions Specialised: MoonSunECI; ParametersToCartesian; DervativeQmatrixSatAxis; InertiaMatrixSatAxis; InertiaMatrixDisplaced; -- CrossMatrixIndex; MatrixFrameChange; AngleVectordeg; FrameChange; deltaP; ComputeDeltaPmap; ComputeDeltaPpoint; MinMaxIsoDeltaPmap; DeltaP_archimedes -- InertiaMatrixLoop; InertiaMatrixTotalDisplaced; Eclipse -- NormaliseIndex; EventsThrusters --UPDATE end 2012 -- rename of Cardan angles to more intuitive psiyaw, thetapitch, phiroll -- idem for OCOMFluids instead of OCOMComponent and InertiaMatrixFluids instead of InertiaMatrixComponent -- add in the port InteractionsFluids a parameter 'n4' for the number of tanks used and all matrixes are dynamic with that size, -- and add REAL mfr[2] for enabling the change of COM and inertia for fluids when thrusters use propellants -- add in the functions for the COMPONENTS the vector size in parameter and delete the function CrossMatrixIndex4 -- add in the function CoordRVECItoEulerAngles more orbital parameter Altitudes of Apogee, Perigee, Excentricity, etc -- add a function InertiaMatrixLoop for computing the increment per nodes of inertia -- add a parameter FromCOMNotToCOM in the function InertiaMatrixDisplaced for using the opposite function -- add a function InertiaMatrixTotalDisplaced for computing the total inertia matrix at COMsat from individual Inertia at COMdry and COMfluids -- add in function DeltaP_archimedes a parameter OUT REAL MinertiaVolume[4,Sat, Sat ] for computing the inertia matrix per volume with respect to the reference frame -- and update a bug in the weightlessness case for the number of nodes wetted and compute the inatia matrix for this case -- and correct slightly the location of the COM in the tank (stricktly at the tank centre now for weightlessness case) and dPoutlet for the cases Nodes is even or odd -- and manage the case of full tanks volume with liquid by computing the COM and dPoutlet even if the fluid volume has not been fully reached -- add a simple function Eclipse for providing a flag to true if the position of the satellite is in eclipse of the sun (in the shadow cone) --UPDATE 2013 -- rename axis Pol instead of OrbitalPolarAxis, Orb instead of OrbAxis, Sat instead of SatAxis -- rename functions CoordECIeulerOrbQuater instead of CoordAeulerBquater, CoordOrbCardanSatQuater instead of CoordAcardanBquater, CoordPolEulerECI CoordOrbitalPolarEulerECI, , CoordECIquaterOrbmatrix CoordECIquaterOrbmatrix -- in 'State port' add WrotSatd[Sat] and Vsat[Sat] -- add a function SkewSymmetricLowTriangle for systematic use -- add functions NormaliseIndex , QMatrix, QMatrixHat, ChangeSatToNum -- add a string parameter for error messages in CheckUnitQ -- change functions Q1productQ2, CoordAqaterBmatrix, DervativeQmatrixSatAxis by using the QMatrix; -- change function InertiaMatrixSatAxis by InertiaMatrixSatAxis2 accepting inertia with two vectors -- add a function EventsThrusters for writing in a string all the thrusters that are being fired -- add ErrRounding: a function for the out of rounding error messages (else some arc sinus>1 may just trig the rounding error). -- -- add a default value to the functions Normalise, CheckUnitQ, CoordAqaterBmatrix with message=empty -- in function DeltaP_archimedes : correct the index of a node on the free surface in weightlessness case iSurface=min(iHalfNodes,nn+1) instead of iSurface=nn -- for the weightlessness case, add also a condition for full tank when the number of nodes per axis is odd. -- add prefix EQUAL to all the variable of port InteractionsFluids -- in function InertiaMatrixDisplaced add a variable image of the input or opposed -- simplify function CrossMatrixEnum with empty bracket OUT matrix replace CrossMatrixSatAxis -- idem function CoordAqaterBmatrixEnum (..OUT MatrixEnum[]) replace previously used functions CoordOrbQuaterSatmatrix; CoordECIquaterSatmatrix; CoordECIquaterOrbmatrix because empty bracket matrix is accepted whatever the enum -- simplify function CoordArotationB using CoordAtoB_BrotationA( -Anglerad,... -- several comments added -- functions ChangeSatToNum, EnumSatAxis not used. EnumSatAxis deleted. ChangeEnum replace ChangeSatToNum -------------------------------------------------------------------------------- -- Libraries USE MATH VERSION "3.0" USE PORTS_LIB VERSION "1.0" --used by sensors USE CONTROL VERSION "3.0" --used by sensors -- Enumerative types -------------------------------------------------------------------------------- -- Axis names for reference systems -------------------------------------------------------------------------------- ENUM ECI = {Xq, Yq, Zq} "Earth Centred Inertial equatorial reference (to Vernal point, ZxX, Earth angular momentum) system axis names" ENUM Pol = {Xp, Yp, Zp} "Orbital polar system (from Focus radius, ZxX, Orbit angular momentum) axis names" ENUM Orb = {X, Y, Z} "Telecom Satellite orbital system (in-plane local horizontal i.e. Velocity for circular, ZxX, to Earth centre) axis names" ENUM Sat = {x, y, z} "Satellite reference system (in plane of launcher i/f --anti-Earth side--, ZxX, normal to launcher i/f ) axis names" ENUM FluidAxis = {xf, yf, zf} "Reference frame for fluid free surface under rotation and accel, not used" --ENUM OrbitalNED = {N, e, D} "Orbital reference system (North, east, Down) axis names" ENUM AttitudeMeasure = {Quaternion, Cardan} -- Global constants CONST REAL go = 9.80665 UNITS "m/s^2" "Standard gravity acceleration" -------------------------------------------------------------------------------- -- Port for forces connections -------------------------------------------------------------------------------- PORT Forces "Forces port" --IN for the frame, OUT for the components in order to get the right sums SUM REAL F[Sat] UNITS "N" "External forces " SUM REAL M[Sat] UNITS "N*m" "External moments " SUM REAL H[Sat] UNITS "N*m*s" "External Angular momentum " SUM REAL mfr[2] UNITS "kg/s" "For components, negative: mass flow rate used and expelled to outside index 1 for Fuel, 2 for Oxydizer " SUM REAL Power UNITS "W" "For components, negative: electrical power used; positive: produced" CONTINUOUS END PORT ---------------------------------------------------------------- -- Port for state connections ---------------------------------------------------------------- PORT State "State port" --SINGLE IN when problems occurs on that port, look on declaration DISCR variable of components -- (INTEGER nt = 1 , INTEGER nSA=2, INTEGER nRW=3) --Satellite state (for control, forces and rotation interactions) EQUAL REAL mass UNITS "kg" " Satellite actual mass " EQUAL REAL OCOM[Sat] UNITS "m/s^2" " Satellite COM location wrt design reference frame at point O -- i.e. centre of launcher interface-- " EQUAL REAL Accel[Sat] UNITS "m/s^2" " Sat axis acceleration wrt ECI, in Sat axis" EQUAL REAL Wrotd[Sat] UNITS "deg/s" " Body Sat frame angular velocity wrt ECI, in Sat axis" EQUAL REAL WrotSatd[Sat] UNITS "deg/s" " Sat axis angular velocity wrt Orb frame, in Sat axis" EQUAL REAL Wdotd[Sat] UNITS "deg/s^2" " Satellite axis angular acceleration " EQUAL REAL psiyawd UNITS "deg" "Yaw angle Sat wrt Orb" EQUAL REAL thetapitchd UNITS "deg" "Pitch angle Sat wrt Orb " EQUAL REAL phirolld UNITS "deg" "Roll angle Sat wrt Orb " EQUAL REAL RAANd UNITS "deg" "PrecessionZA angle Pol wrt ECI equatorial " EQUAL REAL incd UNITS "deg" "NutationU angle Pol wrt ECI equatorial " EQUAL REAL omPHId UNITS "deg" "Rotation propre angle Pol wrt ECI equatorial " EQUAL REAL JD UNITS "day" " Julian Day " -- Sun and Earth in Sat (for sun pressure perturbation, electrical power, gravity gradient) EQUAL REAL RSatSun[Sat] UNITS "m" " satellite COM to sun position vector, in Sat axis for Solar pressure" EQUAL REAL RSatEarth[Sat] UNITS "m" " satellite COM to Earth position vector, in Sat axis for Gravity gradient" EQUAL REAL Vsat[Sat] UNITS "m/s" "Orbital satellite COM velocity, in Sat axis" --Orbital state and for 3D real time visualization EQUAL REAL R[ECI] UNITS "m" "Orbital satellite COM position in ECI" EQUAL REAL V[ECI] UNITS "m/s" "Orbital satellite COM velocity in ECI" EQUAL REAL PZZZQ UNITS "s" " Pilot Date of day from 1900 and time for quaternion, compatible MS Excel " EQUAL REAL Q[4] UNITS "-" " quaternion to orient ECI to Satellite axis " EQUAL REAL Qsat[4] UNITS "-" " quaternion to orient Orbit axis Orb to Sat axis " --REAL ThrOn[13] UNITS "-" " Status of each thrusters On=1 Off=0 " --maxi 36 as for ATV --REAL SAangled[4] UNITS "deg" " Orientation of each SA wrt canonical position " --maxi 4 as for ATV --REAL RWrpm[4] UNITS "rpm" " Rpm of each Reaction wheel " -- maxi 8 for two control momentum gyroscope (CMG) END PORT -------------------------------------------------------------------------------- -- Port for Interactions -------------------------------------------------------------------------------- PORT InteractionsFluids(INTEGER n4 = 1 UNITS no_units "Number of tanks" ) SINGLE IN "Interactions port" --OUT for the Tank IN for the S/C --SINGLE IN when problems occurs on that port, look on declaration DISCR variable of components --port single IN because no management Equal or Sum in case of multiple connections -- global values EQUAL REAL OCOMFluids[Sat] UNITS "m" "Location of the global COM of the connected fluid components wrt the S/C design frame at point O" EQUAL REAL InertiaMatrixFluids[Sat,Sat] UNITS "kg*m^2" "Inertia Matrix of the connected fluid components at their global COM " --additive EQUAL REAL FluidMassComponent[n4] UNITS "kg" "this mass is to be used for the S/C COM computations, even if this is known already in the S/C frame --to avoid unwanted loops-- --and for eventual damping added--" EQUAL REAL FluidRho[n4] UNITS "kg/m3" "rho in the tank" EQUAL REAL SurfaceIsoDeltaP[n4] UNITS "Pa" "value of the iso dP of the free surface for each tank" --additional data for being able to compute the real Archimedes pressure at the injector of the thruster if the location of this one is given wrt the S/C design frame at point O: LocationInjector -- deltaP=rho. integral(dP) = rho. integral(scalar(f ,ds)) with ds= the vector on the curve from the free surface to the injector i;e. sum(ds)= LocationInjector-FreeSurfacePoint EQUAL REAL ForceArchimedes[16] UNITS "N" "Force due to rotation and acceleration: omega, Ai, sinalpha, cosalpha, OCOM[3], Mom[Sat, Sat] dP=rho f .ds i.e.dP= rho gamma h deltaP= integral dP" EQUAL REAL FreeSurfacePoint[n4,Sat] UNITS "m" " for each tank 1 to 4, one point of each free surface --for which 0=f.dsurf when dsurf connect two points of the surface --" EQUAL REAL mfr[2] CONTINUOUS END PORT -- Functions FUNCTION REAL rad (REAL degrees)"set to radians" BODY RETURN degrees*MATH.PI/180. END FUNCTION FUNCTION REAL degfull (REAL radians) "set to degrees without limits, without modulo, used for the speed °/s" BODY RETURN radians/MATH.PI*180. END FUNCTION FUNCTION REAL deg (REAL radians, BOOLEAN flag180=FALSE) "set to degrees within [0,360[" DECLS REAL deg_full INTEGER deg_modulo BODY deg_full=degfull(radians) deg_modulo=deg_full/360 IF(deg_modulo<0) THEN deg_modulo=deg_modulo-1 END IF IF (deg_full-deg_modulo*360>180 AND flag180) THEN --option flag180=TRUE set degrees within ]-180,180] RETURN deg_full-deg_modulo*360 -360 END IF RETURN deg_full-deg_modulo*360 END FUNCTION FUNCTION NO_TYPE SkewSymmetricLowTriangle (INTEGER n, OUT REAL mat[n,n],INTEGER nstart=1 ) "once upper triangle column>row for mat[row, column] are given, this function Skew Symmetric the Lower Triangle row>column" BODY FOR (i IN nstart,n) FOR (j IN i+1,n) mat[j,i]=-mat[i,j] --Upper triangle and diagonal is known END FOR END FOR END FUNCTION FUNCTION NO_TYPE CrossMatrix (REAL X[], OUT REAL CMX[3, 3]) "Cross matrix --skew-symmetric matrix-- for easily compute a vector cross product XxAnyOtherVector" BODY --set diagonal and upper triangle column>row for [row, column] FOR (i IN 1,3) CMX[i, i]=0 END FOR CMX[1, 2]=-X[3] CMX[1, 3]= X[2] CMX[2, 3]=-X[1] SkewSymmetricLowTriangle (3,CMX) --check first line CMX * Y = -X[3]*Y[2] + X[2]*Y[3] as expected for a cros product X^Y[1] END FUNCTION FUNCTION NO_TYPE XcrossY (REAL X[], REAL Y[], OUT REAL XxY[]) "cross product of vectors XxY using Cross matrix for vectorX " DECLS REAL CMX[3, 3]--, XxYc[1], check BODY CrossMatrix (X,CMX) FOR (i IN 1,3) XxY[i]=SUM (j IN 1,3; CMX[i, j]*Y[j]) --classic form XxY[1]=X[2]*Y[3]-X[3]*Y[2] XxY[2]=X[3]*Y[1]-X[1]*Y[3] XxY[3]=X[1]*Y[2]-X[2]*Y[1] END FOR END FUNCTION FUNCTION REAL XscalarY (REAL X[], REAL Y[])"Scalar product of vectors X.Y ; also called dot product of vector" DECLS BODY RETURN X[1]*Y[1]+X[2]*Y[2]+X[3]*Y[3] END FUNCTION FUNCTION REAL Norm (REAL X[])"norm of X; case 0 managed" DECLS REAL X_module2 BODY X_module2=XscalarY(X,X) IF (X_module2==0) THEN RETURN 0 END IF RETURN X_module2**.5 END FUNCTION FUNCTION NO_TYPE ErrRounding (REAL Small, REAL RoundingErrorAcceptable, STRING Message) "Message error routine" DECLS BODY IF abs(Small)>RoundingErrorAcceptable THEN PRINT ("$Message") PRINT(" --------------error= $Small, rounding only up to $RoundingErrorAcceptable at Time=$TIME") END IF END FUNCTION FUNCTION NO_TYPE Normalise (REAL X[], OUT REAL eX[], STRING Message="") "output a unit vector eX from X; except when null; allow message to locate errors, default messagge is empty string " DECLS REAL X_module2, checknorm, eX_module2=.7 REAL XsumAbs=1 BODY XsumAbs=SUM (i IN 1,3;abs(X[i])) X_module2=XscalarY(X,X) FOR ( i IN 1,3) IF (X_module2==0) THEN eX[i]=X[i] --null, no changes ELSE eX[i]=X[i]/sqrt(X_module2)--because not null END IF END FOR --checks IF (X_module2==0) THEN PRINT ("ERROR Normalise NULL vector for $Message at Time=$TIME X=$XsumAbs") ELSE IF (abs(Norm(eX))>1 ) THEN ErrRounding (Norm(eX)-1,1E-8,concatStrings("ERROR Normalise for ", Message)) END IF END IF RETURN END FUNCTION FUNCTION NO_TYPE NormaliseIndex (INTEGER n,REAL X[n,3], OUT REAL eX[n,3], INTEGER Index, STRING Message) "Normalise an array of vectors" DECLS REAL V[3],eV[3] BODY FOR (i IN 1,3) V[i]=X[Index,i] END FOR Normalise (V, eV , Message) FOR (i IN 1,3) eX[Index,i]=eV[i] END FOR END FUNCTION FUNCTION NO_TYPE aXb ( REAL a, REAL X[], REAL b, OUT REAL aXplusb[]) "linear combination to give a.VectorX+b" BODY FOR ( i IN 1,3) aXplusb[i]=a*X[i]+b END FOR END FUNCTION FUNCTION NO_TYPE aXbYc ( REAL a, REAL X[], REAL b,REAL Y[], REAL c, OUT REAL aXplusbYplusc[]) "linear combination to give a.VectorX+b.VectorY+c" BODY FOR ( i IN 1,3) aXplusbYplusc[i]=a*X[i]+b*Y[i]+c END FOR END FUNCTION FUNCTION INTEGER Integer (REAL Real) "Return an integer value, toward zero when negative" DECLS INTEGER i BODY i=Real IF (Real<0 AND i!=Real ) THEN RETURN Real+1 END IF RETURN Real END FUNCTION FUNCTION NO_TYPE CrossMatrixEnum (REAL V[], OUT REAL CMV[]) " cross product of vectors with Enum type " BODY CrossMatrix( V,CMV) END FUNCTION FUNCTION NO_TYPE CheckUnitQ (OUT REAL Q[4], STRING Message="")-- cannot be used in Continuous part because only Out (passed by refrerence) "to check the Quaternion unit norm and make it unit again; to be used only in Function because OUT but cannot be used in Continuous part, message default is empty string" --In quaternion algebra one use only unit Quaternion, norm =sqrt( Q.Q* ) (Q*= quaternion conjugate, and . the quaternion product) DECLS REAL NormQ INTEGER ii BODY --q(1) is real NormQ=sqrt(Q[1]**2 + Q[2]**2 + Q[3]**2 + Q[4]**2) IF NormQ==0 THEN PRINT ("******************************************ERREUR NULL Quaternion for $Message at Time=$TIME") RETURN END IF IF (abs(NormQ-1)>0 ) THEN ErrRounding (NormQ-1,1E-3,concatStrings("mastered not unit Quaternion for ", Message)) END IF FOR (i IN 1,4) Q[i]=Q[i]/NormQ -- re normalise anyway END FOR END FUNCTION FUNCTION NO_TYPE CheckChoiceQuater (REAL Qo[4], OUT REAL Q[4]) -- can be used in Continuous part because IN and OUT "Make Unit and perform the choice of the quaternion with a positive real part because equivalent to the one with the angle (2PI-angle)/2 and axis -U" DECLS REAL NormQ BODY NormQ=sqrt(Qo[1]**2 + Qo[2]**2 + Qo[3]**2 + Qo[4]**2) IF Qo[1]>=0 THEN --consider only positive Q FOR (i IN 1,4) Q[i]=Qo[i]/NormQ END FOR ELSE FOR (i IN 1,4) Q[i]=-Qo[i]/NormQ END FOR END IF END FUNCTION FUNCTION NO_TYPE QMatrix ( REAL Q[4], OUT REAL QMX[4,4]) "Quaternion matrix --skew-symmetric matrix-- for easily compute a product Q.AnyOtherQuaternion = [QMX].AnyOtherQuaternion in matrix " BODY --In quaternion algebra Q={r,v} Q'={r',v'} with r real part, v pure quaternion like a vector --quaternion product= {rr'-v.v',rv'+r'v+v x v'} can be set directly in form of a matrix Q.Q' =[QMX].Q' --set diagonal and upper triangle column>row for [row, column] FOR (i IN 1,4) QMX[i, i]=Q[1] --this come for row=1 from:rr' = Q[1]Q'[1] and for row=2 to 4 from: rv' = Q[1]Q'[row] END FOR FOR (i IN 2,4) QMX[1, i]=-Q[i] --this come for row 1 i>=2 from -v.v' = -Q[i]Q'[i] and by SkewSymm for row >=2 term +r'v END FOR QMX[2, 3]=-Q[4] --this comes for row=2 to 4 from the cross product +v x v', see CrossMatrix QMX[2, 4]=Q[3] QMX[3, 4]=-Q[2] SkewSymmetricLowTriangle (4, QMX) END FUNCTION FUNCTION NO_TYPE QMatrixHat ( REAL Q[4], OUT REAL QMXhat[4,4]) "Quaternion matrix --skew-symmetric matrix-- for easily compute a product AnyOtherQuaternion.Q = [QMXhat].AnyOtherQuaternion in matrix " BODY --In quaternion algebra Q={r,v} Q'={r',v'} with r real part, v pure quaternion like a vector --quaternion product= {r'r-v'.v,r'v+rv'-v' x v} can be set directly in form of a matrix Q'.Q = [QMXhat].Q' (note the reverse commutation) QMatrix (Q, QMXhat)--only the cross product part is to be opposited QMXhat[2, 3]=Q[4] --sign + coming from negative cross product - v' x v , see CrossMatrix QMXhat[2, 4]=-Q[3] QMXhat[3, 4]=Q[2] SkewSymmetricLowTriangle (4, QMXhat, 2) --start the SkewSymm matrix at 2 (first column already SkewSymm so unchanged) END FUNCTION FUNCTION NO_TYPE Q1productQ2 (REAL Q1[4], REAL Q2[4], OUT REAL Q1Q2[4], STRING Message="") "In quaternion algebra Q={r,v} Q'={r',v'} with r real part, v pure quaternion like a vector: quaternion product= {rr'-v.v',rv'+r'v+v x v'} " DECLS REAL QMX[4,4] REAL Error=0 REAL X[3], Y[3], XxY[3],CheckUnit INTEGER ii BODY --q(1) is real CheckUnitQ(Q1, concatStrings("routine Q1 Q1productQ2 ", Message)) CheckUnitQ(Q2, concatStrings("routine Q2 Q1productQ2 ", Message)) QMatrix(Q1,QMX) FOR (i IN 1,4) Q1Q2[i]=SUM(j IN 1,4; QMX[i,j]*Q2[j]) END FOR CheckUnitQ(Q1Q2, concatStrings("routine Q1Q2 Q1productQ2 ", Message)) END FUNCTION FUNCTION NO_TYPE Qinverse (REAL Q[4],OUT REAL Qconjug[4]) "In quaternion algebra for unit quaternion, inverse = conjugate i.e. Qinverse=Q* " DECLS BODY CheckUnitQ(Q, "routine Q Qinverse ") Qconjug[1]=Q[1] --real in 1 FOR (i IN 2,4) Qconjug[i]=-Q[i] END FOR END FUNCTION ---------------------------------------------------------------- -- Frame change functions -- Naming convention CoordA~~~~~~B####### for a given vector in Frame A to be converted in Frame B coordinates. -- moreover ~~~~~~ designate the METHOD that from the frame A orient the frame B : rotation, Euler angle, Cardan angles, quaternion, matrix -- when the METHOD that orient the frames is reversed then this is added with underscore like: toB_BrotationA -- in ####### the output of the function is given : Euler angle, Cardan angles, quaternion, matrix or nothing for a vector output --in short: CoordA METHOD B OUTPUT or CoordAtoB B METHOD A OUTPUT --Note : in order to avoid any confusion, it is recalled that: -- Euler angle corresponds to the Euler's sequence (3; 1; 3) meaning rotation on Z then on the new X then on on the new Z -- Cardan angles corresponds to the Euler's sequence (3; 2; 1) meaning rotation on Z then on the new Y ... ---------------------------------------------------------------- FUNCTION NO_TYPE CoordAtoB_BrotationA (REAL Anglerad, REAL Xa,REAL Ya, OUT REAL xb, OUT REAL yb) "Frame change: vector in frameA to same vector in frameB where Frame B after rotation gives Frame A" --Rotation of a vector 2D. Reference : Lucien Chambadal Dictionnaire des Mathématiques modernes, p197 --There are two meanings RotationVector2D into one single Frame --or CoordAtoB_BrotationA with Frame A rotated wrt Frame B and change the coordinate from Frame A to Frame B -- 1)Rotation: xb, yb are the COORDINATES after rotation of the vectror Xa, Ya. All coordinates are in the same Frame. -- 2)Frame change: Frame B after rotation gives Frame A: The vector V remain absolutely the same but written in two frames A, B. -- Thus Xa, Ya are the COORDINATES of the vector V in A and xb, yb in B DECLS REAL CosAngle,SinAngle BODY CosAngle = cos(Anglerad) SinAngle = sin(Anglerad) xb = Xa * CosAngle - Ya * SinAngle --here -sign yb = Xa * SinAngle + Ya * CosAngle --proof rotation of A wrt B =-90° SinAngle=-1 thus Xa=0 Ya=+1 xb=+1 yb=0 END FUNCTION FUNCTION NO_TYPE CoordArotationB (REAL Anglerad, REAL Xa,REAL Ya, OUT REAL xb, OUT REAL yb) "Frame change: vector in frameA to same vector in frameB where Frame A after rotation gives Frame B" BODY CoordAtoB_BrotationA( -Anglerad, Xa, Ya, xb, yb) -- xb = Xa * CosAngle + Ya * SinAngle and yb = -Xa * SinAngle + Ya * CosAngle --proof rotation of B wrt A =+90° SinAngle=+1 thus Xa=0 Ya=+1 xb=+1 yb=0 END FUNCTION FUNCTION NO_TYPE CoordAtoB_BeulerA (REAL CoordInA[], REAL PrecessionZB, REAL NutationU, REAL RotationZA, OUT REAL CoordInB[]) "Frame change: vector in frameA to same vector in frameB where Frame B after Euler's --PrecessionZB, NutationU, RotationZA-- gives Frame A " DECLS REAL Uu, vv, Tau BODY --First RotationZA: the local frameA is deduced from the frame B with a RotationZA around Zlocal, Phi = RotationZA '(w+phi=Te for satellites) : rotation around Z(=EH), angle from ascending node to X(=ER). CoordAtoB_BrotationA (RotationZA, CoordInA[1], CoordInA[2], Uu, vv ) --Then the nutation: nutation around the node uu, Theta = NutationU '(inclin for satellites) : rotation around the ascending node, angle from ZB to ZA(=EH). CoordAtoB_BrotationA (NutationU, vv, CoordInA[3], Tau, CoordInB[3]) --Finally the precession: rotation around Z0, Psi = PrecessionZB '(Om pour satellites) : rotation around Z0(=ez0), angle between X0 and ascending node line. CoordAtoB_BrotationA ( PrecessionZB, Uu, Tau, CoordInB[1], CoordInB[2]) /*Note The most common sequence associated with the name Euler angles is (3; 1; 3), named for Leonhard Euler, an 18th- century Swiss mathematician and physicist. To disambiguate it from the other conventions that share the same name, it is also known as the x-convention. In the study of the gyroscopic motion of a spinning rigid body, the Euler angles are known respectively as spin (i.e. rotation propre), nutation, and precession. Ref Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors, James Diebel, Stanford University, Stanford, California 94301-9010 Email: diebel@stanford.edu, 20 October 2006*/ END FUNCTION FUNCTION NO_TYPE CoordAqaterBmatrix (IN REAL Q[4] "Quaternions coordinates wrt frame A , real in Q[1] (-)", OUT REAL MatrixAtoB[3,3], IN STRING Message="" ) "Frame change matrix to the one oriented by the Quaternion Vector[B] = MatrixAtoB * Vector[A]" DECLS REAL Qinv[4],QMXinv[4,4],QMXhat[4,4] BODY -- according to the quaternions equation for QAtoB: V[A]=Q.V[B].Qinv i.e. V[B]=Qinv.V[A].Q = Qinv.[Qhat]V[A]=[Qinv]*[Qhat]V[A] -- Hence [Mat]=[Q inv]*[Q hat] and using only the lower right 3x3 sub matrix for vectors CheckUnitQ(Q, concatStrings("routine CoordAqaterBmatrix ", Message)) Qinverse (Q,Qinv) QMatrix(Qinv,QMXinv) QMatrixHat(Q,QMXhat) FOR (i IN 1,3) FOR (k IN 1,3) MatrixAtoB[i,k]=SUM(j IN 1,4; QMXinv[i+1,j]*QMXhat[j,k+1]) --only the lower right 3x3 sub matrix is used for vectors END FOR END FOR --light proof : th=180° cos th/2=0 Q1=0 u=ez Q2=0 Q3=0 Q4=sin th/2 =1 VB(1)=-VA(1) VB(2)=-VA(2) VB(3)=VA(3) END FUNCTION FUNCTION NO_TYPE CoordAqaterBmatrixEnum (IN REAL Q[4] "Quaternions coordinates, real in Q[1] (-)", OUT REAL MatrixEnum[] ) "Frame change matrix to the one oriented by the Quaternion Vector[B] = MatrixAtoB * Vector[A]" BODY CoordAqaterBmatrix (Q, MatrixEnum, " called CoordAqaterBmatrixEnum ") END FUNCTION FUNCTION NO_TYPE CoordAcardanBmatrix (IN REAL psi "Yaw angle (rad)", IN REAL theta "Pitch angle (rad)", IN REAL phi "Roll angle (rad)", OUT REAL MatrixAtoB[3,3] ) "Frame change matrix to the one oriented by the Cardan Vector[B] = MatrixAtoB * Vector[A]" -- from Matrix * Vector[FrameA] gives Vector[FrameB] where FrameB (x,y,z)= (FrameA(X,Y,Z) rotated by the Cardan angles psi, theta, phi on yaw, pitch, roll around Z, Y', x ) -- the vector does not change, just its coordinates are written in FrameB DECLS REAL cost, sint, cosph,sinph, cosps,sinps BODY --matrix of frame change cf. cours technologie spatiale CNES, module 12, p128: cost=cos(theta) --yaw sint=sin(theta) cosph=cos(phi) --pitch sinph=sin(phi) cosps=cos(psi) --roll sinps=sin(psi) MatrixAtoB[1, 1] = cost * cosps MatrixAtoB[1, 2] = cost * sinps MatrixAtoB[1, 3] = - sint MatrixAtoB[2, 1] = sinph * sint * cosps - cosph * sinps MatrixAtoB[2, 2] = sinph * sint * sinps + cosph * cosps MatrixAtoB[2, 3] = sinph * cost MatrixAtoB[3, 1] = cosph * sint * cosps + sinph * sinps MatrixAtoB[3, 2] = cosph * sint * sinps - sinph * cosps MatrixAtoB[3, 3] = cosph * cost --light proof : for angles null except psi=+90° sin ps=1 thus rotation of B wrt A =+90° VB(1)=VA(2) VB(2)=-VA(1) VB(3)=VA(3) /* Note The angles associated with the Euler sequence (1; 2; 3) are sometimes called Cardan angles, for Gerolamo Cardano, an Italian Renaissance mathematician; Tait-Bryan angles, for Peter Guthrie Tait, a 19th-century Scottish mathematical physicist; or nautical angles. They are commonly used in aerospace engineering and computer graphics. Despite the lack of consensus on the issue, these angles are also commonly referred to simply as Euler angles in the aeronautics field, are known respectively as roll, pitch, and yaw, or, equivalently, bank, attitude, and heading. Ref Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors, James Diebel, Stanford University, Stanford, California 94301-9010 Email: diebel@stanford.edu, 20 October 2006*/ END FUNCTION FUNCTION NO_TYPE CoordECIeulerOrbQuater ( REAL OM, REAL inc, REAL omPHI, OUT REAL Q[4]) "Compute the Quaternion that orient FrameA to FrameB from the ECI to Orb with Euler's angles from ECI to Pol" DECLS REAL Q1[4],Q2[4],Q3[4], Q4[4],Q5[4],QQ[4],QQQ[4],QQQQ[4] BODY --transform first ECI to Pol Q1[1]=cos(OM/2.)--precession around Zq Q1[2]=0 Q1[3]=0 Q1[4]=sin(OM/2.) -- Q2[1]=cos(inc/2.)--nutation around node U, i.e. X local after precession Q2[2]=sin(inc/2.) Q2[3]=0 Q2[4]=0 -- Q3[1]=cos((omPHI)/2.)--spin (rotation propre) around Zp, i.e. Z local after nutation Q3[2]=0 Q3[3]=0 Q3[4]=sin((omPHI)/2.) -- --In addition transform Pol to Orb the specific orbital axis for telecom satellite -- Q4[1]=cos((PI/2)/2.)--rotation around Zp, i.e. Z local by + PI/2 for putting the previous Xp (along Radius from focus) to X in Orb Axis (in horizontal plane i.e. perpendicular to radius) Q4[2]=0 Q4[3]=0 Q4[4]=sin((PI/2)/2.) -- Q5[1]=cos((-PI/2)/2.) --finally: around X in Orb Axis by -PI/2 for putting the previous Zp toward Earth to get Z in Orb Axis Q5[2]=sin((-PI/2)/2.) Q5[3]=0 Q5[4]=0 -- -- QQ=...Q3*Q2*Q1 when all Qi are wrt inertial axis BUT HERE better QQ=Q1*Q2*Q3... where Qi are wrt previous local axes Q1productQ2 (Q1, Q2, QQ,"prod Q1, Q2 in CoordECIeulerOrbQuater") Q1productQ2 (QQ, Q3, QQQ,"prod Q3 in CoordECIeulerOrbQuater") Q1productQ2 (QQQ, Q4, QQQQ,"prod Q4 in CoordECIeulerOrbQuater") Q1productQ2 (QQQQ, Q5, Q,"prod Q5 in CoordECIeulerOrbQuater") END FUNCTION FUNCTION NO_TYPE CoordAqaterBeuler --NOT USED not checked (OUT REAL OM, OUT REAL inc, OUT REAL omPHI, REAL Q[4]) "Compute the Euler's angles that orient FrameA to FrameB from the Quaternion " DECLS REAL CheckAcos BODY --CheckUnitQ(Q) omPHI = atan2(Q[2]*Q[4] -Q[1]*Q[3] , Q[1]*Q[2] +Q[3]*Q[4]) inc = acos( min( max( 2.*(Q[1]**2+Q[4]**2)-1 , -1.),1.) ) -- floor top limits for robustness again rounded errors OM = atan2(Q[2]*Q[4] +Q[1]*Q[3] , Q[1]*Q[2] -Q[3]*Q[4]) CheckAcos= 2.*(Q[1]**2+Q[4]**2)-1 IF (abs(CheckAcos)>1 ) THEN ErrRounding (abs(CheckAcos)-1,1E-10,"ERROR CoordAqaterBeuler acos but mastered with min( max( , -1.),1.) ") END IF /*s=Q[1] x=Q[2] y=Q[3] z=Q[4] OM = atan2 (z, s)+atan2 (y, x) inc = arccos (s**2+z**2-x**2-y**2) omPHI = atan2 (z, s)-atan2 (y, x)*/ END FUNCTION FUNCTION NO_TYPE CoordOrbCardanSatQuater (REAL psi "Yaw angle (rad)", REAL theta "Pitch angle (rad)", REAL phi "Roll angle (rad)" , OUT REAL Q[4] "Quaternions coordinates wrt frame A with real in Q[1] (-)") "Compute the Quaternion that orient FrameA to FrameB : from Orb to Sat, from the cardan angles psi, theta, phi on yaw, pitch, roll around Z, Y', x " DECLS REAL Q1[4],Q2[4],Q3[4], QQ[4] BODY Q1[1]=cos(psi/2.) -- yaw on Z Q1[2]=0 Q1[3]=0 Q1[4]=sin(psi/2.) -- Q2[1]=cos(theta/2.)-- pitch on Y local after yaw Q2[2]=0 Q2[3]=sin(theta/2.) Q2[4]=0 -- Q3[1]=cos(phi/2.)--roll on X local after pitch Q3[2]=sin(phi/2.) Q3[3]=0 Q3[4]=0 -- QQ=Q3*Q2*Q1 when all Qi are wrt inertial axis but HERE BETTER QQ=Q1*Q2*Q3 where Qi are wrt previous local axes Q1productQ2 (Q1, Q2, QQ,"prod Q1, Q2 in CoordOrbCardanSatQuater") Q1productQ2 (QQ, Q3, Q,"prod Q3 in CoordOrbCardanSatQuater") END FUNCTION FUNCTION NO_TYPE CoordAquaterBcardan (OUT REAL psi "Yaw angle (rad)", OUT REAL theta "Pitch angle (rad)", OUT REAL phi "Roll angle (rad)" , REAL Q[4] "Quaternions coordinates wrt frame A with real in Q[1] (-)" ) "Compute the Cardan angles psi, theta, phi on yaw, pitch, roll around Z, Y', x that orient FrameA to FrameB from Quaternion " DECLS REAL CheckAsin BODY CheckUnitQ(Q,"routine CoordAquaterBcardan" ) --atan2(sinx,cosx) -- =ATAN(sin/cos) psi = atan2(2.*(Q[2]*Q[3] + Q[1]*Q[4]), Q[1]**2 + Q[2]**2 - Q[3]**2 - Q[4]**2) theta = -asin( min( max( 2.*(Q[2]*Q[4] - Q[1]*Q[3]) , -1.),1.) ) -- floor top limits for robustness again rounded errors phi = atan2(2.*(Q[3]*Q[4] + Q[1]*Q[2]), Q[1]**2 - Q[2]**2 - Q[3]**2 + Q[4]**2) CheckAsin= 2.*(Q[2]*Q[4] - Q[1]*Q[3]) IF (abs(CheckAsin)>1 ) THEN ErrRounding (abs(CheckAsin)-1,1E-10,"ERREUR CoordAquaterBcardan asin but mastered with min( max( , -1.),1.) ") END IF --light proof : th=90° cos th/2=0.7 Q1=0.7 u=ey Q2=0 Q3=sin th/2 =0.7 Q4=0 psi=0 phi=0 theta = -asin(-2*0.7*0.7)=90° END FUNCTION FUNCTION NO_TYPE CoordPolEulerECI (REAL CoordOrbitalPolar[], REAL Precession, REAL Nutation, REAL Rotation, OUT REAL CoordECI[]) "ECI after Precession, Nutation, Rotation gives Pol Axis --this is not Orb for telecom satellites-- " BODY CoordAtoB_BeulerA( CoordOrbitalPolar, Precession, Nutation, Rotation, CoordECI) --correct B=ECI from ECI we have the Euler's angles to get A=OrbitalPolar END FUNCTION FUNCTION NO_TYPE ChangeEnum (REAL X[] "whatever enum or num array", OUT REAL Xn[] "whatever num or enum array" ) "Change the type of enum or num" DECLS INTEGER n BODY n=sizeArrayReal(X[1], 0) FOR (i IN 1,n) Xn[i]=X[i] END FOR --Note:An array with an enumerative index "t" requires the array declaration -- in the function header to define the dimension (e.g. REAL V[Chemicals]) END FUNCTION FUNCTION STRING EventsThrusters (INTEGER n,REAL signal[n]) "produce a simple compact log of active devices Thrusters1.logEventsThrusters string = 7 9" DECLS STRING Event = "" REAL ii BODY FOR (i IN 1,n) IF (signal[i]==1 )THEN ii=i Event=concatStrings(concatStrings(Event," "),realToString(ii)) END IF END FOR RETURN Event END FUNCTION --FUNCTION for the FRAME COMPONENT FUNCTION NO_TYPE DervativeQmatrixSatAxis (REAL Wrot[Sat], OUT REAL DQmat[4,4]) "gives the Dervative matrix for quaternions" -- Qdot= 0.5* DQmat *Q ( in quaternions theory: Qdot= 0.5* Q .Wrot where . is the quaternion product, and Wrot considered as a pure quaternion in Sat) DECLS REAL QWrot[4] INTEGER ii=1 BODY QWrot[ii]=0 FOR (t IN Sat) ii=ii+1 QWrot[ii]=Wrot[t] END FOR QMatrixHat(QWrot,DQmat) END FUNCTION FUNCTION NO_TYPE InertiaMatrixSatAxis2 (REAL Ixxyyzz[Sat],REAL Ixyyzzx[Sat], OUT REAL InertiaMatrixSatFrozen[Sat,Sat]) "Set the inertia matrix from the inputs" BODY InertiaMatrixSatFrozen[x,x] =Ixxyyzz[x] InertiaMatrixSatFrozen[y, y] =Ixxyyzz[y] InertiaMatrixSatFrozen[z, z] =Ixxyyzz[z] InertiaMatrixSatFrozen[x, y] =-Ixyyzzx[x] InertiaMatrixSatFrozen[y, z] =-Ixyyzzx[y] InertiaMatrixSatFrozen[z, x] =-Ixyyzzx[z] --symmetrise (not skew symm) InertiaMatrixSatFrozen[y, x] = InertiaMatrixSatFrozen[x, y] InertiaMatrixSatFrozen[x, z] = InertiaMatrixSatFrozen[z, x] InertiaMatrixSatFrozen[z, y] = InertiaMatrixSatFrozen[y, z] -- [row, column] END FUNCTION FUNCTION NO_TYPE InertiaMatrixLoop (INTEGER n4,INTEGER Nodes, INTEGER i,INTEGER l,INTEGER m,INTEGER n,OUT REAL MinertiaVolume[n4,Sat,Sat],REAL dV_element,REAL OTankNodes[n4,Nodes,Nodes,Nodes,Sat ]) "for a loop on the indexes l ,m, n compute the the inertia/rho increments (i.e. by volume) and add to the matrix" BODY --diagonal positive Ixx, etc. MinertiaVolume[i,x, x ] =MinertiaVolume[i,x, x ]+dV_element *(OTankNodes[i,l,m,n,y ]**2+OTankNodes[i,l,m,n,z ]**2) MinertiaVolume[i,x, y ] =MinertiaVolume[i,x, y ]-dV_element *OTankNodes[i,l,m,n,x ]*OTankNodes[i,l,m,n,y ] MinertiaVolume[i,x, z ] =MinertiaVolume[i,x, z ]-dV_element *OTankNodes[i,l,m,n,x ]*OTankNodes[i,l,m,n,z ] MinertiaVolume[i,y, x ] =MinertiaVolume[i,x, y ] MinertiaVolume[i,y, y ] =MinertiaVolume[i,y, y ]+dV_element *(OTankNodes[i,l,m,n,x ]**2+OTankNodes[i,l,m,n,z ]**2) MinertiaVolume[i,y, z ] =MinertiaVolume[i,y, z ]-dV_element *OTankNodes[i,l,m,n,y ]*OTankNodes[i,l,m,n,z ] MinertiaVolume[i,z, x ] =MinertiaVolume[i,x, z ] MinertiaVolume[i,z, y ] =MinertiaVolume[i,y, z ] MinertiaVolume[i,z, z ] =MinertiaVolume[i,z, z ]+dV_element *(OTankNodes[i,l,m,n,x ]**2+OTankNodes[i,l,m,n,y ]**2) END FUNCTION FUNCTION NO_TYPE InertiaMatrixDisplaced (IN REAL InertiaMatrix[Sat,Sat] "InertiaMatrix wrt reference (by default it is wrt its own COM)", IN REAL Rdisplaced[Sat] "vector from the InertiaMatrix reference to the point wanted (or necessarily to its own COM when FromCOMNotToCOM=FALSE)", IN REAL Mass "total mass of involved in the object characterized by the given InertiaMatrix", OUT REAL InertiaMatrixD[Sat,Sat] "InertiaMatrix displacedby Rd to the new reference", IN BOOLEAN FromCOMNotToCOM=TRUE "when false, the new reference shall be necessarily its own COM") "inertia matrix displaced from its own COM to an other point; with a flag set to FALSE if it is wanted from a point to its own COM (at its own COM the matrix is at minimum amplitude)" -- --Parallel axis theorem... with InertiaMatrix known at its own COM and wanted for any other point. -- Id[i,j] =I[i,j]+ Mass*(Kroneker[i,j]*Rd**2-Rd[i]*Rd[j]) where Rd =Rdisplaced is the vector from its own COM to the point wanted. -- for FromCOMNotToCOM=FALSE, then Id is the wanted Inertia wrt its own COM, while I is already displaced but keeping the rule for Rd: from the InertiaMatrix reference toward the own COM -- hence Id[i,j]+ Mass*(Kroneker[i,j]*Rdopp**2-Rdopp[i]*Rdopp[j]) =I[i,j] with Rdopp=-Rdisplaced DECLS REAL RRIdent3[Sat,Sat]=0 REAL Idelta[Sat,Sat], RR2, RRdisplaced[Sat] BODY FOR (t IN Sat) IF (FromCOMNotToCOM) THEN RRdisplaced[t]=Rdisplaced[t] ELSE RRdisplaced[t]=-Rdisplaced[t] --the vector to COM shall be opposited to get a vector from COM as needed END IF END FOR RR2=SUM(t IN Sat; RRdisplaced[t]**2) FOR (t IN Sat) FOR (u IN Sat) IF (u==t) THEN -- kroneker*R² RRIdent3[t,u]=RR2 END IF Idelta[t,u]=Mass*(RRIdent3[t,u]-RRdisplaced[t]*RRdisplaced[u]) IF (FromCOMNotToCOM) THEN --the part of displaced matrix shall be added (higher inertia when not at its own COM) InertiaMatrixD[t,u]=InertiaMatrix[t,u]+Idelta[t,u] ELSE --the part of displaced matrix shall be deducted (lower inertia when it is at its own COM) InertiaMatrixD[t,u]=InertiaMatrix[t,u]-Idelta[t,u] END IF END FOR END FOR END FUNCTION FUNCTION NO_TYPE InertiaMatrixTotalDisplaced (REAL InertiaMatrixSatFrozenComdry[Sat,Sat],REAL OCOMdry[Sat],REAL massDry , REAL InertiaMatrixFluidsCfluids[Sat,Sat], REAL OCOMFluids[Sat],REAL FluidMassTanks , REAL OCOM[Sat],OUT REAL InertiaMatrixSatFrozen[Sat,Sat]) "inertia matrix non mobile DRY and FLUIDS displaced from their own COM to the current global COM of the sat" DECLS REAL CdryCOM[Sat], CfluidCOM[Sat] REAL InertiaMatrixSatFrozenDry[Sat,Sat],InertiaMatrixFluids[Sat,Sat] BODY aXbYc(-1, OCOMdry ,1, OCOM , 0, CdryCOM ) --vector of displacement from COMdry local to the final current COMsat InertiaMatrixDisplaced( InertiaMatrixSatFrozenComdry, CdryCOM, massDry, InertiaMatrixSatFrozenDry, TRUE) aXbYc(-1, OCOMFluids ,1, OCOM , 0, CfluidCOM ) --vector of displacement COMfluids local to the final current COMsat InertiaMatrixDisplaced( InertiaMatrixFluidsCfluids, CfluidCOM, FluidMassTanks, InertiaMatrixFluids, TRUE) FOR (t IN Sat) FOR (u IN Sat) InertiaMatrixSatFrozen[t,u]=InertiaMatrixFluids[t,u]+InertiaMatrixSatFrozenDry[t,u] END FOR END FOR END FUNCTION --FUNCTIONS for the COMPONENTS FUNCTION NO_TYPE CrossMatrixIndex (INTEGER n17, REAL iV[n17,3], OUT REAL iCMV[n17,3, 3], INTEGER Index) " cross product matrix dedicated for 3 dimensions vector time n17 " DECLS REAL V[3],CMV[3,3] BODY FOR (i IN 1,3) V[i]=iV[Index,i] END FOR CrossMatrix (V, CMV) FOR (i IN 1,3) FOR (j IN 1,3) iCMV[Index,i,j]=CMV[i,j] END FOR END FOR END FUNCTION --FUNCTIONS for Tanks FUNCTION NO_TYPE MatrixFrameChange ( REAL OmegaXo[Sat],REAL OmegaYo[Sat],REAL OmegaZo[Sat], OUT REAL Mom[Sat,Sat]) "set a matrix based on Omega for frame change defined by 3 vectors of base" -- Mom such that Vsat=Mom*Vomega at the same origin DECLS REAL OmegaX[Sat], OmegaY[Sat], OmegaZ[Sat] BODY FOR (t IN Sat) OmegaX[t]=OmegaXo[t] OmegaY[t]=OmegaYo[t] OmegaZ[t]=OmegaZo[t] END FOR --Avoid the Null vector (should be already trapped before) IF ( Norm(OmegaXo)!=0) THEN Normalise (OmegaXo, OmegaX, "function MatrixFrameChange OmegaXi") END IF IF ( Norm(OmegaYo)!=0) THEN Normalise (OmegaYo, OmegaY, "function MatrixFrameChange OmegaYi") END IF IF ( Norm(OmegaZo)!=0) THEN Normalise (OmegaZo, OmegaZ, "function MatrixFrameChange OmegaZi") END IF --IF (Norm(OmegaXo)!=0 AND Norm(OmegaYo)!=0 AND Norm(OmegaZo)!=0) THEN FOR (t IN Sat) Mom[t, x]=OmegaX[t] Mom[t, y]=OmegaY[t] -- correct Y Mom[t, z]=OmegaZ[t] --correct Z END FOR --END IF END FUNCTION FUNCTION REAL AngleVectordeg (REAL A[Sat],REAL B[Sat], OUT REAL cosAlpha, OUT REAL sinAlpha) "gives the angle from vector A to vector B" DECLS REAL aa,bb,sinAlpha_abs,Alphadeg REAL C[Sat] BODY aa=Norm(A) bb=Norm(B) IF (aa!=0 AND bb!=0) THEN cosAlpha=XscalarY(A,B)/(aa*bb) XcrossY(A,B,C) sinAlpha_abs=Norm(C)/(aa*bb) ELSEIF (aa==0 ) THEN RETURN 0 ELSE RETURN 0 END IF sinAlpha =sinAlpha_abs Alphadeg=deg(atan2(sinAlpha, cosAlpha) ) --check IF (cos(rad(Alphadeg))-cosAlpha) >1e-3 THEN Alphadeg=-Alphadeg IF (cos(rad(Alphadeg))-cosAlpha) >1e-3 THEN PRINT ("AngleVectordeg problem with Alphadeg=$Alphadeg") END IF END IF RETURN Alphadeg END FUNCTION FUNCTION NO_TYPE FrameChange ( REAL Mom[Sat,Sat],REAL NodeSatAxis[Sat], REAL OCOM[Sat], OUT REAL NodeOmega[Sat] ) "not used. from a vector in Sat --origin O-- produces the vector in the Omega and F frame starting from current COM" -- Vsat=Mom*Vomega for the same origin BODY FOR (t IN Sat) --vector NodeOmega[t]=SUM (u IN Sat; Mom[u,t]*(NodeSatAxis[u]-OCOM[u])) --inverse mat used, for O to Node from Sat to Omaga frame END FOR --vector END FUNCTION FUNCTION REAL deltaP ( REAL NodeOmega[Sat], REAL NOmeg, REAL NAccel, REAL sinAlpha, REAL cosAlpha, REAL rho) "Function that return the value of the constant dP --units: pressure Pa-- for fulfilling the general archimedes pressure for a given point in the Omega and F frame with origin at the COM" DECLS REAL dP --in Pa BODY --for no rotation and alpha=0, dP reduce to -rho.NAccel.z (i.e. dP= rho.g.h) z=0 =>dP=0 bottom: z<0 dP >0 etc. dP=rho*(NOmeg**2*(NodeOmega[x]**2+NodeOmega[y]**2)/2+NAccel*(sinAlpha*NodeOmega[x]-cosAlpha*NodeOmega[z])) RETURN dP END FUNCTION FUNCTION REAL ComputeDeltaPpoint (REAL Mom[Sat,Sat] UNITS "-" "Matrix based on Omega" , REAL OCOM[Sat] UNITS "m" "Location of the COM with respect to the origin O: vector O to C", REAL NOmeg UNITS "rd/s" "parameter for the function deltaP()", REAL NAccel UNITS "m/s2" "parameter for the function deltaP()", REAL sinAlpha UNITS "-" "parameter for the function deltaP()", REAL cosAlpha UNITS "-" "parameter for the function deltaP()", REAL rho UNITS "kg/m3" "parameter for the function deltaP()", REAL OPoint[Sat] UNITS "m" "Location of a point P with respect to the origin O: vector O to P") "Simple function that return the value of the dP fulfilling the general Archimedes pressure equation under rotation and acceleration for a given point in the sat frame" DECLS REAL PointOmega[Sat] REAL DeltaPpoint BODY -- frame change to Omega and F frame but fromt the current COM location FOR (t IN Sat) --vector PointOmega[t]=SUM (u IN Sat; Mom[u,t]*(OPoint[u]-OCOM[u]) ) -- inverse mat used END FOR --vector DeltaPpoint=deltaP( PointOmega, NOmeg, NAccel, sinAlpha, cosAlpha, rho) RETURN DeltaPpoint -- in Pa END FUNCTION FUNCTION NO_TYPE ComputeDeltaPmap (INTEGER n4 UNITS "-" "number of tanks", INTEGER Nodes UNITS "-" "number of nodes per axis", REAL Mom[Sat,Sat] UNITS "-" "Matrix based on Omega", REAL OCOM[Sat] UNITS "m" "Location of the COM with respect to the origin O: vector O to C", REAL NOmeg UNITS "rd/s" "parameter for the function deltaP()", REAL NAccel UNITS "m/s2" "parameter for the function deltaP()", REAL sinAlpha UNITS "-" "parameter for the function deltaP()", REAL cosAlpha UNITS "-" "parameter for the function deltaP()", REAL rho[n4] UNITS "kg/m3" "parameter for the function deltaP()", REAL OTankNodes[n4,Nodes,Nodes,Nodes,Sat ] UNITS "m" "All the nodes location (fixed in Sat)", OUT REAL IsoDeltaPmap[n4,Nodes,Nodes,Nodes ] UNITS "Pa" "iso dP values wrt COM of the Sat") "Function that return the value of the dP fulfilling the general Archimedes pressure equation under rotation and acceleration for a given map of points in the Omega and F frame" DECLS REAL NodeOmega[Sat] BODY FOR (i IN 1,n4)--num tank -- frame change to Omega and F frame but fromt the current COM location FOR (l IN 1,Nodes) --along length Z FOR (m IN 1,Nodes)--along X FOR (n IN 1,Nodes) --along Y FOR (t IN Sat) --vector NodeOmega[t]=SUM (u IN Sat; Mom[u,t]*(OTankNodes[i,l,m,n,u]-OCOM[u]) ) -- inverse mat used END FOR --vector IsoDeltaPmap[i,l,m,n ] =deltaP( NodeOmega, NOmeg, NAccel, sinAlpha, cosAlpha, rho[i]) END FOR END FOR END FOR END FOR --num tank END FUNCTION FUNCTION NO_TYPE MinMaxIsoDeltaPmap (INTEGER n4, INTEGER Nodes, REAL IsoDeltaPmap[n4,Nodes,Nodes,Nodes ] , OUT REAL IsoDeltaPmin[n4], OUT REAL IsoDeltaPmax[n4]) "Function that return the MinMax value of the iso dP from IsoDeltaPmap" DECLS REAL NodeOmega[Sat] BODY FOR (i IN 1,n4)--num tank IsoDeltaPmin[i]=IsoDeltaPmap[i,1,1,1 ] --init IsoDeltaPmax[i]=IsoDeltaPmap[i,1,1,1 ] --init FOR (l IN 1,Nodes) --along length Z FOR (m IN 1,Nodes)--along X FOR (n IN 1,Nodes) --along Y IF (IsoDeltaPmin[i]>IsoDeltaPmap[i,l,m,n ] ) THEN IsoDeltaPmin[i]=IsoDeltaPmap[i,l,m,n ] END IF IF (IsoDeltaPmax[i] to the given one --When reached, make refinements to find the free surface and adjust the size of the free surface layer for getting the given volume -- Details: -- To find the volume, loock-up for all the nodes and output a approximate volume(Iso-DeltaP) by cumul of elementary volumes fulfilling the condition on Iso-DeltaP -- within the main loop, make Test volume wrt the given volume and if over perform two consecutive loops -- First loop on the volume for all the layers 'lower' than the last one -- Second and last continuation loop on the volume for the free surface (last layer) with linearisation to fit the volume -- then set a flag to exit the main loop on Iso-DeltaP without doing anything else. -- btw, this process allow to compute the COM and the inertia safely as well as to take the best point on the free surface.. --------------------------------------------------------------------------------------------- FOR (cc IN 1,NodesdeltaP+1) --loop on the IsoDeltaP value including the boundaries for the cumul of elementary volumes. --------------------------------------------------------------------------------- IsoDeltaP=IsoDeltaPbottom+step*(cc-1) IF (flag==0) THEN VolumeFluidApprox[i]=0 --for each IsoDeltaP compute the volume: here rough volume FOR (l IN 1,Nodes) --along length Z FOR (m IN 1,Nodes)--along X FOR (n IN 1,Nodes) --along Y IF (IsoDeltaP<=IsoDeltaPmap[i,l,m,n ] ) THEN --Test isoDP VolumeFluidApprox[i]=VolumeFluidApprox[i]+dV_element --update IF(SurfaceIsoDeltaPmin>IsoDeltaPmap[i,l,m,n ] ) THEN SurfaceIsoDeltaPmin=IsoDeltaPmap[i,l,m,n ] --the minimum of the liquid phase is wanted: that will be on the surface. END IF END IF --Test isoDP END FOR END FOR END FOR --Test volume IF (VolumeFluidApprox[i]>=VolumeFluid[i] OR cc == NodesdeltaP+1) THEN --completed and case full tank added with cc == NodesdeltaP+1 VolumeFluidApproxRough=VolumeFluidApprox[i] /* IF NOT((VolumeFluidApprox[i]>=VolumeFluid[i] AND flag==0 ))THEN PRINT ( "Full volume reached: for $VolumeFluidApproxRough $NodesdeltaP +1 ") END IF */ SurfaceIsoDeltaP[i]=SurfaceIsoDeltaPmin --IsoDeltaP --save the last value iOutlet=Nodes/2+1 --for outlet by definition l=1 and m=Nodes/2 n=Nodes/2 but must be integer --case odd: start 1 end n=5 : the best one is the 3rd node = n/2+1 in integer IsoDeltaP_outlet=IsoDeltaPmap[i,1,iOutlet,iOutlet ] IF (2*iHalfNodes==Nodes) THEN --case even => middle between 3 and 2 for n=4 IsoDeltaP_outlet=(IsoDeltaPmap[i,1,iOutlet,iOutlet ] +IsoDeltaPmap[i,1,iOutlet-1,iOutlet-1 ] )/2 END IF dPoutlet[i]=IsoDeltaP_outlet-SurfaceIsoDeltaP[i] --IsoDeltaP --in Pa --redo but only for the right IsoDeltaP with much more info VolumeFluidApprox[i]=0 npt=0 --First loop on the volume for all the layers 'lower' than the free surface (the last one) FOR (l IN 1,Nodes) --along length Z FOR (m IN 1,Nodes)--along X FOR (n IN 1,Nodes) --along Y IF (SurfaceIsoDeltaP[i]<=(IsoDeltaPmap[i,l,m,n ]+step) ) THEN --Test isoDP up the layer before the last one FOR (t IN Sat) --barycenter xG=(m.X+dm.Xd)/(m+dm) COMtank[i,t]=(VolumeFluidApprox[i]*COMtank[i,t]+dV_element *OTankNodes[i,l,m,n,t ])/(VolumeFluidApprox[i]+dV_element) --frame Sat (i/f Launcher) END FOR InertiaMatrixLoop (n4, Nodes, i, l, m, n, MinertiaVolume, dV_element, OTankNodes) VolumeFluidApprox[i]=VolumeFluidApprox[i]+dV_element --update Volpreviouslayer=VolumeFluidApprox[i] END IF --Test isoDP END FOR END FOR END FOR --Second and last continuation loop on the volume for the free surface (last layer) with linearisation to fit the volume FOR (l IN 1,Nodes) --along length Z FOR (m IN 1,Nodes)--along X FOR (n IN 1,Nodes) --along Y IF (SurfaceIsoDeltaP[i]>(IsoDeltaPmap[i,l,m,n ]+step) AND SurfaceIsoDeltaP[i]<=IsoDeltaPmap[i,l,m,n ] ) THEN -- last layer only dV_elementAdjusted=(VolumeFluid[i]-Volpreviouslayer )/(VolumeFluidApproxRough-Volpreviouslayer)*dV_element --linearisation wrt the volume for this layer FOR (t IN Sat) --barycenter xG=(m.X+dm.Xd)/(m+dm) COMtank[i,t]=(VolumeFluidApprox[i]*COMtank[i,t]+dV_elementAdjusted*OTankNodes[i,l,m,n,t ])/(VolumeFluidApprox[i]+dV_elementAdjusted) --roughly trimed with only the right mass of the element (but location still a bit not exact) END FOR InertiaMatrixLoop (n4, Nodes, i, l, m, n, MinertiaVolume, dV_elementAdjusted, OTankNodes) VolumeFluidApprox[i]=VolumeFluidApprox[i]+dV_elementAdjusted --update IF(SurfaceIsoDeltaPmin==IsoDeltaPmap[i,l,m,n ] ) THEN PointFreeSurfIndex[i,1]=l --along Z PointFreeSurfIndex[i,2]=m --X PointFreeSurfIndex[i,3]=n --Y END IF --accessory: selection of the nodes of the free surface for plots npt=npt+1 IF (npt<=1000) THEN FOR (t IN Sat) MapFreeSurface[i,npt,t] = OTankNodes[i,l,m,n,t] END FOR END IF END IF --Test isoDP END FOR END FOR END FOR FOR (t IN Sat) PointFreeSurface[i,t] = OTankNodes[i,PointFreeSurfIndex[i,1],PointFreeSurfIndex[i,2],PointFreeSurfIndex[i,3],t] END FOR --clear the map after the last used FOR (nptplus IN npt+1, min(npt+100,1000)) FOR (t IN Sat) MapFreeSurface[i,nptplus,t] = 0 END FOR END FOR flag=1 --no more work required in the loop cc END IF --on volume END IF --with flag==0 --------------------------------------------------------------------------------- END FOR --cc ELSE --weightlessness case: tank COM in the centre, dP=0 , fluid sticked on the tank wall inb=0 --number of nodes flag=0 -- flag to trig the first time the volume has been reached VolumeWall=0 dV_elementAdjusted=0 FOR (nn IN 0,iHalfNodes) IF (VolumeWall=(Nodes+1-iSurface) OR m>=(Nodes+1-iSurface) OR n>=(Nodes+1-iSurface) ) THEN --PRINT ("Volume on the walls $VolumeWall loop $VolumeWallLoop dV_elementAdjusted,dV_element $dV_elementAdjusted $dV_element") ConditionOnlyLastLayer=(l==iSurface OR m==iSurface OR n==iSurface OR l==(Nodes+1-iSurface) OR m==(Nodes+1-iSurface) OR n==(Nodes+1-iSurface)) ConditionOnlyLastLayer=ConditionOnlyLastLayer AND (l>=iSurface AND m>=iSurface AND n>=iSurface AND l<=(Nodes+1-iSurface) AND m<=(Nodes+1-iSurface) AND n<=(Nodes+1-iSurface) ) IF ( ConditionOnlyLastLayer ) THEN VolumeWallLoop=VolumeWallLoop+dV_elementAdjusted InertiaMatrixLoop (n4, Nodes, i, l, m, n, MinertiaVolume, dV_elementAdjusted, OTankNodes) ELSE VolumeWallLoop=VolumeWallLoop+dV_element InertiaMatrixLoop (n4, Nodes, i, l, m, n, MinertiaVolume, dV_element, OTankNodes) END IF END IF END FOR END FOR END FOR --PRINT ("Volume on the walls $VolumeWall recomputed in loop $VolumeWallLoop for iSurface=$iSurface $dV_element adjust $dV_elementAdjusted for $inbNodesLayer ") FOR (j IN 1,3) PointFreeSurfIndex[n4,j]=iSurface END FOR iCentre=iHalfNodes+1.01 --for the centre but integer: start 1 end n=5 : the best one is the 3rd node = n/2+1 in integer FOR (t IN Sat) PointFreeSurface[i,t] = OTankNodes[i,PointFreeSurfIndex[i,1],PointFreeSurfIndex[i,2],PointFreeSurfIndex[i,3],t] --l m n IF (2*iHalfNodes==Nodes)THEN --case even => middle between 3 and 2 for n=4 COMtank[i,t]= (OTankNodes[i,iCentre,iCentre,iCentre,t] +OTankNodes[i,iCentre-1,iCentre-1,iCentre-1,t] )/2 --centre of tank ELSE --Case odd COMtank[i,t]= OTankNodes[i,iCentre,iCentre,iCentre,t] --centre of tank simple with the natural middle node END IF END FOR VolumeFluidApprox[i]=VolumeFluid[i] dPoutlet[i]=0 END IF -- weightlessness case END FOR --num tank END FUNCTION ------------------------------------------------------------------------------------------------------- --Functions specialised for Orbits FUNCTION REAL JulianDay (STRING DDMMYYYYHHMMSS, OUT REAL Day1900) "compute the Julian day from a Gregorian Calendar date in string; JD Julien day counting starting from 0 in -4712 i.e. 4713BC; function ok for dates > 1500 " DECLS INTEGER Day, Month, Year, Hour, Minute, Second REAL v1, v2 INTEGER a, y, m, BC=0 "Before Christ" REAL JD=0 REAL JD1900=2415020.5 --The Julian date for 1900 January 1 00:00:00.0 UT; Reference for Microsoft Excel BODY v1=stringToReal(DDMMYYYYHHMMSS) IF (v1<0) THEN BC=1 v1=abs(v1) END IF Day=v1/1E12 -- all in Integers Month=(v1-Day*1E12)/1E10 Year=(v1-Day*1E12-Month*1E10)/1E6 IF (BC==1) THEN Year=-Year END IF v2=v1-Day*1E12-Month*1E10-Year*1E6 Hour=v2/1E4 Minute=(v2-Hour*1E4)/1E2 Second=(v2-Hour*1E4-Minute*1E2) a=Integer((14-Month)/12) y=Year+4800-a m=Month+12*a-3 JD=Day+Integer((153*m+2)/5)+365*y+Integer(y/4)-Integer(y/100)+Integer(y/400)-32045 +(Hour-12)/24+Minute/1440+Second/86400 IF (Day/10000+Month/100+Year<=1582.1004) THEN JD=JD+10 END IF IF (Day/10000+Month/100+Year>1582.1004 AND Day/10000+Month/100+Year<=1582.1014) THEN PRINT ("minor error: the given date is not belonging to the Gregorian calendar") END IF --PRINT ("Day, Month, Year, Hour, Minute, Second, v1, v2 $Day, $Month, $Year, $Hour, $Minute, $Second, $v1, $v2") Day1900= JD-JD1900 RETURN JD END FUNCTION FUNCTION NO_TYPE CoordRVECItoEulerAngles (REAL R[ECI], REAL V[ECI], OUT REAL RAAN, OUT REAL inc, OUT REAL omPHI, REAL GMfocus, REAL Rfocus, OUT REAL AltApo, OUT REAL AltPer, OUT REAL Alt, OUT REAL Exc, OUT REAL om, OUT REAL Phi, OUT REAL sma) "Compute the Euler angles that orient FrameECI to FrameORB from orbital R V in ECI and several Keplerian parameters (osculating) of the orbit " DECLS REAL Checkasin, CheckNorm, eR[ECI], eV[ECI], H[ECI], eH[ECI] REAL eX[ECI], eY[ECI], eZ[ECI], mR[ECI] REAL Cosi, Sini, CosOm,SinOm, CosomPhi, SinomPhi REAL vv, rr, hh, Energy, CosE,SinE,cosphi,sinphi,e, mean,CoshH,SinhH,HH,n,dtp BODY XcrossY (R, V, H) --orbital angular momentum axis Normalise(R, eR, "function CoordRVECItoEulerAngles R") Normalise(V, eV, "function CoordRVECItoEulerAngles V") Normalise(H, eH, "function CoordRVECItoEulerAngles H") XcrossY (eH, eR, eX) -- gives the X axis in horizontal plane Cosi = eH[Zq] Sini = abs(1 - eH[Zq]**2) ** 0.5 -- inclination is by definition of the RAAN in 0 to 180° always sinus i is positive IF (abs(Cosi)-1 >1E-6) THEN PRINT (" ERROR in CoordRVECItoEulerAngles cos >+-1 $Cosi") END IF inc=acos(max(-1,min(1,Cosi))) -- by definition always in 0 to 180° IF ( inc== 0) THEN CosOm = 1--convention for this case SinOm = 0 CosomPhi = (eR[Xq] * CosOm + eR[Yq] * SinOm) --eR[Xq] SinomPhi = eR[Zq]--eR[Yq] --PRINT ("CASE special Inclination NULL 00000") ELSE CosOm = -eH[Yq] / Sini SinOm = eH[Xq] / Sini CosomPhi = (eR[Xq] * CosOm + eR[Yq] * SinOm) SinomPhi = eR[Zq] / Sini END IF RAAN = atan2( SinOm, CosOm) omPHI = atan2( SinomPhi, CosomPhi) --Conventional Orb for TelecomSat : Z toward Earth centre, Y = -Angular momentum (Y about south for GEO in winter) and X=Z^Y (X about along velocity for GEO) vv=Norm(V) rr=Norm(R) hh=Norm(H) Energy=vv ** 2 / 2 - GMfocus / rr IF (Energy < 0) THEN --ellipse sma = -GMfocus / 2 / Energy Exc = abs(1 - hh**2 / GMfocus / sma)** 0.5 CosE = 1 / Exc * (1 - rr / sma) SinE = XscalarY(R, V) / Exc / (GMfocus * sma) ** 0.5 cosphi = (CosE - Exc) / (1 - Exc * CosE) sinphi = abs(1 - Exc ** 2) ** 0.5 * SinE / (1 - Exc * CosE) e = atan2( SinE, CosE) --atan2(sinx,cosx) -- =ATAN(sinx/cosx) checked atan2(x,y)inverse tangent of x/y in the range of [-PI; PI] radians. mean = e - Exc * SinE --mean motion ELSE --case not checked not tested sma = GMfocus / 2 / Energy Exc = (1 + hh ** 2 / GMfocus / sma) ** 0.5 CoshH = 1 / Exc * (1 + rr / sma) SinhH = XscalarY(R, V) / Exc / (GMfocus * sma) ** 0.5 cosphi = (CoshH - Exc) / (1 - Exc * CoshH) --mean motion sinphi = abs(Exc ** 2 - 1) ** 0.5 * SinhH / (1 - Exc * CoshH) HH = 1/2*(log(1+SinhH / CoshH)-log(1-SinhH / CoshH)) --atanh(SinhH / CoshH) mean = -HH + Exc * sinh(HH) --mean motion END IF n = (GMfocus / sma ** 3) ** 0.5 dtp = mean / n Phi = atan2(sinphi,cosphi) om = omPHI - Phi AltApo=sma*(1+Exc) -Rfocus AltPer=sma*(1-Exc) -Rfocus Alt=rr-Rfocus END FUNCTION FUNCTION NO_TYPE MoonSunECI (REAL JD, OUT REAL ReMo[ECI ], OUT REAL ReS[ECI ]) "produce the location of Moon and Sun in ECI equatorial for dates in 2000 +-100 years" -- JD Julien day. --Cartesian coordinate ecliptic heliocentric for the radius Sun to Earth, Sun to Moon --Cartesian coordinate ecliptic geocentric for the radius Earth to Moon then transformed into equatorial geocentric for the radius Earth to Moon DECLS REAL L0[25], Lp[25], aMajorsp[25], om0[17], omP[17], OM0[17], OMp[17], exc[17], incl[17] REAL IterationKeplersp[25], DiamKm[25], M0[25], A0[25], B0[25], C0[25], FoyerSatellites[25] REAL RsMoEcliptic[3], RsEEcliptic[3], ReMoEcliptic[3] ,ReSEcliptic[3] REAL PI= 3.141592653589--7932384626433832795 REAL AU = 149.597870E9 --astronomic unit (m) REAL JDbouiges,omB ,LorbitalSun,MorbitalSun,l,OM,M,Diff, F, Ms, b, LongOrb, v, u,LtildaVraie, Ltilda,lh, Bs, Rs, parallaxe INTEGER i REAL CosAngle,SinAngle BODY JDbouiges=JD- 694325 - 1721059.5 --number of days lasted from 0 January 1901 at 0 h. i = 3 --EARTH L0[i] = 4.8689 --References: S. Bouiges, page 153, data ligne 280.... Lp[i] = 0.0172027914 om0[i] = 4.9085 omP[i] = 0.00000081856 OM0[i] = 0 OMp[i] = 0 exc[i] = 0.01675104 incl[i] = 0 aMajorsp[i] = 1.00000023 IterationKeplersp[i] = 3 omB = om0[3] + omP[3] * JDbouiges LorbitalSun = L0[3] + Lp[3] * JDbouiges MorbitalSun = L0[3] + Lp[3] * JDbouiges - omB -- LONGITUDE MOON p156 S. Bouiges l = 0.57999 + 0.229971503 * JDbouiges OM = 4.1867 - 0.00092422 * JDbouiges M = 0.3193 + 0.228027133 * JDbouiges Diff = l - LorbitalSun F = l - OM Ms = MorbitalSun l = l + 0.1098 * sin(M) + 0.003728 * sin(2 * M) + 0.01149 * sin(2 * Diff) l = l - 0.003329 * sin(Ms) + 0.02224 * sin(2 * Diff - M) - 0.001945 * sin(2 * F) l = l + 0.001026 * sin(2 * Diff - 2 * M) + 0.0009983 * sin(2 * Diff - M - Ms) l = l + 0.0009903 * sin(2 * Diff + M) + 0.0008011 * sin(2 * Diff - Ms) l = l + 0.0007156 * sin(M - Ms) - 0.0005323 * sin(M + Ms) - 0.0006074 * sin(Diff) -- LATITUDE MOON b = 0.08995 * sin(F) + 0.0049 * sin(M + F) + 0.00485 * sin(M - F) + 0.003 * sin(2 * Diff - F) b = b + 0.00097 * sin(2 * Diff - M + F) + 0.0008 * sin(2 * Diff - M - F) + 0.00057 * sin(2 * Diff + F) --distance EARTH MOON (page 49) parallaxe = 0.9508 + 0.0518 * cos(M) Rs = 6378137. / sin(rad(parallaxe )) -- Cartesian ecliptic Earth frame ReMoEcliptic[1] = Rs * cos(b) * cos(l) ReMoEcliptic[2] = Rs * cos(b) * sin(l) ReMoEcliptic[3] = Rs * sin(b) --equations of the mean planet Earth around the Sun. omB = om0[3] + omP[3] * JDbouiges LongOrb =LorbitalSun M =MorbitalSun --KEPLER EQUATION u = M --starting guess FOR ( K IN 0, IterationKeplersp[3] * 2) u = M + exc[3] * sin(u) END FOR --true anomaly v = 2 * atan(tan(u / 2) * sqrt((1 + exc[3]) / (1 - exc[3]))) OM = OM0[3] + OMp[3] * JDbouiges LtildaVraie = v + (omB - OM) IF cos(LtildaVraie) == 0 THEN Ltilda = LtildaVraie ELSE Ltilda = atan(tan(LtildaVraie) * cos(incl[3])) IF cos(LtildaVraie) < 0 THEN Ltilda = Ltilda + PI END IF END IF --Longitude and latitude ecliptique héliocentric lh = Ltilda + OM lh = lh + PI --************************************ Spécial terre IF 3 = 3 Then Bs = atan(sin(Ltilda) * tan(incl[3])) Rs = AU * aMajorsp[3] * (1 - exc[3] * cos(u)) --formule avec l--ano excentrique. --coordinates cartesian ecliptic heliocentric RsEEcliptic[1] = Rs * cos(Bs) * cos(lh) --radius Sun -> Earth RsEEcliptic[2] = Rs * cos(Bs) * sin(lh) RsEEcliptic[3] = Rs * sin(Bs) --Finally ray sun -> moon = earth moon + sun earth and ray earth sun = - sun earth FOR (K IN 1,3 ) RsMoEcliptic[K] = ReMoEcliptic[K] + RsEEcliptic[K] ReSEcliptic[K] = - RsEEcliptic[K] END FOR --at the end transform into ECIequatorial CosAngle = cos(rad(+23.45))--"obliquity of the ecliptic in radian (rd)" SinAngle = sin(rad(+23.45)) ReS[Xq] =ReSEcliptic[1] ReS[Yq] = ReSEcliptic[2] * CosAngle - ReSEcliptic[3] * SinAngle ReS[Zq] = ReSEcliptic[2] * SinAngle + ReSEcliptic[3] * CosAngle ReMo[Xq] =ReMoEcliptic[1] ReMo[Yq] = ReMoEcliptic[2] * CosAngle - ReMoEcliptic[3] * SinAngle ReMo[Zq] = ReMoEcliptic[2] * SinAngle + ReMoEcliptic[3] * CosAngle /* --Output Ecliptic only, for plot Figure 3 Sun and Moon orbits in the Frame ECI ecliptic, page 7/19 of Eai-Kci-Me-09 TN3610PhysicalModelSpec Satellite Library05 i=0 FOR (K IN ECI ) i=i+1 ReS[K]=ReSEcliptic[i] ReMo[K]=ReMoEcliptic[i] END FOR */ RETURN END FUNCTION FUNCTION NO_TYPE ParametersToCartesian (REAL altPer_o UNITS "m" "Apogee altitude from Earth", REAL altApo_o UNITS "m" "Perigee altitude from Earth", REAL OM_o UNITS "deg" "RAAN --also called OMEGA-- of the initial orbit", REAL inc_o UNITS "deg" "Inclination of the initial orbit", REAL om_o UNITS "deg" "perigee argument --also called omega-- of the initial orbit" , REAL PHI_o UNITS "deg" "true anomaly", OUT REAL R_o[ECI] UNITS "m" , OUT REAL V_o[ECI] UNITS "m/s" , OUT REAL error[4] UNITS "-" "rounding errors, summary in index 1: error[1]") "give the radius and velocity of an orbit defined by its orbital parameters" DECLS REAL EarthR =6378137 UNITS "m" "equatorial earth radius " REAL GMEarth=3.986005E14 UNITS "m3/s2" "G.M_earth " REAL LocalOrbitalPolar[Pol] , Rrr[ 3], Vvv[ 3] REAL exc, sma,Wenergy,C_orbit,p_orbit,r_module,V_module,v_theta,v_radial, theta_o REAL checkv,checkr,checkvv BODY error[1]=0 error[2]=0 error[3]=0 error[4]=0 --avoid error mistake perigee apogee ; use abs(altApo_o-altPer_o) exc=abs(altApo_o-altPer_o)/(altApo_o+EarthR+altPer_o+EarthR) sma=(altApo_o+EarthR+altPer_o+EarthR)/2 Wenergy=-GMEarth/(2*sma) C_orbit=sqrt((1-exc**2)*sma*GMEarth) p_orbit=C_orbit**2/GMEarth r_module=p_orbit/(1+exc*cos(rad(PHI_o))) V_module=sqrt(2*(Wenergy+GMEarth/r_module)) v_theta=C_orbit/r_module --oriented along local velocity and orthogonal to the radius v_radial=GMEarth/C_orbit*exc*sin(rad(PHI_o)) --oriented along the radius from Earth checkv=V_module-sqrt(v_theta**2+v_radial**2) IF (abs(checkv)>0 ) THEN ErrRounding (checkv,1E-10,"ERROR ParametersToCartesian 1 for V") END IF IF (abs(checkv)>1E-3) THEN error[2]=checkv END IF --Change frame : from Pol (Xp:radius, Yp:orthoradial in orbital plane and Zp:h out of orbital plane) to inertial ECI --Gamma = ATN2( -v_radial,v_theta) --in Ecosim sinus then cos --orbital vector locally using incli,om+phi,OM in the Pol (not the Orb for telecom satellites) LocalOrbitalPolar[Xp] =r_module LocalOrbitalPolar[Yp]= 0 LocalOrbitalPolar[Zp]=0 --om_o+PHI_o --angle from ascending node CoordPolEulerECI (LocalOrbitalPolar, rad(OM_o), rad(inc_o), rad(om_o+PHI_o),R_o) --PrecessionOmega, NutationInclin, RotationPropreTheta Rrr is in ECI LocalOrbitalPolar[Xp] = v_radial LocalOrbitalPolar[Yp] =v_theta LocalOrbitalPolar[Zp] = 0 CoordPolEulerECI (LocalOrbitalPolar, rad(OM_o), rad(inc_o), rad(om_o+PHI_o), V_o) --Vvv is in ECI checkr=r_module-Norm(R_o) -- sqrt(Rrr[Xl]**2+Rrr[Yl]**2+Rrr[Zl]**2) IF (abs(checkr)>0 ) THEN ErrRounding (checkr,1E-07,"ERROR ParametersToCartesian 2 fro R")--lower allowed error because R may be very large END IF IF (abs(checkr)>1E-3) THEN error[3]=checkr END IF checkvv=V_module- Norm(V_o) -- sqrt(Vvv[Xl]**2+Vvv[Yl]**2+Vvv[Zl]**2) IF (abs(checkvv)>0 ) THEN ErrRounding (checkvv,1E-10,"ERROR ParametersToCartesian 3 for V") END IF IF (abs(checkvv)>1E-3) THEN error[4]=checkvv END IF error[1]=error[2]+error[3]+error[4] END FUNCTION FUNCTION NO_TYPE Eclipse (REAL RSatSun[Sat],REAL RSatEarth[Sat], OUT BOOLEAN FlagEclipse,OUT REAL FractionSun) "Function that return the total eclipse flag" DECLS REAL Rearth=6378137 UNITS "m" REAL Hatmosphere=100e3 *0 UNITS "m" "no penumbra, h=0" REAL Rsun=109 * 6378137 UNITS "m" --450000e3 REAL halfconed=0 UNITS "°" REAL angled=0 UNITS "°" REAL angleAtConed=0 UNITS "°" REAL RearthSun[Sat] UNITS "m" REAL RConeEarth[Sat] UNITS "m" REAL RConeSat[Sat] UNITS "m" REAL halfconedTot, angleAtConedTot, halfconedLimit UNITS "°" "not used, for penumbra and total eclipse" BODY --Case sat between Sun and Earth : scalar RSatSun,RSatEarth is negative angled=deg(acos(XscalarY(RSatSun,RSatEarth)/Norm(RSatSun)/Norm(RSatEarth))) FractionSun=1 IF (angled>90) THEN FlagEclipse=FALSE --PRINT (" angle deg = $angled DONE") RETURN END IF --Case sat away Sun and Earth; search the submit of the shadow cone and check the angle between RConeSat,RConeEarth aXbYc(-1, RSatEarth ,1, RSatSun , 0, RearthSun ) halfconed=deg(atan((Rsun-(Rearth+Hatmosphere))/Norm(RearthSun))) aXb(-1* (Rearth+Hatmosphere)/tan(rad(halfconed))/Norm(RearthSun), RearthSun ,0 ,RConeEarth ) aXbYc(-1, RSatEarth ,1, RConeEarth , 0, RConeSat ) angleAtConed=deg(acos(XscalarY(RConeSat,RConeEarth)/Norm(RConeSat)/Norm(RConeEarth))) IF ( angleAtConed <= halfconed ) THEN FlagEclipse=TRUE FractionSun=0 RETURN /* --Penumbra and FULL eclipse halfconedLimit= deg(atan(Rearth/Norm(RConeEarth)))-- the previous RConeEarth halfconedTot=deg(atan((Rsun-Rearth)/Norm(RearthSun))) aXb(-1* (Rearth)/tan(rad(halfconed))/Norm(RearthSun), RearthSun ,0 ,RConeEarth ) aXbYc(-1, RSatEarth ,1, RConeEarth , 0, RConeSat ) angleAtConedTot=deg(acos(XscalarY(RConeSat,RConeEarth)/Norm(RConeSat)/Norm(RConeEarth))) IF ( angleAtConedTot <= halfconedTot ) THEN FractionSun=0 RETURN ELSE FractionSun=(angleAtConed-halfconedLimit)/(halfconed-halfconedLimit)--proportional RETURN END IF*/ END IF FlagEclipse=FALSE RETURN END FUNCTION -------------------------------------------------------------------------------------------------------