# 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
      121867264.184    \     # Span start time TDT J2000
      121941964.184          # Span stop time TDT J2000
   
WRITE "<test-comment>ID=7, ephemeris load part 2</test-comment>"   
CMD CRS_TPU_UP_STRCT_7_2   \
           37350.000  37350.000     \      # t0 and tau scaling parameters
      (      -629330.78 ) \
      (       627304.96 ) \
      (      -18.439696 ) \
      (       3.0544544 ) \
      (    0.0033009957 ) \
      (  -1.5709667E-06 ) \
      (  -4.3026233E-08 ) \
      (   1.6501559E-08 ) \
      (  -8.8104173E-09 ) \
      (   2.2863986E-09 ) \
      (  -2.5785596E-10 ) \
      (  -1.6967761E-11 ) \
      (   3.9683812E-11 )


WRITE "<test-comment>ID=7, ephemeris load part 3</test-comment>"   
CMD CRS_TPU_UP_STRCT_7_3   \
         37350.000  37350.000     \      # t0 and tau scaling parameters
      (     -698597.03 ) \
      (       696458.4 ) \
      (     -9.7445635 ) \
      (      1.5862802 ) \
      (    0.010036416 ) \
      (  3.1642475E-05 ) \
      (  9.3395158E-08 ) \
      (  1.8962638E-08 ) \
      ( -9.8578283E-09 ) \
      (  2.6176821E-09 ) \
      ( -3.9132431E-10 ) \
      ( -5.2580162E-12 ) \
      (  4.7116089E-11 )


WRITE "<test-comment>ID=7, ephemeris load part 4</test-comment>"   
CMD CRS_TPU_UP_STRCT_7_4   \
        37350.000  37350.000      \      # t0 and tau scaling parameters
      (      -484274.32 ) \
      (       482700.76 ) \
      (      -9.1709968 ) \
      (       1.5006363 ) \
      (     0.007145319 ) \
      (   2.1223659E-05 ) \
      (   5.9661573E-08 ) \
      (   1.3144906E-08 ) \
      (  -6.7928809E-09 ) \
      (   1.7952644E-09 ) \
      (  -2.7019098E-10 ) \
      (     1.57101E-11 ) \
      (   2.9857006E-11 )
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
        121941964.184   \    # Span start time TDT J2000
        122040064.184        # Span stop time TDT J2000
     
WRITE "<test-comment>ID=8, ephemeris load part 2</test-comment>"   
CMD CRS_TPU_UP_STRCT_8_2   \
            123750.000     49050.000      \      # t0 and tau scaling parameters
      (        821778.1 ) \
      (       823854.32 ) \
      (        42.05724 ) \
      (       7.0553947 ) \
      (    0.0096767803 ) \
      (  -1.0015683E-05 ) \
      (  -1.7166894E-07 ) \
      (   8.4546627E-09 ) \
      (  -8.3164196E-09 ) \
      (  -8.2873157E-09 ) \
      (  -2.4301698E-09 ) \
      (  -4.4383341E-10 ) \
      (   1.7826096E-10 )
     
WRITE "<test-comment>ID=8, ephemeris load part 3</test-comment>"   
CMD CRS_TPU_UP_STRCT_8_3   \          
           123750.000     49050.000   \      # t0 and tau scaling parameters
      (      912485.82 ) \
      (       914652.1 ) \
      (      23.498778 ) \
      (      4.0295194 ) \
      (     0.03213502 ) \
      (  0.00013567227 ) \
      (  6.3635525E-07 ) \
      (  1.3933459E-08 ) \
      ( -8.9858077E-09 ) \
      ( -9.2040864E-09 ) \
      ( -2.6120688E-09 ) \
      ( -4.6566129E-10 ) \
      (  1.2369128E-10 )
         
WRITE "<test-comment>ID=8, ephemeris load part 4</test-comment>"   
CMD CRS_TPU_UP_STRCT_8_4   \          
          123750.000     49050.000   \      # t0 and tau scaling parameters
      (       632332.68 ) \
      (       633931.98 ) \
      (       21.763134 ) \
      (       3.7090475 ) \
      (     0.022781228 ) \
      (    9.066589E-05 ) \
      (   4.1000021E-07 ) \
      (   9.3859853E-09 ) \
      (  -6.3082553E-09 ) \
      (  -6.3882908E-09 ) \
      (  -1.8699211E-09 ) \
      (  -3.4924597E-10 ) \
      (   1.2369128E-10 )

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
         121867264.1840 \    # Span start time TDT J2000
         122040064.1840      # Span stop time TDT J2000
     
CMD CRS_TPU_UP_STRCT_9_2   \          
            432000.000               86400.000 \      # t0 and tau scaling parameters
      (      194480.91 ) \
      (      1451131.1 ) \
      (      31.150342 ) \
      (      38.236914 ) \
      (    0.093806739 ) \
      ( -0.00014021542 ) \
      ( -5.6249701e-06 ) \
      ( -5.8113073e-08 ) \
      (  2.6811904e-08 ) \
      (  1.5927071e-08 ) \
      (  -5.493348e-09 ) \
      (  4.6493369e-09 ) \
      (  -1.925946e-08 )
     
CMD CRS_TPU_UP_STRCT_9_3   \          
            432000.000               86400.000 \      # t0 and tau scaling parameters
      (        216031.1 ) \
      (       1611095.1 ) \
      (       17.971075 ) \
      (       20.977572 ) \
      (      0.29969258 ) \
      (    0.0022101488 ) \
      (   1.7086466e-05 ) \
      (    1.563094e-07 ) \
      (    3.187597e-08 ) \
      (   1.7527782e-08 ) \
      (  -6.0172169e-09 ) \
      (   5.2677933e-09 ) \
      (  -2.1405867e-08 )

        
CMD CRS_TPU_UP_STRCT_9_4   \          
            432000.000               86400.000 \      # t0 and tau scaling parameters
      (        149635.5 ) \
      (       1116618.4 ) \
      (       16.495383 ) \
      (       19.529392 ) \
      (      0.21284514 ) \
      (    0.0014793182 ) \
      (   1.0980817e-05 ) \
      (   9.7868906e-08 ) \
      (   2.1900632e-08 ) \
      (   1.2150849e-08 ) \
      (  -4.2273314e-09 ) \
      (   3.6343408e-09 ) \
      (  -1.4788384e-08 )

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