# 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 \
     3 \                     # 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
   
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.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 )


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.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 )


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.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 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   \
     3 \                     # 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=8, ephemeris load part 2</test-comment>"   
CMD CRS_TPU_UP_STRCT_8_2   \
        210150.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=8, ephemeris load part 3</test-comment>"   
CMD CRS_TPU_UP_STRCT_8_3   \          
        210150.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=8, ephemeris load part 4</test-comment>"   
CMD CRS_TPU_UP_STRCT_8_4   \          
       210150.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 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   \          
     3 \                     # 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
     
CMD CRS_TPU_UP_STRCT_9_2   \          
          296550.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  )
     
CMD CRS_TPU_UP_STRCT_9_3   \          
          296550.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 )

        
CMD CRS_TPU_UP_STRCT_9_4   \          
          296550.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 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