Skip to content

Instantly share code, notes, and snippets.

@iczero
Created December 14, 2021 01:40
Show Gist options
  • Save iczero/12baa17a0ea24e73288133eca1ca8020 to your computer and use it in GitHub Desktop.
Save iczero/12baa17a0ea24e73288133eca1ca8020 to your computer and use it in GitHub Desktop.
awefwef
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
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment