# Procedure to Load CRISP TPU data structures for CONTOUR SPT
# <proc-author>Paul Carr</proc-author>
# <creation-date>14 Dec 2001</creation-date>
# This procedure loads Gene Heyler's parameters and the latest
# JPL ephemeris processed by N. Pinkine.

ARGS(BC)
PAUSE
WRITE "<test-comment>routing CRISP real time packets to PQ 2</test-comment>"
CMD CD_RT_TLM_PKT_SEL PQ2 EN ALL SYS 0x600

PAUSE   # continue to load DS 3

#  ID=3,  OffsetMatrix 

cmd CRS_TPU_UP_STRCT_3 \ 
  0.0  0.0 (-1.0)   \
   0.0  1.0  0.0     \
   1.0  0.0  0.0

WAIT 10
PAUSE   # verify command received, then continue
CMD CRS_TPU_MEM_STR_READ 3
WAIT 5
PAUSE   # verify command received, then continue

# ID=4,  Tracker control params
cmd CRS_TPU_UP_STRCT_4 \ 
0.50  \                        # MirrAng_Thrsh
2.00  \                        # Mirror feed forward
2.00  \                        # Filter lag
1.00  \                        # roll control gain  1.0
0.50  \                        # ab-tracker gain  0.5
512.00  \                      # x_goal (pix)
512.00  \                      # y_goal (pix)
0.6887  \                      # system focal length (m)
3                             # G&C msg usage rate (sec)

WAIT 10
PAUSE   # verify command received, then continue
CMD CRS_TPU_MEM_STR_READ 4
WAIT 5
PAUSE   # verify command received, then continue

# <test-comment>ID=5,  Tracker static target </test-comment>  
WRITE "<test-comment>ID=5,  Tracker static target </test-comment> "      
cmd CRS_TPU_UP_STRCT_5 \ 
0.999950  0.000000  (-0.009989)
WAIT 10
PAUSE   # verify command received, then continue
CMD CRS_TPU_MEM_STR_READ 5
WAIT 5
PAUSE   # verify command received, then continue

# <test-comment>ID=6, EKF control params</test-comment>   
WRITE "<test-comment>ID=6, EKF control params</test-comment>"  
cmd CRS_TPU_UP_STRCT_6 \ 
   2.0e-04 \                       # measurement sigma
   8.66e+03 8.10e+01 8.10e+01 1.00e-10 1.00e-10 1.00e-10  \ # state covariance diag 
   0.00e+00 0.00e+00 0.00e+00 0.00e+00 0.00e+00 0.00e+00    # system noise diag     
WAIT 10

PAUSE   # verify command received, then continue
CMD CRS_TPU_MEM_STR_READ 6
WAIT 5

PAUSE   # verify command received, then continue
# <test-comment>ID=7, ephemeris load part 1</test-comment>   
WRITE "<test-comment>ID=7, ephemeris load part 1</test-comment>"   

# ID=7  (in four parts)
CMD CRS_TPU_UP_STRCT_7_1 \
     2 \                     # Number of valid spans
     1 \                     # Use G&C Central-Body as ref frame origin - Field is ignored by CRISP
         121521664.1840 \    # Span start time TDT J2000
         121694464.1840      # Span stop time TDT J2000
   
WRITE "<test-comment>ID=7, ephemeris load part 2</test-comment>"   
CMD CRS_TPU_UP_STRCT_7_2   \
             86400.000               86400.000 \      # t0 and tau scaling parameters
      ( -0.561970384833E+07) \
      (  0.145794018000E+07) \
      ( -0.850131051805E+03) \
      (  0.351955760999E+02) \
      (  0.952355559679E-01) \
      (  0.425372767889E-04) \
      ( -0.239970650495E-05) \
      ( -0.494874520599E-07) \
      ( -0.123674677726E-07) \
      ( -0.368778961967E-07) \
      ( -0.178232779294E-07) \
      ( -0.792428521582E-08) \
      (  0.173166264482E-07)


WRITE "<test-comment>ID=7, ephemeris load part 3</test-comment>"   
CMD CRS_TPU_UP_STRCT_7_3   \
             86400.000               86400.000 \      # t0 and tau scaling parameters
      ( -0.623312043559E+07) \
      (  0.161433708955E+07) \
      ( -0.380849964005E+03) \
      (  0.126449733148E+02) \
      (  0.225619197350E+00) \
      (  0.154023453843E-02) \
      (  0.112552512804E-04) \
      (  0.728870412708E-07) \
      ( -0.139095528477E-07) \
      ( -0.398976691705E-07) \
      ( -0.192129966729E-07) \
      ( -0.558877820474E-08) \
      (  0.196718573348E-07)


