awefwef

sun.rb

require 'matrix.rb'

def vector_rotation_to_quaternion(v1, v2)
  b = v1.dot(v2)
  c = v1.cross(v2)
  angle = Math.atan2(c.norm(), b)
  m = Math.sin(angle / 2)
  if c.zero? then
    a = c
  else
    a = c.normalize()
  end
  return Vector[a[0] * m, a[1] * m, a[2] * m, Math.cos(angle / 2)]
end

def quaternion_multiply(q1, q2)
  x = q1[3] * q2[0] + q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1]
  y = q1[3] * q2[1] - q1[0] * q2[2] + q1[1] * q2[3] + q1[2] * q2[0]
  z = q1[3] * q2[2] + q1[0] * q2[1] - q1[1] * q2[0] + q1[2] * q2[3]
  w = q1[3] * q2[3] - q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2]
  return Vector[x, y, z, w]
end

# use -x panel
TARGET_VEC = Vector[0, -1, 0]

def awefwef()
  # star tracker state tracking
  star_tracker_voltage = tlm("EPS_MGR FSW_TLM_PKT STAR_TRACKER_VOLTAGE")
  star_tracker_enabled = star_tracker_voltage != 0

  # reaction wheels state tracking
  wheel_voltage = tlm("EPS_MGR FSW_TLM_PKT WHEEL_VOLTAGE")
  wheel_enabled = wheel_voltage != 0

  # coarse sun sensor data
  css_posx = tlm("ADCS HK_FSW_TLM_PKT CSS_ILLUM_YAW_0")
  css_negx = tlm("ADCS HK_FSW_TLM_PKT CSS_ILLUM_YAW_180")
  css_posy = tlm("ADCS HK_FSW_TLM_PKT CSS_ILLUM_YAW_90")
  css_negy = tlm("ADCS HK_FSW_TLM_PKT CSS_ILLUM_YAW_270")
  css_posz = tlm("ADCS HK_FSW_TLM_PKT CSS_ILLUM_Z")
  css_negz = tlm("ADCS HK_FSW_TLM_PKT CSS_ILLUM_Z_N")

  css_vec = Vector[css_posx - css_negx, css_posy - css_negy, css_posz - css_negz]
  if css_vec.zero? then
    puts("No sun found")
    if star_tracker_enabled then
      # disable star tracker if on dark side
      cmd("EPS_MGR", "SET_SWITCH_STATE", "COMPONENT_IDX" => "ADCS_STAR_TRACKER", "COMPONENT_STAT" => "OFF")
    end
    if wheel_enabled then
      # disable wheel if on dark side
      cmd("EPS_MGR", "SET_SWITCH_STATE", "COMPONENT_IDX" => "ADCS_REACTION_WHEEL", "COMPONENT_STAT" => "OFF")
    end
    return
  end
  css_vec = css_vec.normalize()

  print("CSS vector is: ")
  puts(css_vec)

  target_rotation = vector_rotation_to_quaternion(TARGET_VEC, css_vec)
  print("Target rotation quaternion: ")
  puts(target_rotation)

  cr_x = tlm("ADCS HK_FSW_TLM_PKT QBL_X")
  cr_y = tlm("ADCS HK_FSW_TLM_PKT QBL_Y")
  cr_z = tlm("ADCS HK_FSW_TLM_PKT QBL_Z")
  cr_w = tlm("ADCS HK_FSW_TLM_PKT QBL_S")
  current_rotation = Vector[cr_x, cr_y, cr_z, cr_w]
  print("Current rotation quaternion: ")
  puts(current_rotation)

  new_target = quaternion_multiply(current_rotation, target_rotation)
  print("Target for QBL: ")
  puts(new_target)

  # invert for QTR
  new_target = Vector[-new_target[0], -new_target[1], -new_target[2], new_target[3]]

  cmd("ADCS", "SET_CONTROL_TARGET", "TARGET_FRAME" => "LVLH", "QTR" => new_target)

  # enable star tracker if necessary
  if not star_tracker_enabled then
    cmd("EPS_MGR", "SET_SWITCH_STATE", "COMPONENT_IDX" => "ADCS_STAR_TRACKER", "COMPONENT_STAT" => "ON")
  end
  # enable wheels if necessary
  if not wheel_enabled then
    cmd("EPS_MGR", "SET_SWITCH_STATE", "COMPONENT_IDX" => "ADCS_REACTION_WHEEL", "COMPONENT_STAT" => "ON")
  end
end

while true
  awefwef()
  wait(15)
end