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 __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 __init__(self, port_id, properties, dynamics):
ComponentBase.__init__(self, 1)
self.dynamics = dynamics
self.port_id = port_id
self.pos_vector = properties['pos_vector']
self.normal_vector = properties['normal_vector']
self.cosine_error = np.deg2rad(properties['cosine_error'])
self.I_nr_std = properties['nr_stddev_c']
self.I_max = properties['I_max']
self.scidriver = SCIDriver()
self.nrs_c = NormalRandom([self.cosine_error, 0, 0])
self.nrs_I = NormalRandom([self.I_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_I_measured = []
self.I_measured = 0
self.condition_ = False
self.albeo_correction = False
self.isDark = False
class SunSensor(ComponentBase):
def __init__(self, port_id, properties, dynamics):
ComponentBase.__init__(self, 1)
self.dynamics = dynamics
self.port_id = port_id
self.pos_vector = properties['pos_vector']
self.normal_vector = properties['normal_vector']
self.cosine_error = np.deg2rad(properties['cosine_error'])
self.I_nr_std = properties['nr_stddev_c']
self.I_max = properties['I_max']
self.scidriver = SCIDriver()
self.nrs_c = NormalRandom([self.cosine_error, 0, 0])
self.nrs_I = NormalRandom([self.I_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_I_measured = []
self.I_measured = 0
self.condition_ = False
self.albeo_correction = False
self.isDark = 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.I_measured = 0
if isDark:
self.I_measured = 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)
# Corroborates that the sensor reaches it light and is not obstructed by the satellite.
self.calc_ang_parameters(sun_unit_vector_from_c_b)
if self.condition_:
theta = np.arccos(self.cos_theta) + self.nrs_c()[0]
# El valor absoluto evita que el ruido gaussiano de un coseno de theta bajo cero cuando el coseno de theta
# original es cercano o igual a cero.
cos_theta = np.abs(np.cos(theta))
self.I_measured = self.I_max * cos_theta + self.nrs_I()[0]
if self.I_measured < 0:
self.I_measured = 0
def calc_ang_parameters(self, input_b_norm):
self.cos_theta = np.inner(self.normal_vector, input_b_norm)
self.condition_ = self.cos_theta > 0.0
def get_vector_sun_b(self):
return self.current_sun_vector_b
def get_I_normal_vector_b(self):
return [self.I_measured, self.normal_vector]
def get_I_b(self):
return self.I_measured
def get_log_values(self, subsys):
report = {'I_measured': np.array(self.historical_I_measured)}
return report
def log_value(self):
self.historical_sun_vector_b.append(self.current_sun_vector_b)
self.historical_I_measured.append(self.I_measured)
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)
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 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)