WRITE "<test-comment>ID=7, ephemeris load part 4</test-comment>"   
CMD CRS_TPU_UP_STRCT_7_4   \
             86400.000               86400.000 \      # t0 and tau scaling parameters
      ( -0.432125742928E+07) \
      (  0.111972732925E+07) \
      ( -0.377500120606E+03) \
      (  0.135645263859E+02) \
      (  0.162934586088E+00) \
      (  0.104513580272E-02) \
      (  0.735956202353E-05) \
      (  0.427625611974E-07) \
      ( -0.930461928560E-08) \
      ( -0.286892904330E-07) \
      ( -0.151780296714E-07) \
      ( -0.470094581803E-08) \
      (  0.142645619467E-07)
WAIT 10
PAUSE   # verify command received, then continue
CMD CRS_TPU_MEM_STR_READ 7
WAIT 10
PAUSE   # verify command received, then continue

WRITE "<test-comment>ID=8, ephemeris load part 1</test-comment>"   
# ID=8       
CMD CRS_TPU_UP_STRCT_8_1   \
     2 \                     # Number of valid spans
     1 \                     # Use G&C Central-Body as ref frame origin - Field is ignored by CRISP
         121867264.1840 \    # Span start time TDT J2000
         122040064.1840      # Span stop time TDT J2000
     
WRITE "<test-comment>ID=8, ephemeris load part 2</test-comment>"   
CMD CRS_TPU_UP_STRCT_8_2   \
            432000.000               86400.000 \      # t0 and tau scaling parameters
      (  0.194260495836E+06) \
      (  0.145113114415E+07) \
      (  0.311503417264E+02) \
      (  0.382369137689E+02) \
      (  0.938067389357E-01) \
      ( -0.140216131655E-03) \
      ( -0.562500895405E-05) \
      ( -0.586463267639E-07) \
      (  0.268776942939E-07) \
      (  0.156406941250E-07) \
      ( -0.547609597505E-08) \
      (  0.464587936815E-08) \
      ( -0.192061703095E-07)
     
WRITE "<test-comment>ID=8, ephemeris load part 3</test-comment>"   
CMD CRS_TPU_UP_STRCT_8_3   \          
            432000.000               86400.000 \      # t0 and tau scaling parameters
      (  0.215861088073E+06) \
      (  0.161109512989E+07) \
      (  0.179710753414E+02) \
      (  0.209775715421E+02) \
      (  0.299692577842E+00) \
      (  0.221014795334E-02) \
      (  0.170864113556E-04) \
      (  0.155711160267E-06) \
      (  0.319054228870E-07) \
      (  0.172744118445E-07) \
      ( -0.606741366970E-08) \
      (  0.525762733008E-08) \
      ( -0.213791733813E-07)
         
WRITE "<test-comment>ID=8, ephemeris load part 4</test-comment>"   
CMD CRS_TPU_UP_STRCT_8_4   \          
            432000.000               86400.000 \      # t0 and tau scaling parameters
      (  0.149550716086E+06) \
      (  0.111661841991E+07) \
      (  0.164953826064E+02) \
      (  0.195293923083E+02) \
      (  0.212845139343E+00) \
      (  0.147931759813E-02) \
      (  0.109807537802E-04) \
      (  0.975121548136E-07) \
      (  0.219261075361E-07) \
      (  0.119608290538E-07) \
      ( -0.424186215786E-08) \
      (  0.360930467307E-08) \
      ( -0.148181838287E-07)

WAIT 10
PAUSE   # verify commands received, then continue
CMD CRS_TPU_MEM_STR_READ 8
WAIT 10
PAUSE   # verify command received, then continue

WRITE "<test-comment>ID=9, ephemeris load part 1</test-comment>"   
# ID=9       
CMD CRS_TPU_UP_STRCT_9_1   \          
     2 \                     # Number of valid spans
     1 \                     # Use G&C Central-Body as ref frame origin - Field is ignored by CRISP
         121694464.1840 \    # Span start time TDT J2000
         121867264.1840      # Span stop time TDT J2000
     
