L3=128 DOLOOP IAD=LI, L2, L3 IFSS-IAND(FMTBUF(D),255) IF(IFSS.GT.127) IFSS:IFSS-256 JCOUNT-JCOUNT+l NFSS-NFSS + IFSS ENDLOOP C CONVERT INTO RADIANS IF(IGAIN.NE.O)THEN C IGAIN=1=> COARSE RESOLUTION => HIGH GAIN ROLL=NFSS/SECFSH ELSE C IGAIN=O => FINE RESOLUTION => L0W GAIN ROLL=NFSS/SECFSL ENDIF IF(JCOUNT.NE.0) ROLL=ROLL/JCOUNT C CALL SUNCO ROUTINE TO GET ATTITUDE MATRIX UPDATED FOR ROLL CALL SUNCO(ATTC,SUN,SRA,SDE,ROLL,ICOUNT) C CHECK # OF ITERATIONS, JUST OUT OF INTEREST. IF(ICOUNT.GT.5)IRET=1 ENDIF ENDIF C IF(DEBLUR)THEN C GET PARAMETERS A511, 512, 513 (TRUE XYZ ERRORS) DOLOOP I=1, 3 I2(1)=0 I2(2)=FMTBUF(697+I) ERR(I)=(I4-D32768)/SECA*0.33DO ENDLOOP C APPLY ROTATIONS ERRORS AROUND Y AND Z CALL DROTX(ATTC,ERR(2),2) CALL DROTX(ATTC,ERR(3),3) C APPLY ROTATION ABOUT X, IF ROLL ERROR AVAILABLE C (IF ROLL ERROR CORRECTION ALREADY PERFORMED THEN SKIP) IF(ICRS1.NE.O.AND.ICFL.EQ.O.AND.SUNPRE)CALL DROTX(ATTC,ERR(1),1) ENDIF RETURN END C C C SUBROUTINE SUNCO(ATT,SUN,SRA,SDE,ROLL,ICOUNT) C CP SUBROUTINE TO RECOMPUTE THE EXOSAT ATTITUDE MATRIX AS FUNCTION OF CP STAR REFERENCE POSITION AND SUN EFFECTIVE POSITION C Cl ATT (3, 3) R*8 OLD ATTITUDE MATRIX Cl ATT(l,l),ATT(2,l),ATT(3,I) = STAR TRACKER X-AXIS CI ATT(I,2),ATT(2,2),ATT(3,2) = STAR TRACKER Y-AXIS ci ATT(1,3),ATT(2,3),ATT(3,3) = STAR TRACKER Z-AXIS cl SUN(3) R*8 S/C TO SUN UNIT VECTOR Cl SRA R*8 RIGHT ASCENSION Of REFERENCE STAR (RADIANS) CI SDE R*8 DECLINATION OF REFERENCE STAR (RADIANS) Cl ROLL R*8 ACTUAL ROLL ERROR MEASURED BY FSS (RADIANS) CI (POSITIVE WHEN SUN IS BELOW X-Y PLANE) CO ATT(3,3) R*8 UPDATED ATTITUDE MATRIX (TWO-WAY PARAMETER!) C CO ICOUNT I ITERATION COUNT (FOR TESTING) C C CC PROJ-EXO, SUBJ-ATT, UTIL=SPEC, AUTH=A.SCHUETZ/ECD/OAD,AUG'86 C CS SUBROUTINES CALLED CS DRCTR ROTATES A SET OF REFERENCE AXES AROUND AN ARBITRARY AXIS CS DROTX ROTATES A SET OF REFERENCE AXES AROUND ONE OF THEM CS MVW MOVE n 16-BIT WORDS FROM 2ND ARGUMENT TO 3RD ARGUMENT C IMPLICIT REAL*8 (A-H,O-Z) DIMENSION ATT(3,3),SUN(3).FSSDAT(3) C CV LOCAL VARIABLES CV FSSDAT(3) R*8 FINE SUN SENSOR DATA CV (1) = MISALIGNEMENT IN ROLL (RADIANS) CV (2) = MISALIGNEMENT IN PITCH (RADIANS) CV (3) = SCALE FACTOR CV RLIM MINIMUM ROLL ERROR CORRECTION ON RADIANS (=10 ARCSEC) CV STAR STAR UNIT VECTOR CV FSSR FINE SUN SENSOR REFERENCE AXES IN INERTIAL REFERENCE SYSTEM CV SUNS SUN UNIT VECTOR IN FSS REFERENCE SYSTEM C DIMENSION STAR(3),SUNB(3),FSSR(3,3) DATA FSSDAT/.00285DO,.00049DO,I.ODO/ ! = 9.8 1.7 ARC MINS DATA RLIM/4.848D-5/ C C IF PRESENT CONVERT STAR POSITION INTO A UNIT VECTOR (USE SUNB) C IF NOT ASSUME STAR IS ON X-AXIS IF(SRA.EQ.O.DO.AND.SDE.EQ.O.DO)THEN C COMMENT THIS OCCURS DURING CERTAIN CONTINGENCIES, BUT SUNCO CI WILL STILL BEHAVE CORRECTLY. CALL MVW(12,ATT(1,1),SUNB) ELSE C COMMENT NORMAL CASE - VALID STAR COORDINATES - CONVERT SUND(l)=DCOS(SDE)*DCOS(SRA) SUNB(2)=DCOS(SDE)*DSIN(SRA) SUNB(3)=DSIN(SDE) ENDIF C C EXPRESS STAR POSITION IN STAR TRACKER REFERENCE MATRIX STAR (1) =ATT(1,1)*SUNB(1)+ATT(2,1)*SUNB(2)+ATT(3,1)*SUNB(3) STAR (2) =ATT(1,2)*SUNB(1)+ATT(2,2)*SUNB(2)+ATT(3,2)*SUNB(3) STAR (3) =ATT(1,3)*SUNS(l)+ATT(2,3)*SUNB(2)+AIT(3,3)*SUNB(3) C ICOUNT=0 C 10 CONTINUE ICOUNT=ICOUNT+l C C INITIALISE FSS REFERENCE MATRIX TO STAR TRACKER REFERENCE MATRIX CALL MVW(36,ATT,FSSR) C C APPLY MISALlGNMEHTS IN ROLL AND PITCH CALL DROTX(FSSR,FSSDAT(1), 1) CALL DROTX(FSSR,FSSDAT(2),2) C C COMPUTE PREDICTED SUN POSITION IN FSS REFERENCE SUNB(1)=FSSR(1,1)*SUN(1) + FSSR(2,1)*SUN(2) + FSSR(3,1)*SUN(3) SUNB(2)=FSSR(1,2)*SUN(1) + FSSR(2,2)*SUN(2) + FSSR(3,2)*SUN(3) SUNB(3)=FSSR(1,3)*SUN(1) + FSSR(2,3)*SUN(2) + FSSR(3,3)*SUN(3) C C DERIVE PREDICTED ROLL ANGLE ERROR (= MINUS ROLL ANGLE) PROLL=-DATAN(SUNB(3)/SUNB(2)) C C RESIDUAL ROLL ERROR = ACTUAL - PREDICTED DROLL=ROLL*FSSDAT(3) - PROLL C C IF RESIDUAL ROLL ERROR IS SMALL ENOUGH RETURN IF(DABS(DROLL).LT.RLIM.OR.ICOUNT.GE.10)RETURN C C APPLY THAT CORRECTION TO THE INITIAL ATTITUDE MATRIX WITH THE