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 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 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 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)