CMD CRS_TPU_UP_STRCT_9_2   \          
            259200.000               86400.000 \      # t0 and tau scaling parameters
      ( -0.270928661032E+07) \
      (  0.145282258395E+07) \
      ( -0.441158318766E+03) \
      (  0.255154151681E+02) \
      ( -0.219060996016E+01) \
      (  0.225275906195E+01) \
      (  0.251336915813E+01) \
      (  0.782994764822E+00) \
      ( -0.823213214509E+00) \
      ( -0.113650935822E+01) \
      ( -0.380468958811E+00) \
      (  0.349865530007E+00) \
      (  0.698236938446E+00)
     
CMD CRS_TPU_UP_STRCT_9_3   \          
            259200.000               86400.000 \      # t0 and tau scaling parameters
      ( -0.300697649400E+07) \
      (  0.161194518506E+07) \
      ( -0.217602776643E+03) \
      (  0.109374905433E+02) \
      ( -0.878727146668E+00) \
      (  0.112331209343E+01) \
      (  0.125121955730E+01) \
      (  0.389796396233E+00) \
      ( -0.409813233818E+00) \
      ( -0.565781821433E+00) \
      ( -0.189405616877E+00) \
      (  0.174171364752E+00) \
      (  0.347597843422E+00)

        
CMD CRS_TPU_UP_STRCT_9_4   \          
            259200.000               86400.000 \      # t0 and tau scaling parameters
      ( -0.208427891606E+07) \
      (  0.111739729824E+07) \
      ( -0.203379257936E+03) \
      (  0.138533716784E+02) \
      ( -0.323324492698E+00) \
      (  0.502793693032E+00) \
      (  0.559604534239E+00) \
      (  0.174336775500E+00) \
      ( -0.183287334610E+00) \
      ( -0.253045157288E+00) \
      ( -0.847119178056E-01) \
      (  0.778979122046E-01) \
      (  0.155463346192E+00)

WAIT 10
PAUSE   # verify commands received, then continue
CMD CRS_TPU_MEM_STR_READ 9
WAIT 10
PAUSE   # verify command received, then continue

# <test-comment>ID=10, mirror coefficients</test-comment> 
WRITE "<test-comment>ID=10, mirror coefficients</test-comment>"

CMD CRS_TPU_UP_STRCT_10_1   \
   0.0000000     \   #    MCoeff_aa x(1)  modeled 
   0.0000000     \   #    MCoeff_aa x(2)   
   0.9999851     \   #    MCoeff_aa x(3)   
  (-0.0000000)   \   #    MCoeff_aa x(4)   
   0.0000000     \   #    MCoeff_aa x(5)   
   0.0000000     \   #    MCoeff_aa x(6)   
  (-0.0000000)   \   #    MCoeff_aa x(7)   
   0.0000000     \   #    MCoeff_aa x(8)   
  (-0.0000000)   \   #    MCoeff_aa x(9)   
   0.0000000         #    MCoeff_aa x(10)  
   
   
CMD CRS_TPU_UP_STRCT_10_2   \
  (-1.5707318)   \   #    MCoeff_aa y(1)  modeled 
  (-1.9999178)   \   #    MCoeff_aa y(2)   
  (-0.0000000)   \   #    MCoeff_aa y(3)   
   0.9999589     \   #    MCoeff_aa y(4)   
  (-0.0000000)   \   #    MCoeff_aa y(5)   
  (-0.0000000)   \   #    MCoeff_aa y(6)   
   0.0000000     \   #    MCoeff_aa y(7)   
  (-0.0000000)   \   #    MCoeff_aa y(8)   
   0.0000000     \   #    MCoeff_aa y(9)   
  (-0.0000000)       #    MCoeff_aa y(10)  
  
CMD CRS_TPU_UP_STRCT_10_3   \
   0.0000000     \   #    MCoeff_ab x(1)  modeled 
   0.0000000     \   #    MCoeff_ab x(2)   
   0.9999851     \   #    MCoeff_ab x(3)   
  (-0.0000000)   \   #    MCoeff_ab x(4)   
   0.0000000     \   #    MCoeff_ab x(5)   
  (-0.0000000)   \   #    MCoeff_ab x(6)   
  (-0.0000000)   \   #    MCoeff_ab x(7)   
  (-0.0000000)   \   #    MCoeff_ab x(8)   
   0.0000000     \   #    MCoeff_ab x(9)   
   0.0000000         #    MCoeff_ab x(10)  */
   
