def __init__(self, init_componenets, dynamics):
ComponentBase.__init__(self, 5)
self.dynamics = dynamics
self.components = Components(init_componenets, self.dynamics, 3)
self.current_omega_c_gyro = np.zeros(3)
self.force_thruster_b = np.array([0.0, 0.0, -0.00001])
self.torque_thruster_b = np.zeros(3)
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.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):
# 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, init_componenets, dynamics):
ComponentBase.__init__(self, 5)
self.dynamics = dynamics
self.components = Components(init_componenets, self.dynamics, 3)
self.current_voltage = 0
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)