class MTTModel(ComponentBase):
def __init__(self, mtt_properties):
ComponentBase.__init__(self, 50)
self.q_b2c = Quaternions(mtt_properties['q_b2c'])
self.max_c_am2 = mtt_properties['max_c_am2']
self.min_c_am2 = mtt_properties['min_c_am2']
self.bias_c = mtt_properties['bias_c']
self.rw_stddev_c = mtt_properties['rw_stddev_c']
self.rw_limit_c = mtt_properties['rw_limit_c']
self.nr_stddev_c = mtt_properties['nr_stddev_c']
self.step_width = mtt_properties['prop_step']
self.historical_mtt_torque_b = []
self.mtt_torque_b = np.zeros(3)
def main_routine(self, count, sc_isDark):
return
def set_step_width(self, value):
if value < self.step_width:
self.step_width = value
def calc_torque(self, control_torque_b, mag_earth_b):
# Body frame to components frame
mag_earth_c = self.q_b2c.frame_conv(mag_earth_b)
control_mag_mom_b = np.cross(mag_earth_b, control_torque_b)
control_mag_mom_c = self.q_b2c.frame_conv(control_mag_mom_b)
for i in range(3):
if control_mag_mom_c[i] > self.max_c_am2[i]:
control_mag_mom_c[i] = self.max_c_am2[i]
elif control_mag_mom_c[i] < self.min_c_am2[i]:
control_mag_mom_c[i] = self.min_c_am2[i]
mtt_torque_c = np.cross(control_mag_mom_c, nT2T * mag_earth_c)
q_c2b = Quaternions(self.q_b2c.conjugate())
self.mtt_torque_b = q_c2b.frame_conv(mtt_torque_c)
return self.mtt_torque_b
def get_torque(self):
return self.mtt_torque_b
def get_current(self):
return
def log_value(self):
self.historical_mtt_torque_b.append(self.mtt_torque_b)
def get_log_values(self, subsys):
report = {
'RWModel' + subsys + '_c(X)[Nm]':
np.array(self.historical_mtt_torque_b)[:, 0],
'RWModel' + subsys + '_c(Y)[Nm]':
np.array(self.historical_mtt_torque_b)[:, 1],
'RWModel' + subsys + '_c(Z)[Nm]':
np.array(self.historical_mtt_torque_b)[:, 2]
}
return report
def __init__(self, init_componenets, subsystem_setting, dynamics):
# prescalar_time in [ms]
ComponentBase.__init__(
self,
prescalar_time=(1000 / subsystem_setting['ADCS_com_frequency']))
# component quaternion
self.q_b2c = subsystem_setting['q_b2c']
# cycle in [ms]
self.ctrl_cycle = 1000 / subsystem_setting['ADCS_com_frequency']
self.port_id = subsystem_setting['port_id']
self.comp_number = subsystem_setting['ADCS_COMPONENT_NUMBER']
self.dynamics = dynamics
self.components = Components(init_componenets, self.dynamics,
self.port_id)
self.current_omega_c_gyro = np.zeros(3)
self.torque_rw_b = np.zeros(3)
self.omega_b_est = np.zeros(3)
self.control_torque = np.zeros(3)
self.historical_control = []
self.P_omega = subsystem_setting['P_omega']
self.I_quat = subsystem_setting['I_quat']
self.P_quat = subsystem_setting['P_quat']
self.rw_torque_b = np.zeros(3)
self.q_i2b_est = Quaternions([0, 0, 0, 1])
self.q_b2b_now2tar = Quaternions([0, 0, 0, 1])
self.q_i2b_tar = Quaternions([0, 0, 0, 1])
self.adcs_mode = DETUMBLING
for rw in self.components.rwmodel:
rw.set_step_width(self.ctrl_cycle / 1000)
self.controller = Controller().pid(self.P_quat, self.I_quat,
self.P_omega,
self.ctrl_cycle / 1000)
def __init__(self, init_componenets, subsystem_setting, dynamics):
ComponentBase.__init__(
self, (1000 / subsystem_setting['ODCS_com_frequency']))
self.dynamics = dynamics
self.ctrl_cycle = 1000 / subsystem_setting['ODCS_com_frequency']
self.port_id = subsystem_setting['port_id']
self.components = Components(init_componenets, self.dynamics,
self.port_id)
self.current_omega_c_gyro = np.zeros(3)
self.current_position_i = np.zeros(3)
self.current_velocity_i = np.zeros(3)
self.force_thruster_i = np.array([0.0, 0.0, 0.0])
self.force_thruster_b = np.array([0.0, 0.0, 0.0])
self.torque_thruster_b = np.zeros(3)
self.target_position = np.zeros(3)
self.target_velocity = np.zeros(3)
self.q_i2b_est = Quaternions([0, 0, 0, 1])
self.q_b2b_now2tar = Quaternions([0, 0, 0, 1])
self.q_i2b_tar = Quaternions([0, 0, 0, 1])
self.ctrl_ang = np.pi / 4
self.mag_thrust = 0
self.control_torque_b = np.zeros(3)
self.control_force_i = np.zeros(3)
self.historical_control_torque_b = []
self.historical_control_force_i = []
self.historical_thruster_i = []
self.g = np.array([0, 0, -1.62])
self.sliding = SlidingControl(self.g, self.dynamics.attitude.Inertia,
self.dynamics.attitude.inv_Inertia,
self.dynamics.attitude.current_mass,
self.ctrl_cycle)
for thr in self.components.thruster:
thr.set_step_width(self.ctrl_cycle / 1000)
def __init__(self, init_componenets, subsystem_setting, dynamics):
# prescalar_time in [ms]
ComponentBase.__init__(
self,
prescalar_time=(1000 / subsystem_setting['ADCS_com_frequency']))
# component quaternion
self.q_b2c = subsystem_setting['q_b2c']
# cycle in [ms]
self.ctrl_cycle = 1000 / subsystem_setting['ADCS_com_frequency']
self.port_id = subsystem_setting['port_id']
self.comp_number = subsystem_setting['ADCS_COMPONENT_NUMBER']
self.dynamics = dynamics
self.components = Components(init_componenets, self.dynamics,
self.port_id)
self.current_omega_c_gyro = np.zeros(3)
self.torque_rw_b = np.zeros(3)
self.error_int = 0
self.omega_b_est = np.zeros(3)
self.control_torque = np.zeros(3)
self.P_omega = subsystem_setting['P_omega']
self.I_quat = subsystem_setting['I_quat']
self.P_quat = subsystem_setting['P_quat']
self.rw_torque_b = np.zeros(3)
self.q_i2b_est = Quaternions([0, 0, 0, 1])
self.q_b2b_now2tar = Quaternions([0, 0, 0, 1])
self.q_i2b_tar = Quaternions([0, 0, 0, 1])
def __init__(self, attitude_spacecraft):
self.attitudestep = attitude_spacecraft['attitudestep']
self.attitudecountTime = 0
# First time is False to not update the init data
self.attitude_update_flag = False
# quaternion = [i, j, k, 1]
self.current_quaternion_i2b = Quaternions(
attitude_spacecraft['Quaternion_i2b'])
self.current_omega_b = np.array(attitude_spacecraft['Omega_b'])
self.Inertia = attitude_spacecraft['Inertia']
self.inv_Inertia = np.linalg.inv(self.Inertia)
self.current_mass = attitude_spacecraft['Mass']
self.current_h_rw_b = np.zeros(3)
self.int_torque_b = np.zeros(3)
self.ext_torque_b = np.zeros(3)
self.h_total_b = np.zeros(3)
self.h_total_i_norm = 0
self.new_omega_b = np.zeros(3)
self.new_quaternion_i2b = np.zeros(3)
self.h_total_i = np.zeros(3)
self.int_force_b = np.zeros(3)
self.ext_force_b = np.zeros(3)
self.S_omega = np.zeros((3, 3))
self.Omega = np.zeros((4, 4))
self.historical_quaternion_i2b = []
self.historical_omega_b = []
self.historical_torque_t_b = []
self.historical_h_total_i = []
self.historical_mass = []
def __init__(self, mtt_properties):
ComponentBase.__init__(self, 50)
self.q_b2c = Quaternions(mtt_properties['q_b2c'])
self.max_c_am2 = mtt_properties['max_c_am2']
self.min_c_am2 = mtt_properties['min_c_am2']
self.bias_c = mtt_properties['bias_c']
self.rw_stddev_c = mtt_properties['rw_stddev_c']
self.rw_limit_c = mtt_properties['rw_limit_c']
self.nr_stddev_c = mtt_properties['nr_stddev_c']
self.step_width = mtt_properties['prop_step']
self.historical_mtt_torque_b = []
self.mtt_torque_b = np.zeros(3)
def __init__(self, port_id, properties, dynamics):
ComponentBase.__init__(self, 10)
self.dynamics_in_gyro = dynamics
self.port_id = port_id
self.current = properties['current']
self.q_b2c = Quaternions(properties['q_b2c'])
self.scalefactor = properties['ScaleFactor']
self.bias_c = properties['Bias_c']
self.n_rw_c = RandomWalk(properties['rw_stepwidth'], properties['rw_stddev_c'], properties['rw_limit_c'])
self.range_to_const = properties['Range_to_const']
self.range_to_zero = properties['Range_to_zero']
self.current_omega_c = np.zeros(3)
self.historical_omega_c = []
self.scidriver = SCIDriver()
self.scidriver.connect_port(self.port_id, 0, 0)
def __init__(self, port_id, properties, dynamics):
ComponentBase.__init__(self, 2)
self.dynamics_in_mag = dynamics
self.port_id = port_id
self.current = properties['current']
self.q_b2c = Quaternions(properties['q_b2c'])
self.scalefactor = properties['ScaleFactor']
self.bias_c = properties['Bias_c']
self.nrs_c = NormalRandom(properties['nr_stddev_c'])
self.n_rw_c = RandomWalk(properties['rw_stepwidth'],
properties['rw_stddev_c'],
properties['rw_limit_c'])
self.current_magVect_c = np.zeros(3)
self.historical_magVect_c = []
self.scidriver = SCIDriver()
self.scidriver.connect_port(self.port_id, 0, 0)
def calculate_control_torque(self):
q_b2i_est = Quaternions(self.q_i2b_est.conjugate())
# First it is necessary to pass the quaternion from attitude to inertial,
# then the target vector is rotated from the inertial to body frame
q_i2b_now2tar = q_b2i_est * self.q_i2b_tar
q_i2b_now2tar.normalize()
torque_direction = np.zeros(3)
torque_direction[0] = q_i2b_now2tar()[0]
torque_direction[1] = q_i2b_now2tar()[1]
torque_direction[2] = q_i2b_now2tar()[2]
angle_rotation = 2 * np.arccos(q_i2b_now2tar()[3])
error_omega_ = self.omega_b_tar - self.omega_b_est
error_ = angle_rotation * torque_direction
"""
self.controller.open_loop([self.dynamics.attitude.current_quaternion_i2b,
self.dynamics.attitude.current_omega_b,
self.control_torque],
self.dynamics.simtime.current_jd)
self.control_torque = np.zeros(3)
"""
self.control_torque = self.controller.closed_loop([
self.dynamics.attitude.current_quaternion_i2b,
self.dynamics.attitude.current_omega_b, self.control_torque
], self.dynamics.simtime.current_jd)
def __init__(self, port_id, properties, dynamics):
ComponentBase.__init__(self, 10)
self.dynamics = dynamics
self.port_id = port_id
self.pos_vector = properties['pos_vector']
self.normal_vector = properties['normal_vector']
self.q_b2c = Quaternions(properties['q_b2c'])
self.q_c2b = Quaternions(self.q_b2c.conjugate())
self.calib_h = properties['h']
self.calib_x0 = properties['x0']
self.calib_y0 = properties['y0']
self.calib_delta = np.deg2rad(properties['delta'])
self.xyd_nr_std = properties['nr_stddev_c']
self.scidriver = SCIDriver()
self.xyd_nrs_c = NormalRandom([self.xyd_nr_std, 0, 0])
self.scidriver.connect_port(self.port_id, 0, 0)
self.current_sun_vector_b = np.zeros(3)
self.current_sun_vector_c = np.zeros(3)
self.historical_sun_vector_b = []
self.historical_sun_vector_c = []
self.historical_V_ratio_m = []
self.historical_rd_m = []
self.historical_theta_m = []
self.historical_phi_m = []
self.cos_theta = 0
self.theta_fss = 0
self.phi_fss = 0
self.T = np.array(
[[np.cos(self.calib_delta),
np.sin(self.calib_delta)],
[-np.sin(self.calib_delta),
np.cos(self.calib_delta)]])
self.Tinv = np.array(
[[np.cos(self.calib_delta), -np.sin(self.calib_delta)],
[np.sin(self.calib_delta),
np.cos(self.calib_delta)]])
self.calib_params = [
self.calib_h, self.calib_x0, self.calib_y0, self.T, self.q_c2b
]
self.V0_ratio = np.array([self.calib_x0, self.calib_y0])
self.V_ratio_m = np.zeros(2)
self.rfss_c = np.zeros(3)
self.rd_c = np.zeros(2)
self.condition_ = False
self.albeo_correction = False
def calc_torque(self, control_torque_b, mag_earth_b):
# Body frame to components frame
mag_earth_c = self.q_b2c.frame_conv(mag_earth_b)
control_mag_mom_b = np.cross(mag_earth_b, control_torque_b)
control_mag_mom_c = self.q_b2c.frame_conv(control_mag_mom_b)
for i in range(3):
if control_mag_mom_c[i] > self.max_c_am2[i]:
control_mag_mom_c[i] = self.max_c_am2[i]
elif control_mag_mom_c[i] < self.min_c_am2[i]:
control_mag_mom_c[i] = self.min_c_am2[i]
mtt_torque_c = np.cross(control_mag_mom_c, nT2T * mag_earth_c)
q_c2b = Quaternions(self.q_b2c.conjugate())
self.mtt_torque_b = q_c2b.frame_conv(mtt_torque_c)
return self.mtt_torque_b
def rungeonestep(self, last_omega_b, last_quaternion_q_i2b, last_torque_b):
dt = self.mpc_sim_step_prop
x = np.concatenate((last_omega_b, last_quaternion_q_i2b()))
k1 = self.dynamics_and_kinematics(x, last_torque_b)
xk2 = x + (dt / 2.0) * k1
k2 = self.dynamics_and_kinematics(xk2, last_torque_b)
xk3 = x + (dt / 2.0) * k2
k3 = self.dynamics_and_kinematics(xk3, last_torque_b)
xk4 = x + dt * k3
k4 = self.dynamics_and_kinematics(xk4, last_torque_b)
next_x = x + (dt / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4)
omega_b = next_x[0:3]
q_i2b = Quaternions(next_x[3:])
q_i2b.normalize()
return q_i2b, omega_b
def calculate_control_force_torque(self):
q_b2i_est = Quaternions(self.q_i2b_est.conjugate())
# First it is necessary to pass the quaternion from attitude to inertial,
# then the target vector is rotated from the inertial to body frame
q_i2b_now2tar = q_b2i_est * self.q_i2b_tar
q_i2b_now2tar.normalize()
omega_b_tar = np.zeros(3)
error_omega = omega_b_tar - self.dynamics.attitude.current_omega_b
error_vel = self.dynamics.trajectory.current_velocity - np.zeros(3)
error_pos = self.dynamics.trajectory.current_position - np.zeros(3)
self.sliding.set_current_state(error_pos, error_vel, error_omega,
q_i2b_now2tar)
self.sliding.calc_force_torque()
self.control_force_i = self.sliding.get_force_i()
self.torque_thruster_b = self.sliding.get_torque_b()
return
def __init__(self, control_mode, dynamics, controller_parameters,
ctrl_cycle):
self.mpc_control_mode = control_mode
if self.mpc_control_mode == REF_POINT:
self.objective_func = self.objective_func_for_constant_ref_point
elif self.mpc_control_mode == NAD_POINT or self.mpc_control_mode == GS_POINT:
self.objective_func = self.objective_func_for_variable_ref_point
elif self.mpc_control_mode == DETUMBLING:
self.objective_func = self.objective_func_for_detumbling
else:
print('Control mode not selected')
self.mpc_inertia = dynamics.attitude.Inertia
self.mpc_inv_inertia = dynamics.attitude.inv_Inertia
self.mpc_current_omega_b = np.zeros(3)
self.mpc_current_quaternion_i2b = np.zeros(4)
self.mpc_current_h_total_b = np.zeros(3)
self.mpc_current_torque_b = np.zeros(3)
self.mpc_dynamics_orb = deepcopy(dynamics.orbit.propagator_model)
self.N_pred_horiz = controller_parameters['N_pred_horiz']
self.mpc_main_count_time = 0
self.mpc_start_jd = dynamics.simtime.current_jd
self.mpc_att_step_prop = dynamics.simtime.attitudestep
self.mpc_orb_step_prop = dynamics.simtime.orbitstep
self.mpc_sim_step_prop = dynamics.simtime.stepsimTime
self.mpc_ctrl_cycle = ctrl_cycle
self.mpc_array_jd = []
self.mpc_array_sc_pos_i = []
self.mpc_array_sc_vel_i = []
self.mpc_array_sideral = []
self.mpc_array_tar_pos_i = []
self.mpc_array_tar_pos_sc_i = []
self.mpc_array_lat = []
self.mpc_array_lon = []
self.mpc_array_alt = []
self.mpc_array_mag_i = []
self.mpc_array_decyear = []
# Geodetic to ECEF
self.tar_pos_ecef = geodetic_to_ecef(
tar_alt * 1e-3, tar_long * deg2rad, tar_lat * deg2rad) * 1e3
# Vector direction of the Body frame to point to another vector
self.b_dir = np.array([0, 0, 1])
self.current_tar_sc_i = np.array([1, 1, 1])
self.q_b2b_now2tar = Quaternions([0, 0, 0, 1])
def calculate_control_torque(self):
q_b2i_est = Quaternions(self.q_i2b_est.conjugate())
# First it is necessary to pass the quaternion from attitude to inertial,
# then the target vector is rotated from the inertial to body frame
q_i2b_now2tar = q_b2i_est * self.q_i2b_tar
q_i2b_now2tar.normalize()
torque_direction = np.zeros(3)
torque_direction[0] = q_i2b_now2tar()[0]
torque_direction[1] = q_i2b_now2tar()[1]
torque_direction[2] = q_i2b_now2tar()[2]
angle_rotation = 2 * np.arccos(q_i2b_now2tar()[3])
error_omega_ = self.omega_b_tar - self.omega_b_est
error_ = angle_rotation * torque_direction
control = self.controller.calc_control(error_, error_omega_,
self.adcs_mode)
self.control_torque = control
def calculate_control_torque(self):
q_b2i_est = Quaternions(self.q_i2b_est.conjugate())
# First it is necessary to pass the quaternion from attitude to inertial,
# then the target vector is rotated from the inertial to body frame
q_i2b_now2tar = q_b2i_est * self.q_i2b_tar
q_i2b_now2tar.normalize()
torque_direction = np.zeros(3)
torque_direction[0] = q_i2b_now2tar()[0]
torque_direction[1] = q_i2b_now2tar()[1]
torque_direction[2] = q_i2b_now2tar()[2]
angle_rotation = 2*np.arccos(q_i2b_now2tar()[3])
error_integral = self.ctrl_cycle * angle_rotation * torque_direction
omega_b_tar = np.array([-0.0, -0.0, 0.0])
error_omega = omega_b_tar - self.omega_b_est
self.error_int += self.ctrl_cycle*error_omega
self.control_torque = self.P_quat.dot(angle_rotation*torque_direction) + self.I_quat.dot(error_integral)
class ADCS(ComponentBase):
def __init__(self, init_componenets, subsystem_setting, dynamics):
# prescalar_time in [ms]
ComponentBase.__init__(
self,
prescalar_time=(1000 / subsystem_setting['ADCS_com_frequency']))
# component quaternion
self.q_b2c = subsystem_setting['q_b2c']
# cycle in [ms]
self.ctrl_cycle = 1000 / subsystem_setting['ADCS_com_frequency']
self.port_id = subsystem_setting['port_id']
self.comp_number = subsystem_setting['ADCS_COMPONENT_NUMBER']
self.dynamics = dynamics
self.components = Components(init_componenets, self.dynamics,
self.port_id)
self.current_omega_c_gyro = np.zeros(3)
self.current_magVect_c_magSensor = np.zeros(3)
self.magVect_i = np.zeros(3)
self.magVect_b = np.zeros(3)
self.sunPos_i = np.zeros(3)
self.sunDir_b = np.zeros(3)
self.sunPos_est_b = np.zeros(3)
self.torque_rw_b = np.zeros(3)
self.omega_b_est = np.zeros(3)
self.omega_b_tar = np.zeros(3)
self.b_dir = np.zeros(3)
self.b_tar_b = np.zeros(3)
self.b_tar_i = np.zeros(3)
self.control_torque = np.zeros(3)
self.tar_pos_eci = np.zeros(3)
self.historical_control = []
self.historical_estimation = []
self.historical_P = []
self.historical_P_det = []
self.historical_P_det = []
self.historical_magVect_i = []
self.historical_ssEst_b = []
self.historical_fssEst_b = []
self.historical_fssQdr_d = []
self.historical_ssDir_b = []
self.historical_eskf_res = []
self.historical_eskf_obserrmag = []
self.historical_eskf_obserrcss = []
self.historical_eskf_bias = []
self.historical_theta_e = []
self.historical_b_tar_b = []
self.historical_b_tar_i = []
self.historical_b_dir_b = []
self.historical_omega_b_tar = []
self.historical_vec_dir_tar_b = []
self.historical_calc_time = []
self.historical_cost_function = []
self.historical_tar_pos_eci = []
self.current_calc_time = 0
self.current_theta_e = 0
self.current_cost = 0
self.vec_u_e = np.zeros(3)
self.P_omega = subsystem_setting['P_omega']
self.I_quat = subsystem_setting['I_quat']
self.P_quat = subsystem_setting['P_quat']
self.rw_torque_b = np.zeros(3)
self.q_i2b_est = Quaternions([0, 0, 0, 1])
self.q_i2b_est_eskf_temp = Quaternions([0, 0, 0, 1])
self.q_b2b_now2tar = Quaternions([0, 0, 0, 1])
self.q_i2b_tar = Quaternions([0, 0, 0, 1])
self.eskf = ErrorStateKalmanFilter(
dim_x=7,
dim_dx=6,
dim_u=3,
dim_z=3,
inertia=self.dynamics.attitude.Inertia,
invInertia=self.dynamics.attitude.inv_Inertia)
self.jacobians = Jacobians()
self.number_ss = len(self.components.sunsensors)
self.number_fss = len(self.components.fss)
self.current_I_sunsensors = np.zeros(self.number_ss)
self.V_ratios_fss = np.zeros((self.number_fss, 2))
self.params_fss = [None] * self.number_fss
self.rsfss_b = np.zeros((self.number_fss, 3))
self.qdrsfss_b = np.zeros((self.number_fss, 2))
self.tick_temp = 1
self.adcs_mode = GS_POINT
self.components.mtt.set_step_width(self.ctrl_cycle / 1000)
for rw in self.components.rwmodel:
rw.set_step_width(self.ctrl_cycle / 1000)
# self.controller = Controller().pid(self.P_quat, self.I_quat, self.P_omega, self.ctrl_cycle/1000)
control_parameters = {'N_pred_horiz': 10}
# Geodetic to ECEF
self.tar_pos_ecef = geodetic_to_ecef(
tar_alt * 1e-3, tar_long * deg2rad, tar_lat * deg2rad) * 1e3
self.controller = Controller().mpc(self.adcs_mode, self.dynamics,
control_parameters, self.ctrl_cycle)
def main_routine(self, count, sc_isDark):
# self.read_sensors(sc_isDark)
self.determine_attitude()
self.check_mode()
self.calculate_control_torque()
# self.calc_rw_torque()
# self.calc_mtt_torque()
return
def read_sensors(self, sc_isDark):
self.current_omega_c_gyro = self.components.gyro.measure(
self.dynamics.attitude.current_omega_b)
self.current_magVect_c_magSensor = self.components.mag.measure(
self.magVect_b)
for i in range(self.number_ss):
self.components.sunsensors[i].measure(sc_isDark)
self.current_I_sunsensors[i] = self.components.sunsensors[
i].get_I_b()
for i in range(self.number_fss):
self.components.fss[i].measure(sc_isDark)
self.V_ratios_fss[i] = self.components.fss[i].get_V_ratio_b()
self.params_fss[i] = self.components.fss[i].calib_params
def determine_attitude(self):
# To do: Omega Estimate
self.omega_b_est = self.dynamics.attitude.current_omega_b
# True/Simulated quaternion
att = self.dynamics.attitude.get_current_q_i2b()
self.q_i2b_est.setquaternion(att)
# # Simulated Sun Vector preprocessing
# self.sunPos_i = self.dynamics.ephemeris.selected_body['SUN'].get_pos_from_center_i()
# self.sunDir_b = unitVector(self.dynamics.ephemeris.selected_body['SUN'].get_pos_from_sc_b())
# # Coarse Sun Sensor preprocessing (vector extraction from currents)
# self.sunPos_est_b = self.eskf.get_ss_vect(self.current_I_sunsensors, self.components.sunsensors[0].I_max)
# # Fine Sun Sensor preprocessing (vector extraction from quadrature voltage)
# for i in range(self.number_fss):
# self.rsfss_b[i] = self.eskf.get_fss_vect(self.V_ratios_fss[i], self.params_fss[i])
# self.qdrsfss_b[i] = self.eskf.get_fss_qdvect(self.V_ratios_fss[i], self.params_fss[i])
# # Fine Sun Sensor average preprocessing (vector extraction from quadrature voltage)
# self.rsfss_mean_b = self.eskf.get_fss_mean_vect(self.rsfss_b)
# Estimate Spacecraft Attitude quaternion
# self.eskf_process(self.ctrl_cycle/1000)
return
def check_mode(self):
if self.adcs_mode == DETUMBLING:
self.omega_b_tar = np.array([0.0, 0.0, 0.0])
# self.controller.set_gain(self.P_omega, self.I_quat, np.diag([0.0, 0.0, 0.0]))
elif self.adcs_mode == NAD_POINT:
print('Nadir pointing mode...')
elif self.adcs_mode == REF_POINT:
# Vector direction of the Body frame to point to another vector
self.b_dir = np.array([0, 0, 1])
# Vector target from Inertial frame
i_tar = np.array([1, -1, -1])
i_tar = i_tar / np.linalg.norm(i_tar)
self.b_tar_i = i_tar
self.controller.set_ref_vector_i(i_tar)
# Vector target from body frame
self.b_tar_b = self.q_i2b_est.frame_conv(i_tar)
self.b_tar_b /= np.linalg.norm(self.b_tar_b)
self.current_theta_e = np.arccos(np.dot(self.b_dir, self.b_tar_b))
self.vec_u_e = np.cross(self.b_dir, self.b_tar_b)
self.vec_u_e /= np.linalg.norm(self.vec_u_e)
self.q_b2b_now2tar.setquaternion(
[self.vec_u_e, self.current_theta_e])
self.q_b2b_now2tar.normalize()
self.q_i2b_tar = self.q_i2b_est * self.q_b2b_now2tar
elif self.adcs_mode == GS_POINT:
# omega_target_b error
sideral = gstime(self.dynamics.simtime.current_jd)
tar_pos_i = rotationZ(self.tar_pos_ecef, -sideral)
self.tar_pos_eci = tar_pos_i
vel_gs_i = np.cross(earth_rot_i, tar_pos_i)
vel_gs_sc = vel_gs_i - self.dynamics.orbit.current_velocity_i
pos_sc2tar_i = tar_pos_i - self.dynamics.orbit.current_position_i
mag_omega_gs_sc = np.linalg.norm(vel_gs_sc) / np.linalg.norm(
pos_sc2tar_i)
unit_vec_pos = pos_sc2tar_i / np.linalg.norm(pos_sc2tar_i)
unit_vec_vel = vel_gs_sc / np.linalg.norm(vel_gs_sc)
unit_vec_omega_gs_sc = np.cross(unit_vec_pos, unit_vec_vel)
omega_gs_from_sc_i = mag_omega_gs_sc * unit_vec_omega_gs_sc
self.omega_b_tar = self.q_i2b_est.frame_conv(omega_gs_from_sc_i)
# Vector direction of the Body frame to point to another vector
self.b_dir = np.array([0, 0, 1])
# Error
self.b_tar_i = pos_sc2tar_i / np.linalg.norm(pos_sc2tar_i)
current_tar_pos_b = self.q_i2b_est.frame_conv(pos_sc2tar_i)
self.b_tar_b = current_tar_pos_b / np.linalg.norm(
current_tar_pos_b)
self.current_theta_e = np.arccos(np.dot(self.b_dir, self.b_tar_b))
self.vec_u_e = np.cross(self.b_dir, self.b_tar_b)
self.vec_u_e /= np.linalg.norm(self.vec_u_e)
self.q_b2b_now2tar.setquaternion(
[self.vec_u_e, self.current_theta_e])
self.q_b2b_now2tar.normalize()
self.q_i2b_tar = self.q_i2b_est * self.q_b2b_now2tar
else:
print('No mode selected')
def calculate_control_torque(self):
q_b2i_est = Quaternions(self.q_i2b_est.conjugate())
# First it is necessary to pass the quaternion from attitude to inertial,
# then the target vector is rotated from the inertial to body frame
q_i2b_now2tar = q_b2i_est * self.q_i2b_tar
q_i2b_now2tar.normalize()
torque_direction = np.zeros(3)
torque_direction[0] = q_i2b_now2tar()[0]
torque_direction[1] = q_i2b_now2tar()[1]
torque_direction[2] = q_i2b_now2tar()[2]
angle_rotation = 2 * np.arccos(q_i2b_now2tar()[3])
# error_omega_ = self.omega_b_tar - self.omega_b_est
# error_ = angle_rotation * torque_direction
start_time = time.time()
# Control MPC
# control_mag_torque, self.current_cost = self.controller.open_loop([self.dynamics.attitude.current_quaternion_i2b,
# self.dynamics.attitude.current_omega_b,
# self.control_torque], self.dynamics.simtime.current_jd)
self.current_calc_time = time.time() - start_time
# self.control_torque = control_mag_torque
# Control PID
# self.control_torque = 2e-5 * angle_rotation * self.vec_u_e - self.omega_b_est * 5e-4
# self.control_torque = 2e-5 * angle_rotation * self.vec_u_e + (self.omega_b_tar - self.omega_b_est) * 5e-4
def calc_mtt_torque(self):
self.components.mtt.calc_torque(self.control_torque,
self.current_magVect_c_magSensor)
return
def calc_rw_torque(self):
f = 0
for rw in self.components.rwmodel:
rw.control_power()
rw.set_torque(self.control_torque[f], self.ctrl_cycle)
self.rw_torque_b += rw.calc_torque(self.ctrl_cycle)
f += 1
return
def get_rwtorque(self):
return self.rw_torque_b
def set_magVector(self, mag_i, mag_b):
self.magVect_i = mag_i
self.magVect_b = mag_b
def save_data(self):
self.historical_control.append(self.control_torque)
self.historical_magVect_i.append(self.magVect_i)
self.historical_theta_e.append(self.current_theta_e)
self.historical_omega_b_tar.append(self.omega_b_tar)
self.historical_b_tar_b.append(self.b_tar_b)
self.historical_b_tar_i.append(self.b_tar_i)
self.historical_b_dir_b.append(self.b_dir)
self.historical_vec_dir_tar_b.append(self.vec_u_e)
self.historical_calc_time.append(self.current_calc_time)
self.historical_cost_function.append(self.current_cost)
self.historical_tar_pos_eci.append(self.tar_pos_eci)
def get_log_values(self, subsys):
report = {
'MTT_' + subsys + '_b(X)[Am]': 0,
'MTT_' + subsys + '_b(Y)[Am]': 0,
'MTT_' + subsys + '_b(Z)[Am]': 0
}
report_control = {
'Control_' + subsys + '_b(X)[Nm]':
np.array(self.historical_control)[:, 0],
'Control_' + subsys + '_b(Y)[Nm]':
np.array(self.historical_control)[:, 1],
'Control_' + subsys + '_b(Z)[Nm]':
np.array(self.historical_control)[:, 2]
}
report_target_state = {
'Theta_error [rad]':
np.array(self.historical_theta_e),
'Vector_dir_b(X) [-]':
np.array(self.historical_b_dir_b)[:, 0],
'Vector_dir_b(Y) [-]':
np.array(self.historical_b_dir_b)[:, 1],
'Vector_dir_b(Z) [-]':
np.array(self.historical_b_dir_b)[:, 2],
'Vector_tar_b(X) [-]':
np.array(self.historical_b_tar_b)[:, 0],
'Vector_tar_b(Y) [-]':
np.array(self.historical_b_tar_b)[:, 1],
'Vector_tar_b(Z) [-]':
np.array(self.historical_b_tar_b)[:, 2],
'Vector_tar_i(X) [-]':
np.array(self.historical_b_tar_i)[:, 0],
'Vector_tar_i(Y) [-]':
np.array(self.historical_b_tar_i)[:, 1],
'Vector_tar_i(Z) [-]':
np.array(self.historical_b_tar_i)[:, 2],
'Vector_dir_tar_b(X) [-]':
np.array(self.historical_vec_dir_tar_b)[:, 0],
'Vector_dir_tar_b(Y) [-]':
np.array(self.historical_vec_dir_tar_b)[:, 1],
'Vector_dir_tar_b(Z) [-]':
np.array(self.historical_vec_dir_tar_b)[:, 2],
'omega_b_tar(X) [rad/s]':
np.array(self.historical_omega_b_tar)[:, 0],
'omega_b_tar(Y) [rad/s]':
np.array(self.historical_omega_b_tar)[:, 1],
'omega_b_tar(Z) [rad/s]':
np.array(self.historical_omega_b_tar)[:, 2],
'Calculation_time [sec]':
np.array(self.historical_calc_time),
'Cost_function [-]':
np.array(self.historical_cost_function),
'tar_pos_eci(X) [m]':
np.array(self.historical_tar_pos_eci)[:, 0],
'tar_pos_eci(Y) [m]':
np.array(self.historical_tar_pos_eci)[:, 1],
'tar_pos_eci(Z) [m]':
np.array(self.historical_tar_pos_eci)[:, 2]
}
report = {**report, **report_control, **report_target_state}
return report
def __init__(self, init_componenets, subsystem_setting, dynamics):
# prescalar_time in [ms]
ComponentBase.__init__(
self,
prescalar_time=(1000 / subsystem_setting['ADCS_com_frequency']))
# component quaternion
self.q_b2c = subsystem_setting['q_b2c']
# cycle in [ms]
self.ctrl_cycle = 1000 / subsystem_setting['ADCS_com_frequency']
self.port_id = subsystem_setting['port_id']
self.comp_number = subsystem_setting['ADCS_COMPONENT_NUMBER']
self.dynamics = dynamics
self.components = Components(init_componenets, self.dynamics,
self.port_id)
self.current_omega_c_gyro = np.zeros(3)
self.current_magVect_c_magSensor = np.zeros(3)
self.magVect_i = np.zeros(3)
self.magVect_b = np.zeros(3)
self.sunPos_i = np.zeros(3)
self.sunDir_b = np.zeros(3)
self.sunPos_est_b = np.zeros(3)
self.torque_rw_b = np.zeros(3)
self.omega_b_est = np.zeros(3)
self.control_torque = np.zeros(3)
self.historical_control = []
self.historical_estimation = []
self.historical_P = []
self.historical_P_det = []
self.historical_P_det = []
self.historical_magVect_i = []
self.historical_ssEst_b = []
self.historical_fssEst_b = []
self.historical_fssQdr_d = []
self.historical_ssDir_b = []
self.historical_eskf_res = []
self.historical_eskf_obserrmag = []
self.historical_eskf_obserrcss = []
self.historical_eskf_bias = []
self.P_omega = subsystem_setting['P_omega']
self.I_quat = subsystem_setting['I_quat']
self.P_quat = subsystem_setting['P_quat']
self.rw_torque_b = np.zeros(3)
self.q_i2b_est = Quaternions([0, 0, 0, 1])
self.q_i2b_est_eskf_temp = Quaternions([0, 0, 0, 1])
self.q_b2b_now2tar = Quaternions([0, 0, 0, 1])
self.q_i2b_tar = Quaternions([0, 0, 0, 1])
self.eskf = ErrorStateKalmanFilter(
dim_x=7,
dim_dx=6,
dim_u=3,
dim_z=3,
inertia=self.dynamics.attitude.Inertia,
invInertia=self.dynamics.attitude.inv_Inertia)
self.jacobians = Jacobians()
self.number_ss = len(self.components.sunsensors)
self.number_fss = len(self.components.fss)
self.current_I_sunsensors = np.zeros(self.number_ss)
self.V_ratios_fss = np.zeros((self.number_fss, 2))
self.params_fss = [None] * self.number_fss
self.rsfss_b = np.zeros((self.number_fss, 3))
self.qdrsfss_b = np.zeros((self.number_fss, 2))
self.tick_temp = 1
self.adcs_mode = DETUMBLING
self.components.mtt.set_step_width(self.ctrl_cycle / 1000)
for rw in self.components.rwmodel:
rw.set_step_width(self.ctrl_cycle / 1000)
# self.controller = Controller().pid(self.P_quat, self.I_quat, self.P_omega, self.ctrl_cycle/1000)
control_parameters = {'N_pred_horiz': 3}
self.controller = Controller().mpc(self.dynamics, control_parameters,
self.ctrl_cycle)
class Magnetometer(ComponentBase):
def __init__(self, port_id, properties, dynamics):
ComponentBase.__init__(self, 2)
self.dynamics_in_mag = dynamics
self.port_id = port_id
self.current = properties['current']
self.q_b2c = Quaternions(properties['q_b2c'])
self.scalefactor = properties['ScaleFactor']
self.bias_c = properties['Bias_c']
self.nrs_c = NormalRandom(properties['nr_stddev_c'])
self.n_rw_c = RandomWalk(properties['rw_stepwidth'],
properties['rw_stddev_c'],
properties['rw_limit_c'])
self.current_magVect_c = np.zeros(3)
self.historical_magVect_c = []
self.scidriver = SCIDriver()
self.scidriver.connect_port(self.port_id, 0, 0)
def main_routine(self, count, sc_isDark):
return
def set_port_id(self, port):
self.port_id = port
def measure(self, magVect_b):
# RangeCheck
# TO DO
# Convert magnetic vector from body (b) coordinate system to sensor actual coordinate system (c)
magVect_c = self.q_b2c.frame_conv(magVect_b)
# Addition of scale factor
magVect_c = self.scalefactor.dot(magVect_c)
# Addition of bias
magVect_c += self.bias_c
# Addition of random walk
#magVect_c += self.n_rw_c()
# Adding gaussian noise
magVect_c += self.nrs_c()
self.current_magVect_c = magVect_c
return self.current_magVect_c
def get_magVect_c(self):
return self.current_magVect_c
def get_log_values(self, subsys):
report = {
'magSensor_refVect' + subsys + '_c(X)[T?]':
np.array(self.historical_magVect_c)[:, 0],
'magSensor_refVect' + subsys + '_c(Y)[T?]':
np.array(self.historical_magVect_c)[:, 1],
'magSensor_refVect' + subsys + '_c(Z)[T?]':
np.array(self.historical_magVect_c)[:, 2]
}
return report
def get_current(self):
return self.current
def log_value(self):
self.historical_magVect_c.append(self.current_magVect_c)
class Gyro(ComponentBase):
def __init__(self, port_id, properties, dynamics):
ComponentBase.__init__(self, 10)
self.dynamics_in_gyro = dynamics
self.port_id = port_id
self.current = properties['current']
self.q_b2c = Quaternions(properties['q_b2c'])
self.scalefactor = properties['ScaleFactor']
self.bias_c = properties['Bias_c']
self.n_rw_c = RandomWalk(properties['rw_stepwidth'], properties['rw_stddev_c'], properties['rw_limit_c'])
self.range_to_const = properties['Range_to_const']
self.range_to_zero = properties['Range_to_zero']
self.current_omega_c = np.zeros(3)
self.historical_omega_c = []
self.scidriver = SCIDriver()
self.scidriver.connect_port(self.port_id, 0, 0)
def main_routine(self, count):
self.measure(self.dynamics_in_gyro.attitude.current_omega_b)
return
def set_port_id(self, port):
self.port_id = port
def measure(self, omega_b):
self.RangeCheck()
# Convert angular velocity from body (b) coordinate system to sensor actual coordinate system (c)
omega_c = self.q_b2c.frame_conv(omega_b)
# Addition of scale factor
omega_c = self.scalefactor.dot(omega_c)
# Addition of bias
omega_c += self.bias_c
# Addition of random walk
#omega_c += self.n_rw_c()
# Adding gaussian noise
#omega_c += self.nrs_c
self.current_omega_c = omega_c
self.clip()
return self.current_omega_c
def RangeCheck(self):
if self.range_to_const < 0.0 or self.range_to_zero < 0.0:
print("Gyro: range should be positive!!")
elif self.range_to_const > self.range_to_zero:
print("Gyro: range2 should be greater than range1!!")
return
def clip(self):
for i in range(self.current_omega_c.size):
if self.range_to_const <= self.current_omega_c[i] < self.range_to_zero:
self.current_omega_c[i] = self.range_to_const
elif -self.range_to_const >= self.current_omega_c[i] > -self.range_to_zero:
self.current_omega_c[i] = -self.range_to_const
elif np.abs(self.current_omega_c[i]) >= self.range_to_zero:
self.current_omega_c[i] = 0.0
return
def get_omega_c(self):
return self.current_omega_c
def get_historical_omega_c(self):
return self.historical_omega_c
def get_current(self):
return self.current
def log_value(self):
self.historical_omega_c.append(self.current_omega_c)
def get_class_q_b2i(self):
return Quaternions(self.current_quaternion_i2b.conjugate())
def calangmom(self):
h_spacecraft_b = self.Inertia.dot(self.current_omega_b)
self.h_total_b = self.current_h_rw_b + h_spacecraft_b
q_bi2 = Quaternions(self.current_quaternion_i2b.conjugate())
self.h_total_i = q_bi2.frame_conv(self.h_total_b)
self.h_total_i_norm = np.linalg.norm(self.h_total_i)
class Attitude(object):
def __init__(self, attitude_spacecraft):
self.attitudestep = attitude_spacecraft['attitudestep']
self.attitudecountTime = 0
# First time is False to not update the init data
self.attitude_update_flag = False
# quaternion = [i, j, k, 1]
self.current_quaternion_i2b = Quaternions(
attitude_spacecraft['Quaternion_i2b'])
self.current_omega_b = np.array(attitude_spacecraft['Omega_b'])
self.Inertia = attitude_spacecraft['Inertia']
self.inv_Inertia = np.linalg.inv(self.Inertia)
self.current_mass = attitude_spacecraft['Mass']
self.current_h_rw_b = np.zeros(3)
self.int_torque_b = np.zeros(3)
self.ext_torque_b = np.zeros(3)
self.h_total_b = np.zeros(3)
self.h_total_i_norm = 0
self.new_omega_b = np.zeros(3)
self.new_quaternion_i2b = np.zeros(3)
self.h_total_i = np.zeros(3)
self.int_force_b = np.zeros(3)
self.ext_force_b = np.zeros(3)
self.S_omega = np.zeros((3, 3))
self.Omega = np.zeros((4, 4))
self.historical_quaternion_i2b = []
self.historical_omega_b = []
self.historical_torque_t_b = []
self.historical_h_total_i = []
self.historical_mass = []
def update_attitude(self, current_simtime):
if self.attitude_update_flag:
while np.abs(current_simtime - self.attitudecountTime -
self.attitudestep) > 1e-6:
self.rungeonestep(self.attitudecountTime, self.attitudestep)
self.attitudecountTime += self.attitudestep
self.rungeonestep(self.attitudecountTime, self.attitudestep)
self.attitudecountTime = current_simtime
else:
self.attitude_update_flag = True
self.calangmom()
self.reset_var_b()
def save_attitude_data(self):
self.historical_quaternion_i2b.append(self.current_quaternion_i2b())
self.historical_omega_b.append(self.current_omega_b)
self.historical_torque_t_b.append(self.total_torque_b())
self.historical_h_total_i.append(self.h_total_i_norm)
self.historical_mass.append(self.current_mass)
def add_ext_torque_b(self, ext_torque_b):
self.ext_torque_b += ext_torque_b
def add_ext_force_b(self, ext_force_b):
self.ext_force_b += ext_force_b
def get_ext_force_b(self):
return self.ext_force_b
def add_int_torque_b(self, torque_b):
self.int_torque_b += torque_b
def add_int_force_b(self, force_b):
self.int_force_b += force_b
def get_current_q_i2b(self):
return self.current_quaternion_i2b()
def get_class_q_i2b(self):
return self.current_quaternion_i2b
def get_class_q_b2i(self):
return Quaternions(self.current_quaternion_i2b.conjugate())
def total_torque_b(self):
return self.ext_torque_b + self.int_torque_b
def reset_var_b(self):
self.ext_torque_b = np.zeros(3)
self.int_torque_b = np.zeros(3)
self.ext_force_b = np.zeros(3)
self.int_force_b = np.zeros(3)
def dynamics_and_kinematics(self, x, t):
x_omega_b = x[0:3]
x_quaternion_i2b = x[3:]
self.skewsymmetricmatrix(x_omega_b)
self.omega4kinematics(x_omega_b)
h_total_b = self.current_h_rw_b + self.Inertia.dot(x_omega_b)
w_dot = -self.inv_Inertia.dot(
self.S_omega.dot(h_total_b) - self.total_torque_b())
q_dot = 0.5 * self.Omega.dot(x_quaternion_i2b)
f_x = np.concatenate((w_dot, q_dot))
return f_x
def rungeonestep(self, t, dt):
x = np.concatenate(
(self.current_omega_b, self.current_quaternion_i2b()))
k1 = self.dynamics_and_kinematics(x, t)
xk2 = x + (dt / 2.0) * k1
k2 = self.dynamics_and_kinematics(xk2, (t + dt / 2.0))
xk3 = x + (dt / 2.0) * k2
k3 = self.dynamics_and_kinematics(xk3, (t + dt / 2.0))
xk4 = x + dt * k3
k4 = self.dynamics_and_kinematics(xk4, (t + dt))
next_x = x + (dt / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4)
self.current_omega_b = next_x[0:3]
self.current_quaternion_i2b.setquaternion(next_x[3:])
self.current_quaternion_i2b.normalize()
def calangmom(self):
h_spacecraft_b = self.Inertia.dot(self.current_omega_b)
self.h_total_b = self.current_h_rw_b + h_spacecraft_b
q_bi2 = Quaternions(self.current_quaternion_i2b.conjugate())
self.h_total_i = q_bi2.frame_conv(self.h_total_b)
self.h_total_i_norm = np.linalg.norm(self.h_total_i)
def skewsymmetricmatrix(self, x_omega_b):
self.S_omega[1, 0] = x_omega_b[2]
self.S_omega[2, 0] = -x_omega_b[1]
self.S_omega[0, 1] = -x_omega_b[2]
self.S_omega[0, 2] = x_omega_b[1]
self.S_omega[2, 1] = x_omega_b[0]
self.S_omega[1, 2] = -x_omega_b[0]
def omega4kinematics(self, x_omega_b):
self.Omega[1, 0] = -x_omega_b[2]
self.Omega[2, 0] = x_omega_b[1]
self.Omega[3, 0] = -x_omega_b[0]
self.Omega[0, 1] = x_omega_b[2]
self.Omega[0, 2] = -x_omega_b[1]
self.Omega[0, 3] = x_omega_b[0]
self.Omega[1, 2] = x_omega_b[0]
self.Omega[1, 3] = x_omega_b[1]
self.Omega[2, 1] = -x_omega_b[0]
self.Omega[2, 3] = x_omega_b[2]
self.Omega[3, 1] = -x_omega_b[1]
self.Omega[3, 2] = -x_omega_b[2]
def get_log_values(self):
report_attitude = {
'omega_t_b(X)[rad/s]': np.array(self.historical_omega_b)[:, 0],
'omega_t_b(Y)[rad/s]': np.array(self.historical_omega_b)[:, 1],
'omega_t_b(Z)[rad/s]': np.array(self.historical_omega_b)[:, 2],
'q_t_i2b(0)[-]': np.array(self.historical_quaternion_i2b)[:, 0],
'q_t_i2b(1)[-]': np.array(self.historical_quaternion_i2b)[:, 1],
'q_t_i2b(2)[-]': np.array(self.historical_quaternion_i2b)[:, 2],
'q_t_i2b(3)[-]': np.array(self.historical_quaternion_i2b)[:, 3],
'torque_t_b(X)[Nm]': np.array(self.historical_torque_t_b)[:, 0],
'torque_t_b(Y)[Nm]': np.array(self.historical_torque_t_b)[:, 1],
'torque_t_b(Z)[Nm]': np.array(self.historical_torque_t_b)[:, 2],
'h_total[Nms]': np.array(self.historical_h_total_i),
'Mass [kg]': np.array(self.historical_mass)
}
return report_attitude
class ADCS(ComponentBase):
def __init__(self, init_componenets, subsystem_setting, dynamics):
# prescalar_time in [ms]
ComponentBase.__init__(
self,
prescalar_time=(1000 / subsystem_setting['ADCS_com_frequency']))
# component quaternion
self.q_b2c = subsystem_setting['q_b2c']
# cycle in [ms]
self.ctrl_cycle = 1000 / subsystem_setting['ADCS_com_frequency']
self.port_id = subsystem_setting['port_id']
self.comp_number = subsystem_setting['ADCS_COMPONENT_NUMBER']
self.dynamics = dynamics
self.components = Components(init_componenets, self.dynamics,
self.port_id)
self.current_omega_c_gyro = np.zeros(3)
self.torque_rw_b = np.zeros(3)
self.error_int = 0
self.omega_b_est = np.zeros(3)
self.control_torque = np.zeros(3)
self.P_omega = subsystem_setting['P_omega']
self.I_quat = subsystem_setting['I_quat']
self.P_quat = subsystem_setting['P_quat']
self.rw_torque_b = np.zeros(3)
self.q_i2b_est = Quaternions([0, 0, 0, 1])
self.q_b2b_now2tar = Quaternions([0, 0, 0, 1])
self.q_i2b_tar = Quaternions([0, 0, 0, 1])
def main_routine(self, count):
self.read_sensors()
self.determine_attitude()
self.check_mode()
self.calculate_control_torque()
return
def read_sensors(self):
self.current_omega_c_gyro = self.components.gyro.measure(
self.dynamics.attitude.current_omega_b)
def determine_attitude(self):
self.omega_b_est = self.current_omega_c_gyro
att = self.dynamics.attitude.get_current_q_i2b()
self.q_i2b_est.setquaternion(att)
return
def check_mode(self):
# Vector direction of the Body frame to point to another vector
b_dir = np.array([0, 0, 1])
# Vector target from Inertial frame
i_tar = np.array([1, 1, 1])
i_tar = i_tar / np.linalg.norm(i_tar)
# Vector target from body frame
b_tar = self.q_i2b_est.frame_conv(i_tar)
b_lambda = np.cross(b_dir, b_tar)
rot = np.arcsin(
np.linalg.norm(b_lambda) /
(np.linalg.norm(b_tar) * np.linalg.norm(b_dir)))
self.q_b2b_now2tar.setquaternion([b_lambda, rot])
self.q_b2b_now2tar.normalize()
self.q_i2b_tar = self.q_i2b_est * self.q_b2b_now2tar
return
def calculate_control_torque(self):
q_b2i_est = Quaternions(self.q_i2b_est.conjugate())
# First it is necessary to pass the quaternion from attitude to inertial,
# then the target vector is rotated from the inertial to body frame
q_i2b_now2tar = q_b2i_est * self.q_i2b_tar
q_i2b_now2tar.normalize()
torque_direction = np.zeros(3)
torque_direction[0] = q_i2b_now2tar()[0]
torque_direction[1] = q_i2b_now2tar()[1]
torque_direction[2] = q_i2b_now2tar()[2]
angle_rotation = 2 * np.arccos(q_i2b_now2tar()[3])
error_integral = self.ctrl_cycle * angle_rotation * torque_direction
omega_b_tar = np.array([-0.0, -0.0, 0.0])
error_omega = omega_b_tar - self.omega_b_est
self.error_int += self.ctrl_cycle * error_omega
self.control_torque = self.P_quat.dot(
angle_rotation *
torque_direction) + self.I_quat.dot(error_integral)
def get_torque(self):
return self.rw_torque_b
def log_value(self):
return
def get_log_values(self):
return
class FineSunSensor(ComponentBase):
def __init__(self, port_id, properties, dynamics):
ComponentBase.__init__(self, 10)
self.dynamics = dynamics
self.port_id = port_id
self.pos_vector = properties['pos_vector']
self.normal_vector = properties['normal_vector']
self.q_b2c = Quaternions(properties['q_b2c'])
self.q_c2b = Quaternions(self.q_b2c.conjugate())
self.calib_h = properties['h']
self.calib_x0 = properties['x0']
self.calib_y0 = properties['y0']
self.calib_delta = np.deg2rad(properties['delta'])
self.xyd_nr_std = properties['nr_stddev_c']
self.scidriver = SCIDriver()
self.xyd_nrs_c = NormalRandom([self.xyd_nr_std, 0, 0])
self.scidriver.connect_port(self.port_id, 0, 0)
self.current_sun_vector_b = np.zeros(3)
self.current_sun_vector_c = np.zeros(3)
self.historical_sun_vector_b = []
self.historical_sun_vector_c = []
self.historical_V_ratio_m = []
self.historical_rd_m = []
self.historical_theta_m = []
self.historical_phi_m = []
self.cos_theta = 0
self.theta_fss = 0
self.phi_fss = 0
self.T = np.array(
[[np.cos(self.calib_delta),
np.sin(self.calib_delta)],
[-np.sin(self.calib_delta),
np.cos(self.calib_delta)]])
self.Tinv = np.array(
[[np.cos(self.calib_delta), -np.sin(self.calib_delta)],
[np.sin(self.calib_delta),
np.cos(self.calib_delta)]])
self.calib_params = [
self.calib_h, self.calib_x0, self.calib_y0, self.T, self.q_c2b
]
self.V0_ratio = np.array([self.calib_x0, self.calib_y0])
self.V_ratio_m = np.zeros(2)
self.rfss_c = np.zeros(3)
self.rd_c = np.zeros(2)
self.condition_ = False
self.albeo_correction = False
def main_routine(self, count, sc_isDark):
self.measure(sc_isDark)
def set_port_id(self, port):
self.port_id = port
def measure(self, sc_isDark):
isDark = sc_isDark
self.V_ratio_m = np.array([0, 0])
if isDark:
self.V_ratio_m = np.array([0, 0])
else:
self.calc_sun_vector_b()
return
def calc_sun_vector_b(self):
sun_vector_from_c_b = self.dynamics.ephemeris.selected_body[
'SUN'].get_pos_from_sc_b() - self.pos_vector
sun_unit_vector_from_c_b = sun_vector_from_c_b / np.linalg.norm(
sun_vector_from_c_b)
self.rfss_c = self.q_b2c.frame_conv(sun_unit_vector_from_c_b, "q")
# Corroborates that the sensor reaches it light and is not obstructed by the satellite.
self.calc_ang_parameters(sun_unit_vector_from_c_b, self.rfss_c)
if self.condition_:
if self.theta_fss < np.pi / 3: # Field of view (datasheet pag.8)
xd = np.sign(self.rfss_c[2]) * self.calib_h * np.tan(
self.theta_fss) * np.sin(self.phi_fss)
yd = self.calib_h * np.tan(self.theta_fss) * np.cos(
self.phi_fss)
Vratio = self.Tinv.dot(np.array([xd, yd])) - self.V0_ratio
self.V_ratio_m = Vratio + self.xyd_nrs_c()[0:2]
self.rd_c = np.array([xd, yd])
else:
self.V_ratio_m = np.array([0, 0])
def calc_ang_parameters(self, input_b_norm, rfss):
self.cos_theta = np.inner(self.normal_vector, input_b_norm)
self.condition_ = self.cos_theta > 0.0
self.theta_fss = np.arccos(rfss[0])
if self.theta_fss != 0:
self.phi_fss = np.arccos(rfss[1] / np.sin(self.theta_fss))
def get_vector_sun_b(self):
return self.current_sun_vector_b
def get_normal_vector_b(self):
return self.normal_vector
def get_V_ratio_b(self):
return self.V_ratio_m
def get_log_values(self, subsys):
report = {'V_ratio_m': np.array(self.historical_V_ratio_m)}
return report
def log_value(self):
self.historical_sun_vector_b.append(self.current_sun_vector_b)
self.historical_V_ratio_m.append(self.V_ratio_m)
self.historical_sun_vector_c.append(self.rfss_c)
self.historical_rd_m.append(self.rd_c)
self.historical_theta_m.append(self.theta_fss)
self.historical_phi_m.append(self.phi_fss)
class ADCS(ComponentBase):
def __init__(self, init_componenets, subsystem_setting, dynamics):
# prescalar_time in [ms]
ComponentBase.__init__(
self,
prescalar_time=(1000 / subsystem_setting['ADCS_com_frequency']))
# component quaternion
self.q_b2c = subsystem_setting['q_b2c']
# cycle in [ms]
self.ctrl_cycle = 1000 / subsystem_setting['ADCS_com_frequency']
self.port_id = subsystem_setting['port_id']
self.comp_number = subsystem_setting['ADCS_COMPONENT_NUMBER']
self.dynamics = dynamics
self.components = Components(init_componenets, self.dynamics,
self.port_id)
self.current_omega_c_gyro = np.zeros(3)
self.torque_rw_b = np.zeros(3)
self.omega_b_est = np.zeros(3)
self.control_torque = np.zeros(3)
self.historical_control = []
self.P_omega = subsystem_setting['P_omega']
self.I_quat = subsystem_setting['I_quat']
self.P_quat = subsystem_setting['P_quat']
self.rw_torque_b = np.zeros(3)
self.q_i2b_est = Quaternions([0, 0, 0, 1])
self.q_b2b_now2tar = Quaternions([0, 0, 0, 1])
self.q_i2b_tar = Quaternions([0, 0, 0, 1])
self.adcs_mode = DETUMBLING
for rw in self.components.rwmodel:
rw.set_step_width(self.ctrl_cycle / 1000)
self.controller = Controller().pid(self.P_quat, self.I_quat,
self.P_omega,
self.ctrl_cycle / 1000)
def main_routine(self, count):
self.read_sensors()
self.check_mode()
self.determine_attitude()
self.calculate_control_torque()
self.calc_rw_torque()
return
def read_sensors(self):
self.current_omega_c_gyro = self.components.gyro.measure(
self.dynamics.attitude.current_omega_b)
def determine_attitude(self):
self.omega_b_est = self.current_omega_c_gyro
att = self.dynamics.attitude.get_current_q_i2b()
self.q_i2b_est.setquaternion(att)
return
def check_mode(self):
if self.adcs_mode == DETUMBLING:
self.omega_b_tar = np.array([0.0, 0.0, 0.0])
self.controller.set_gain(self.P_omega, self.I_quat,
np.diag([0.0, 0.0, 0.0]))
elif self.adcs_mode == NAD_POINT:
print('Nadir pointing mode...')
elif self.adcs_mode == REF_POINT:
# Vector direction of the Body frame to point to another vector
b_dir = np.array([0, 0, 1])
# Vector target from Inertial frame
i_tar = np.array([1, 1, 1])
i_tar = i_tar / np.linalg.norm(i_tar)
# Vector target from body frame
b_tar = self.q_i2b_est.frame_conv(i_tar)
b_tar /= np.linalg.norm(b_tar)
b_lambda = np.cross(b_dir, b_tar)
b_lambda /= np.linalg.norm(b_lambda)
rot = np.arccos(np.dot(b_dir, b_tar))
self.q_b2b_now2tar.setquaternion([b_lambda, rot])
self.q_b2b_now2tar.normalize()
self.q_i2b_tar = self.q_i2b_est * self.q_b2b_now2tar
else:
print('No mode selected')
def calculate_control_torque(self):
q_b2i_est = Quaternions(self.q_i2b_est.conjugate())
# First it is necessary to pass the quaternion from attitude to inertial,
# then the target vector is rotated from the inertial to body frame
q_i2b_now2tar = q_b2i_est * self.q_i2b_tar
q_i2b_now2tar.normalize()
torque_direction = np.zeros(3)
torque_direction[0] = q_i2b_now2tar()[0]
torque_direction[1] = q_i2b_now2tar()[1]
torque_direction[2] = q_i2b_now2tar()[2]
angle_rotation = 2 * np.arccos(q_i2b_now2tar()[3])
error_omega_ = self.omega_b_tar - self.omega_b_est
error_ = angle_rotation * torque_direction
control = self.controller.calc_control(error_, error_omega_,
self.adcs_mode)
self.control_torque = control
def calc_rw_torque(self):
f = 0
for rw in self.components.rwmodel:
rw.control_power()
rw.set_torque(self.control_torque[f], self.ctrl_cycle)
self.rw_torque_b += rw.calc_torque(self.ctrl_cycle)
f += 1
return
def get_rwtorque(self):
return self.rw_torque_b
def save_data(self):
self.historical_control.append(self.control_torque)
def get_log_values(self, subsys):
report = {
'RWModel_' + subsys + '_b(X)[Nm]': 0,
'RWModel_' + subsys + '_b(Y)[Nm]': 0,
'RWModel_' + subsys + '_b(Z)[Nm]': 0
}
if hasattr(self.components, 'gyro'):
gyro = self.components.gyro
report['gyro_omega_' + subsys + '_c(X)[rad/s]'] = np.array(
gyro.historical_omega_c)[:, 0]
report['gyro_omega_' + subsys + '_c(Y)[rad/s]'] = np.array(
gyro.historical_omega_c)[:, 1]
report['gyro_omega_' + subsys + '_c(Z)[rad/s]'] = np.array(
gyro.historical_omega_c)[:, 2]
if hasattr(self.components, 'rwmodel'):
for rw in self.components.rwmodel:
report['RWModel_' + subsys + '_b(X)[Nm]'] += np.array(
rw.historical_rw_torque_b)[:, 0]
report['RWModel_' + subsys + '_b(Y)[Nm]'] += np.array(
rw.historical_rw_torque_b)[:, 1]
report['RWModel_' + subsys + '_b(Z)[Nm]'] += np.array(
rw.historical_rw_torque_b)[:, 2]
report_control = {
'Control_(X)[Nm]': np.array(self.historical_control)[:, 0],
'Control_(Y)[Nm]': np.array(self.historical_control)[:, 1],
'Control_(Z)[Nm]': np.array(self.historical_control)[:, 2]
}
report = {**report, **report_control}
return report
class ODCS(ComponentBase):
def __init__(self, init_componenets, subsystem_setting, dynamics):
ComponentBase.__init__(
self, (1000 / subsystem_setting['ODCS_com_frequency']))
self.dynamics = dynamics
self.ctrl_cycle = 1000 / subsystem_setting['ODCS_com_frequency']
self.port_id = subsystem_setting['port_id']
self.components = Components(init_componenets, self.dynamics,
self.port_id)
self.current_omega_c_gyro = np.zeros(3)
self.current_position_i = np.zeros(3)
self.current_velocity_i = np.zeros(3)
self.force_thruster_i = np.array([0.0, 0.0, 0.0])
self.force_thruster_b = np.array([0.0, 0.0, 0.0])
self.torque_thruster_b = np.zeros(3)
self.target_position = np.zeros(3)
self.target_velocity = np.zeros(3)
self.q_i2b_est = Quaternions([0, 0, 0, 1])
self.q_b2b_now2tar = Quaternions([0, 0, 0, 1])
self.q_i2b_tar = Quaternions([0, 0, 0, 1])
self.ctrl_ang = np.pi / 4
self.mag_thrust = 0
self.control_torque_b = np.zeros(3)
self.control_force_i = np.zeros(3)
self.historical_control_torque_b = []
self.historical_control_force_i = []
self.historical_thruster_i = []
self.g = np.array([0, 0, -1.62])
self.sliding = SlidingControl(self.g, self.dynamics.attitude.Inertia,
self.dynamics.attitude.inv_Inertia,
self.dynamics.attitude.current_mass,
self.ctrl_cycle)
for thr in self.components.thruster:
thr.set_step_width(self.ctrl_cycle / 1000)
#thr.set_max_thrust(300)
#thr.set_burn_time(10)
def main_routine(self, count):
self.read_sensors()
self.determine_attitude()
self.determine_orbit()
self.check_mode()
self.calculate_control_force_torque()
self.calc_thruster_force_i()
return
def read_sensors(self):
self.current_omega_c_gyro = self.components.gyro.measure(
self.dynamics.attitude.current_omega_b)
self.current_position_i = self.dynamics.trajectory.current_position
self.current_velocity_i = self.dynamics.trajectory.current_velocity
def determine_attitude(self):
att = self.dynamics.attitude.get_current_q_i2b()
self.q_i2b_est.setquaternion(att)
return
def determine_orbit(self):
return
def check_mode(self):
# Vector direction of the Body frame to point to another vector
b_dir = np.array([0, 0, 1])
# Vector target from Inertial frame
i_tar = np.array([0, 0, 1])
i_tar = i_tar / np.linalg.norm(i_tar)
# Vector target from body frame
b_tar = self.q_i2b_est.frame_conv(i_tar)
b_lambda = np.cross(b_dir, b_tar)
rot = np.arcsin(
np.linalg.norm(b_lambda) /
(np.linalg.norm(b_tar) * np.linalg.norm(b_dir)))
self.q_b2b_now2tar.setquaternion([b_lambda, rot])
self.q_b2b_now2tar.normalize()
self.q_i2b_tar = self.q_i2b_est * self.q_b2b_now2tar
return
def calculate_control_force_torque(self):
q_b2i_est = Quaternions(self.q_i2b_est.conjugate())
# First it is necessary to pass the quaternion from attitude to inertial,
# then the target vector is rotated from the inertial to body frame
q_i2b_now2tar = q_b2i_est * self.q_i2b_tar
q_i2b_now2tar.normalize()
omega_b_tar = np.zeros(3)
error_omega = omega_b_tar - self.dynamics.attitude.current_omega_b
error_vel = self.dynamics.trajectory.current_velocity - np.zeros(3)
error_pos = self.dynamics.trajectory.current_position - np.zeros(3)
self.sliding.set_current_state(error_pos, error_vel, error_omega,
q_i2b_now2tar)
self.sliding.calc_force_torque()
self.control_force_i = self.sliding.get_force_i()
self.torque_thruster_b = self.sliding.get_torque_b()
return
def calc_thruster_force_b(self):
for thr in self.components.thruster:
thr.set_thrust_i(self.control_force_i /
len(self.components.thruster))
thr.calc_thrust_and_torque_b(self.q_i2b_est)
self.torque_thruster_b = self.control_force_i
def calc_thruster_force_i(self):
thruster_force_i = 0
for thr in self.components.thruster:
thr.set_thrust_i(self.control_force_i /
len(self.components.thruster))
thr.calc_thrust_and_torque_i(self.ctrl_cycle)
thruster_force_i += thr.get_force_i()
self.force_thruster_i = thruster_force_i
def get_force_b(self):
return self.force_thruster_b
def get_force_i(self):
return self.control_force_i
def get_torque_b(self):
return self.torque_thruster_b
def log_value(self):
self.historical_control_torque_b.append(self.control_torque_b)
self.historical_control_force_i.append(self.control_force_i)
self.historical_thruster_i.append(self.force_thruster_i)
return
def get_log_values(self):
report = {
'Control_X_i [N]': np.array(self.historical_control_force_i)[:, 0],
'Control_Y_i [N]': np.array(self.historical_control_force_i)[:, 1],
'Control_Z_i [N]': np.array(self.historical_control_force_i)[:, 2],
'Thruster_X_i [N]': np.array(self.historical_thruster_i)[:, 0],
'Thruster_Y_i [N]': np.array(self.historical_thruster_i)[:, 1],
'Thruster_Z_i [N]': np.array(self.historical_thruster_i)[:, 2]
}
return report
class ADCS(ComponentBase):
def __init__(self, init_componenets, subsystem_setting, dynamics):
# prescalar_time in [ms]
ComponentBase.__init__(
self,
prescalar_time=(1000 / subsystem_setting['ADCS_com_frequency']))
# component quaternion
self.q_b2c = subsystem_setting['q_b2c']
# cycle in [ms]
self.ctrl_cycle = 1000 / subsystem_setting['ADCS_com_frequency']
self.port_id = subsystem_setting['port_id']
self.comp_number = subsystem_setting['ADCS_COMPONENT_NUMBER']
self.dynamics = dynamics
self.components = Components(init_componenets, self.dynamics,
self.port_id)
self.current_omega_c_gyro = np.zeros(3)
self.current_magVect_c_magSensor = np.zeros(3)
self.magVect_i = np.zeros(3)
self.magVect_b = np.zeros(3)
self.sunPos_i = np.zeros(3)
self.sunDir_b = np.zeros(3)
self.sunPos_est_b = np.zeros(3)
self.torque_rw_b = np.zeros(3)
self.omega_b_est = np.zeros(3)
self.control_torque = np.zeros(3)
self.historical_control = []
self.historical_estimation = []
self.historical_P = []
self.historical_P_det = []
self.historical_P_det = []
self.historical_magVect_i = []
self.historical_ssEst_b = []
self.historical_fssEst_b = []
self.historical_fssQdr_d = []
self.historical_ssDir_b = []
self.historical_eskf_res = []
self.historical_eskf_obserrmag = []
self.historical_eskf_obserrcss = []
self.historical_eskf_bias = []
self.P_omega = subsystem_setting['P_omega']
self.I_quat = subsystem_setting['I_quat']
self.P_quat = subsystem_setting['P_quat']
self.rw_torque_b = np.zeros(3)
self.q_i2b_est = Quaternions([0, 0, 0, 1])
self.q_i2b_est_eskf_temp = Quaternions([0, 0, 0, 1])
self.q_b2b_now2tar = Quaternions([0, 0, 0, 1])
self.q_i2b_tar = Quaternions([0, 0, 0, 1])
self.eskf = ErrorStateKalmanFilter(
dim_x=7,
dim_dx=6,
dim_u=3,
dim_z=3,
inertia=self.dynamics.attitude.Inertia,
invInertia=self.dynamics.attitude.inv_Inertia)
self.jacobians = Jacobians()
self.number_ss = len(self.components.sunsensors)
self.number_fss = len(self.components.fss)
self.current_I_sunsensors = np.zeros(self.number_ss)
self.V_ratios_fss = np.zeros((self.number_fss, 2))
self.params_fss = [None] * self.number_fss
self.rsfss_b = np.zeros((self.number_fss, 3))
self.qdrsfss_b = np.zeros((self.number_fss, 2))
self.tick_temp = 1
self.adcs_mode = DETUMBLING
self.components.mtt.set_step_width(self.ctrl_cycle / 1000)
for rw in self.components.rwmodel:
rw.set_step_width(self.ctrl_cycle / 1000)
# self.controller = Controller().pid(self.P_quat, self.I_quat, self.P_omega, self.ctrl_cycle/1000)
control_parameters = {'N_pred_horiz': 3}
self.controller = Controller().mpc(self.dynamics, control_parameters,
self.ctrl_cycle)
def main_routine(self, count, sc_isDark):
self.read_sensors(sc_isDark)
self.check_mode()
self.determine_attitude()
self.calculate_control_torque()
# self.calc_rw_torque()
# self.calc_mtt_torque()
return
def read_sensors(self, sc_isDark):
self.current_omega_c_gyro = self.components.gyro.measure(
self.dynamics.attitude.current_omega_b)
self.current_magVect_c_magSensor = self.components.mag.measure(
self.magVect_b)
for i in range(self.number_ss):
self.components.sunsensors[i].measure(sc_isDark)
self.current_I_sunsensors[i] = self.components.sunsensors[
i].get_I_b()
for i in range(self.number_fss):
self.components.fss[i].measure(sc_isDark)
self.V_ratios_fss[i] = self.components.fss[i].get_V_ratio_b()
self.params_fss[i] = self.components.fss[i].calib_params
def determine_attitude(self):
# To do: Omega Estimate
self.omega_b_est = self.current_omega_c_gyro
# True/Simulated quaternion
att = self.dynamics.attitude.get_current_q_i2b()
self.q_i2b_est.setquaternion(att)
# Simulated Sun Vector preprocessing
self.sunPos_i = self.dynamics.ephemeris.selected_body[
'SUN'].get_pos_from_center_i()
self.sunDir_b = unitVector(
self.dynamics.ephemeris.selected_body['SUN'].get_pos_from_sc_b())
# Coarse Sun Sensor preprocessing (vector extraction from currents)
self.sunPos_est_b = self.eskf.get_ss_vect(
self.current_I_sunsensors, self.components.sunsensors[0].I_max)
# Fine Sun Sensor preprocessing (vector extraction from quadrature voltage)
for i in range(self.number_fss):
self.rsfss_b[i] = self.eskf.get_fss_vect(self.V_ratios_fss[i],
self.params_fss[i])
self.qdrsfss_b[i] = self.eskf.get_fss_qdvect(
self.V_ratios_fss[i], self.params_fss[i])
# Fine Sun Sensor average preprocessing (vector extraction from quadrature voltage)
self.rsfss_mean_b = self.eskf.get_fss_mean_vect(self.rsfss_b)
# Estimate Spacecraft Attitude quaternion
# self.eskf_process(self.ctrl_cycle/1000)
return
def check_mode(self):
if self.adcs_mode == DETUMBLING:
self.omega_b_tar = np.array([0.0, 0.0, 0.0])
#self.controller.set_gain(self.P_omega, self.I_quat, np.diag([0.0, 0.0, 0.0]))
elif self.adcs_mode == NAD_POINT:
print('Nadir pointing mode...')
elif self.adcs_mode == REF_POINT:
# Vector direction of the Body frame to point to another vector
b_dir = np.array([0, 0, 1])
# Vector target from Inertial frame
i_tar = np.array([1, 1, 1])
i_tar = i_tar / np.linalg.norm(i_tar)
# Vector target from body frame
b_tar = self.q_i2b_est.frame_conv(i_tar)
b_tar /= np.linalg.norm(b_tar)
b_lambda = np.cross(b_dir, b_tar)
b_lambda /= np.linalg.norm(b_lambda)
rot = np.arccos(np.dot(b_dir, b_tar))
self.q_b2b_now2tar.setquaternion([b_lambda, rot])
self.q_b2b_now2tar.normalize()
self.q_i2b_tar = self.q_i2b_est * self.q_b2b_now2tar
else:
print('No mode selected')
def calculate_control_torque(self):
q_b2i_est = Quaternions(self.q_i2b_est.conjugate())
# First it is necessary to pass the quaternion from attitude to inertial,
# then the target vector is rotated from the inertial to body frame
q_i2b_now2tar = q_b2i_est * self.q_i2b_tar
q_i2b_now2tar.normalize()
torque_direction = np.zeros(3)
torque_direction[0] = q_i2b_now2tar()[0]
torque_direction[1] = q_i2b_now2tar()[1]
torque_direction[2] = q_i2b_now2tar()[2]
angle_rotation = 2 * np.arccos(q_i2b_now2tar()[3])
error_omega_ = self.omega_b_tar - self.omega_b_est
error_ = angle_rotation * torque_direction
"""
self.controller.open_loop([self.dynamics.attitude.current_quaternion_i2b,
self.dynamics.attitude.current_omega_b,
self.control_torque],
self.dynamics.simtime.current_jd)
self.control_torque = np.zeros(3)
"""
self.control_torque = self.controller.closed_loop([
self.dynamics.attitude.current_quaternion_i2b,
self.dynamics.attitude.current_omega_b, self.control_torque
], self.dynamics.simtime.current_jd)
#control = self.controller.calc_control(error_, error_omega_, self.adcs_mode)
def calc_mtt_torque(self):
self.components.mtt.calc_torque(self.control_torque,
self.current_magVect_c_magSensor)
return
def calc_rw_torque(self):
f = 0
for rw in self.components.rwmodel:
rw.control_power()
rw.set_torque(self.control_torque[f], self.ctrl_cycle)
self.rw_torque_b += rw.calc_torque(self.ctrl_cycle)
f += 1
return
def get_rwtorque(self):
return self.rw_torque_b
def set_magVector(self, mag_i, mag_b):
self.magVect_i = mag_i
self.magVect_b = mag_b
def save_data(self):
self.historical_control.append(self.control_torque)
self.historical_magVect_i.append(self.magVect_i)
def get_log_values(self, subsys):
report = {
'MTT_' + subsys + '_b(X)[Am]': 0,
'MTT_' + subsys + '_b(Y)[Am]': 0,
'MTT_' + subsys + '_b(Z)[Am]': 0
}
report_control = {
'Control_' + subsys + '_b(X)[Nm]':
np.array(self.historical_control)[:, 0],
'Control_' + subsys + '_b(Y)[Nm]':
np.array(self.historical_control)[:, 1],
'Control_' + subsys + '_b(Z)[Nm]':
np.array(self.historical_control)[:, 2]
}
report = {**report, **report_control}
return report