CMD CRS_TPU_UP_STRCT_10_4   \
  (-7.8536590)   \   #    MCoeff_ab y(1)  modeled */
  (-1.9999178)   \   #    MCoeff_ab y(2)   */
  (-0.0000000)   \   #    MCoeff_ab y(3)   */
   0.9999589     \   #    MCoeff_ab y(4)   */
  (-0.0000000)   \   #    MCoeff_ab y(5)   */
  (-0.0000000)   \   #    MCoeff_ab y(6)   */
   0.0000000    \    #    MCoeff_ab y(7)   */
  (-0.0000000)   \   #    MCoeff_ab y(8)   */
   0.0000000    \    #    MCoeff_ab y(9)   */
  (-0.0000000)       #    MCoeff_ab y(10)  */
  
  
CMD CRS_TPU_UP_STRCT_10_5   \
     0.0000000   \   #          MCoeff_fa x(1) modeled */
     0.0000000   \   #          MCoeff_fa x(2)   */
     0.9999851   \   #          MCoeff_fa x(3)   */
    (-0.0000000) \   #          MCoeff_fa x(4)   */
     0.0000000   \   #          MCoeff_fa x(5)   */
     0.0000000   \   #          MCoeff_fa x(6)   */
    (-0.0000000) \   #          MCoeff_fa x(7)   */
     0.0000000   \   #          MCoeff_fa x(8)   */
    (-0.0000000) \   #          MCoeff_fa x(9)   */
     0.0000000       #          MCoeff_fa x(10)  */
     
CMD CRS_TPU_UP_STRCT_10_6   \
     7.0750066   \   #          MCoeff_fa y(1)  lab fitted */
    (-4.0324435) \   #          MCoeff_fa y(2)   */
   0.0           \   #          MCoeff_fa y(3)   */
     2.0045665   \   #          MCoeff_fa y(4)   */
    0.43701519   \   #          MCoeff_fa y(5)   */
    0.0          \   #          MCoeff_fa y(6)   */
   (-0.43202675) \   #          MCoeff_fa y(7)   */
    0.0          \   #          MCoeff_fa y(8)   */
    0.0          \   #          MCoeff_fa y(9)   */
    0.10523149       #          MCoeff_fa y(10)  */
    
    
CMD CRS_TPU_UP_STRCT_10_7   \
     0.0000000   \   #          MCoeff_fb x(1)   modeled*/
     0.0000000   \   #          MCoeff_fb x(2)   */
     0.9999851   \   #          MCoeff_fb x(3)   */
    (-0.0000000) \   #          MCoeff_fb x(4)   */
     0.0000000   \   #          MCoeff_fb x(5)   */
     0.0000000   \   #          MCoeff_fb x(6)   */
    (-0.0000000) \   #          MCoeff_fb x(7)   */
     0.0000000   \   #          MCoeff_fb x(8)   */
    (-0.0000000) \   #          MCoeff_fb x(9)   */
     0.0000000       #          MCoeff_fb x(10)  */
     
CMD CRS_TPU_UP_STRCT_10_8   \
    (-1.2071)   \    #          MCoeff_fb y(1)  lab fitted */
    (-1.1024)   \    #          MCoeff_fb y(2)   */
        0.0     \    #          MCoeff_fb y(3)   */
     0.5560     \    #          MCoeff_fb y(4)   */
     0.5532     \    #          MCoeff_fb y(5)   */
        0.0     \    #          MCoeff_fb y(6)   */
    (-0.5467)   \    #          MCoeff_fb y(7)   */
        0.0     \    #          MCoeff_fb y(8)   */
        0.0     \    #          MCoeff_fb y(9)   */
     0.1342          #          MCoeff_fb y(10)  */
WAIT 10
PAUSE   # verify commands received, then continue
CMD CRS_TPU_MEM_STR_READ 10
WAIT 10
PAUSE   # verify command received, then continue

   # ID=11, gate control params */
CMD CRS_TPU_UP_STRCT_11   \
0.10   \   # gate size   */
0.50   \   # alpha       */
3          # count       */  
                      
WAIT 10
PAUSE   # verify commands received, then continue
CMD CRS_TPU_MEM_STR_READ 11
WAIT 10
PAUSE   # verify command received, then continue

# ID=12, gyro alignment */
CMD CRS_TPU_UP_STRCT_12   \
(-0.5)   \ 
  0.5    \
(-0.5)   \
  0.5    # assumed IRU-to-body alignment, from Gabe */              
WAIT 10
PAUSE   # verify commands received, then continue
CMD CRS_TPU_MEM_STR_READ 12
WAIT 10
PAUSE   # verify command received, then continue