def start(sql_path=DB_FILE,num_runs=1,gyro_count=4,gyro_vars:GyroVars=GyroVars(), camera_params:CameraParameters=CisLunarCameraParameters) -> OPNAV_EXIT_STATUS:
"""
Entry point into OpNav System.
[sql_path]: path to .sqlite file with the SQL prefix
[num_runs]: number of times observation step is run. Does not include propulsion events
[num_runs] corresponds to number of trajectory measurements, not attitude measurements
[gyro_count]: number of gyro measurements taken per observation step
[gyro_vars]: GyroVars object containing gyroscope properties
@return
opnav_exit_status: 0 (success), ...
"""
assert gyro_count > 1
create_session = create_sensor_tables_from_path(sql_path)
session = create_session()
assert len(session.query(OpNavTrajectoryStateModel).all()) >= 1 and len(session.query(OpNavAttitudeStateModel).all()) >= 1
propulsion_exit_status = __process_propulsion_events(session)
if propulsion_exit_status is not OPNAV_EXIT_STATUS.SUCCESS:
print(f'propulsion processing result: {propulsion_exit_status}')
# TODO: Handle propulsion processing failure
return propulsion_exit_status
for run in range(num_runs):
# process propagation step
__observe(session, gyro_count)
propagation_exit_status = __process_propagation(session, gyro_vars, camera_params)
# TODO: Handle exit status
if propagation_exit_status is not OPNAV_EXIT_STATUS.SUCCESS:
print(f'propagation processing result: {propagation_exit_status}')
return propagation_exit_status
return OPNAV_EXIT_STATUS.SUCCESS
def test_pressure_model():
create_session = create_sensor_tables_from_path(MEMORY_DB_PATH)
session = create_session()
pressure_measurements = session.query(PressureModel).all()
assert 0 == len(pressure_measurements)
measurement_taken = datetime.datetime.now()
pressure = 15.0
new_measurement = PressureModel(measurement_taken=measurement_taken,
pressure=pressure)
session.add(new_measurement)
session.commit()
pressure_measurements = session.query(PressureModel).all()
assert 1 == len(pressure_measurements)
last_measurement = pressure_measurements[0]
assert last_measurement.measurement_taken == measurement_taken
assert last_measurement.pressure == pressure
session.close()
session = create_session()
pressure_measurements = session.query(PressureModel).all()
assert 1 == len(pressure_measurements)
last_measurement = pressure_measurements[0]
assert last_measurement.measurement_taken == measurement_taken
assert last_measurement.pressure == pressure
def start():
"""
Confirms if estimates were computed successfully and if
they were added tp the appropriate databases.
Return:
opnav_exit_status: 0 (success), ...
"""
create_session = create_sensor_tables_from_path(DB_FILE)
session = create_session()
new_entry = OpNavCoordinatesModel(time_retrieved=datetime.now(),
velocity_x=0.,
velocity_y=0.,
velocity_z=0.,
position_x=0.,
position_y=0.,
position_z=0.,
attitude_q1=0.,
attitude_q2=0.,
attitude_q3=0.,
attitude_q4=0.,
attitude_rod1=0.,
attitude_rod2=0.,
attitude_rod3=0.,
attitude_b1=0.,
attitude_b2=0.,
attitude_b3=0.)
session.add(new_entry)
session.commit()
# Check db entries
entries = session.query(OpNavCoordinatesModel).all()
for entry in entries:
print(entry)
return 0
def __init__(self):
super().__init__()
logger.info("Initializing...")
self.init_parameters()
self.command_queue = Queue()
self.downlink_queue = Queue()
self.FMQueue = Queue()
self.commands_to_execute = []
self.downlinks_to_execute = []
self.telemetry = Telemetry(self)
self.burn_queue = Queue()
self.reorientation_queue = Queue()
self.reorientation_list = []
self.maneuver_queue = Queue() # maneuver queue
# self.init_comms()
self.command_handler = CommandHandler()
self.downlink_handler = DownlinkHandler()
self.command_counter = 0
self.downlink_counter = 0
self.command_definitions = CommandDefinitions(self)
self.init_sensors()
self.last_opnav_run = datetime.now() # Figure out what to set to for first opnav run
self.log_dir = LOG_DIR
self.logger = get_log()
self.attach_sigint_handler() # FIXME
if os.path.isdir(self.log_dir):
self.flight_mode = RestartMode(self)
else:
os.makedirs(CISLUNAR_BASE_DIR)
os.mkdir(LOG_DIR)
self.flight_mode = BootUpMode(self)
self.create_session = create_sensor_tables_from_path(DB_FILE)
def __init__(self, parent):
super().__init__(parent)
logger.info("Restarting...")
create_session = create_sensor_tables_from_path(DB_FILE)
self.session = create_session()
self.log()
def __init__(self, parent):
super().__init__(parent)
logger.info("Restarting...")
logger.info("Creating DB session...")
create_session = create_sensor_tables_from_path(DB_FILE)
self.session = create_session()
# add info about restart to database
logger.debug("Logging to DB...")
self.log()
def __init__(self):
super().__init__()
logger.info("Initializing...")
self.init_parameters()
self.command_queue = Queue()
self.downlink_queue = Queue()
self.FMQueue = Queue()
self.commands_to_execute = []
self.downlinks_to_execute = []
self.burn_queue = Queue()
self.reorientation_queue = Queue()
self.reorientation_list = []
self.maneuver_queue = Queue() # maneuver queue
self.opnav_queue = Queue() # determine state of opnav success
# self.init_comms()
logger.info("Initializing commands and downlinks")
self.command_handler = CommandHandler()
self.downlink_handler = DownlinkHandler()
self.command_counter = 0
self.downlink_counter = 0
self.command_definitions = CommandDefinitions(self)
self.last_opnav_run = datetime.now() # Figure out what to set to for first opnav run
self.log_dir = LOG_DIR
self.logger = get_log()
self.attach_sigint_handler() # FIXME
self.file_block_bank = {}
self.need_to_reboot = False
self.gom: Gomspace
self.gyro: GyroSensor
self.adc: ADC
self.rtc: RTC
self.radio: Radio
self.mux: camera.CameraMux
self.camera: camera.Camera
self.nemo_manager: NemoManager
self.init_sensors()
# Opnav subprocess variables
self.opnav_proc_queue = Queue()
self.opnav_process = Process() # define the subprocess
if os.path.isdir(self.log_dir):
logger.info("We are in Restart Mode")
self.flight_mode = RestartMode(self)
else:
logger.info("We are in Bootup Mode")
os.makedirs(CISLUNAR_BASE_DIR, exist_ok=True)
os.mkdir(LOG_DIR)
self.flight_mode = BootUpMode(self)
self.create_session = create_sensor_tables_from_path(DB_FILE)
logger.info("Initializing Telemetry")
self.telemetry = Telemetry(self)
logger.info("Done intializing")
def __init__(self):
super().__init__()
logger.info("Initializing...")
self.init_parameters()
self.command_queue = Queue()
self.downlink_queue = Queue()
self.FMQueue = Queue()
self.commands_to_execute = []
self.downlinks_to_execute = []
self.telemetry = Telemetry(self)
self.burn_queue = Queue()
self.reorientation_queue = Queue()
self.reorientation_list = []
self.maneuver_queue = Queue() # maneuver queue
self.opnav_queue = Queue() # determine state of opnav success
# self.init_comms()
self.command_handler = CommandHandler()
self.downlink_handler = DownlinkHandler()
self.command_counter = 0
self.downlink_counter = 0
self.command_definitions = CommandDefinitions(self)
self.last_opnav_run = datetime.now() # Figure out what to set to for first opnav run
self.log_dir = LOG_DIR
self.logger = get_log()
self.attach_sigint_handler() # FIXME
self.need_to_reboot = False
self.gom = None
self.gyro = None
self.adc = None
self.rtc = None
self.radio = None
self.mux = None
self.camera = None
self.init_sensors()
# Telemetry
self.tlm = Telemetry(self)
# Opnav subprocess variables
self.opnav_proc_queue = Queue()
self.opnav_process = Process() # define the subprocess
if os.path.isdir(self.log_dir):
self.flight_mode = RestartMode(self)
else:
os.makedirs(CISLUNAR_BASE_DIR, exist_ok=True)
os.mkdir(LOG_DIR)
self.flight_mode = BootUpMode(self)
self.create_session = create_sensor_tables_from_path(DB_FILE)
def __init__(self, parent):
# The purpose of the parent object is to ensure that only one object is defined for each sensor/component
# So almost all calls to sensors will be made through self._parent.<insert sensor stuff here>
super().__init__(parent)
self.gom = GomSensor(parent)
self.gyr = GyroSensor(parent)
self.prs = PressureSensor(parent)
self.thm = ThermocoupleSensor(parent)
self.rpi = PiSensor(parent)
self.rtc = RtcSensor(parent)
self.opn = OpNavSensor(parent)
self.sensors = [self.gom, self.gyr, self.prs, self.thm, self.rpi, self.rtc]
create_session = create_sensor_tables_from_path(DB_FILE) # instantiate DB session
self.session = create_session() # instantiate DB session
def run_mode(self):
logger.info("Boot up beginning...")
time.sleep(BOOTUP_SEPARATION_DELAY)
create_session = create_sensor_tables_from_path(DB_FILE)
self.session = create_session()
self.log()
# deploy antennae
# FIXME: differentiate between Hydrogen and Oxygen. Each satellite now has different required Bootup behaviors
logger.info("Antennae deploy...")
self.parent.gom.burnwire1(5)
if self.parent.need_to_reboot:
# TODO: double check the boot db history to make sure we aren't going into a boot loop
# TODO: downlink something to let ground station know we're alive
os.system("sudo reboot")
def run_mode(self):
logger.info("Boot up beginning...")
logger.info("Time when sleep starts: " + str(datetime.now()))
time.sleep(BOOTUP_SEPARATION_DELAY)
logger.info("Time when sleep stops: " + str(datetime.now()))
logger.info("Creating DB session...")
create_session = create_sensor_tables_from_path(DB_FILE)
self.session = create_session()
logger.info("Logging info to DB...")
self.log()
# TODO deploy antennae
# logger.info("Beginning burn wire...")
# parent.gom.burnwire1(5)
logger.info("Transferring to RestartMode via sudo reboot")
os.system("sudo reboot")
def setup_function(function):
# Reset databases
create_session = create_sensor_tables_from_path(sql_path)
session = create_session()
session.query(OpNavTrajectoryStateModel).delete()
assert len(session.query(OpNavTrajectoryStateModel).all()) == 0
session.query(OpNavAttitudeStateModel).delete()
assert len(session.query(OpNavAttitudeStateModel).all()) == 0
session.query(OpNavEphemerisModel).delete()
assert len(session.query(OpNavEphemerisModel).all()) == 0
session.query(OpNavCameraMeasurementModel).delete()
assert len(session.query(OpNavCameraMeasurementModel).all()) == 0
session.query(OpNavPropulsionModel).delete()
assert len(session.query(OpNavPropulsionModel).all()) == 0
session.query(OpNavGyroMeasurementModel).delete()
assert len(session.query(OpNavGyroMeasurementModel).all()) == 0
new_bootup = RebootsModel(is_bootup=False, reboot_at=datetime.now())
session.add(new_bootup)
session.commit()
def test_telemetry_model():
create_session = create_sensor_tables_from_path(MEMORY_DB_PATH)
session = create_session()
telemetry_measurements = session.query(TelemetryModel).all()
assert 0 == len(telemetry_measurements), "FAIL"
print("Creation of empty session -- PASS")
print()
make_entry(session)
telemetry_measurements = session.query(TelemetryModel).all()
assert 1 == len(telemetry_measurements), "FAIL"
make_entry(session)
make_entry(session)
telemetry_measurements = session.query(TelemetryModel).all()
assert 3 == len(telemetry_measurements), "FAIL"
print("Creation of 3 entries -- PASS")
print()
print("displaying all TelemetryModel entries:")
db_functions.display_model("Telemetry", session)
print()
print("display_model -- PASS")
print()
print(
"displaying two (out of three) most recent entries in TelemetryModel:")
db_functions.display_model_amount("Telemetry", 2, session)
print()
print("display_model_amount -- PASS")
print()
print("displaying most recent RPI entry:")
db_functions.telemetry_query("RPI", 1, session)
print()
print("displaying 3 most recent GYRO entries:")
db_functions.telemetry_query("GYRO", 3, session)
print()
print("telemetry_query -- PASS")
print()
print("... making another entry to TelemetryModel...")
print("currently 4 entries")
print()
make_entry(session)
telemetry_measurements = session.query(TelemetryModel).all()
assert 4 == len(telemetry_measurements), "FAIL"
print("cleaning TelemetryModel with entry_limit=2 \nonly two entries now:")
db_functions.clean("Telemetry", session, 2)
telemetry_measurements = session.query(TelemetryModel).all()
assert 2 == len(telemetry_measurements), "FAIL"
db_functions.display_model("Telemetry", session)
print("length of TelemetryModel:", len(telemetry_measurements))
print()
print("clean -- PASS")
print()
print("wiping TelemetryModel of all entries")
print("length before wiping:", len(telemetry_measurements))
db_functions.wipe("Telemetry", session)
telemetry_measurements = session.query(TelemetryModel).all()
assert 0 == len(telemetry_measurements), "FAIL"
db_functions.display_model("Telemetry", session)
print("length after wiping:", len(telemetry_measurements))
print()
print("ALL TESTS PASS")
def test_start(mocker):
# Add data to test database
create_session = create_sensor_tables_from_path(sql_path)
session = create_session()
# entries = session.query(OpNavPropulsionModel).all()
time = datetime.now()
entries = [
OpNavPropulsionModel.from_tuples(
0.0001, time_start=time+timedelta(0,-110), time_end=time+timedelta(0,-100)), # 10 second propulsion that occured 100 seconds ago
OpNavTrajectoryStateModel.from_tuples(
(0.,2.,3.), (2.,2.,3.), opnav_constants.TrajUKFConstants.P0.reshape(6,6), time=time+timedelta(0,-200)), # initial state used by propulsion step, output used by propagation step
OpNavAttitudeStateModel.from_tuples(
(0., 0., 0., 1.), (0., 1., 0.), (0.,2.,0.,), opnav_constants.AttitudeUKFConstants.P0.reshape(6,6), time=time+timedelta(0,-200)),
OpNavEphemerisModel.from_tuples(
sun_eph=(1111., 1111., 1111., 124., 24., 4.), moon_eph=(4244., 42424., 4244., 42., 42., 42.), time=time+timedelta(0,20)),
OpNavEphemerisModel.from_tuples(
sun_eph=(2222., 2222., 2222., 124., 24., 4.), moon_eph=(4244., 42424., 4244., 42., 42., 42.), time=time+timedelta(0,-3)),
OpNavEphemerisModel.from_tuples(
sun_eph=(3333., 3333., 3333., 124., 24., 4.), moon_eph=(4244., 42424., 4244., 42., 42., 42.), time=time+timedelta(1,0)),
#OpNavCameraMeasurementModel.from_tuples(
# measurement=(0.2, 0.1, 0.2, 0.1, 0.11, 0.22), time=time), # camera measurement taken now, uses state
OpNavGyroMeasurementModel.from_tuples(
measurement=(0., 2., 1.), time=time+timedelta(0,-50)), # Not picked because gyro was read before this run's corresponding cam meas
OpNavGyroMeasurementModel.from_tuples(
measurement=(2., 1., 1.), time=time+timedelta(0,0.1)),
OpNavGyroMeasurementModel.from_tuples(
measurement=(2., 1., 1.), time=time+timedelta(0,0.21234)),
OpNavGyroMeasurementModel.from_tuples(
measurement=(2., 1., 1.), time=time+timedelta(0,0.31244)),
]
session.bulk_save_objects(entries)
session.commit()
# mocks
# do not test observe()
def observe_mock(session, gyro_count):
return opnav_constants.OPNAV_EXIT_STATUS.SUCCESS
# mocker.patch('core.opnav.__observe', side_effect=observe_mock)
def select_camera_mock(id):
print("select_mock")
return
# Don't use select_camera_mock for software demo
#mocker.patch('core.opnav.select_camera', side_effect=select_camera_mock)
idx = [0]
timestamps = [981750, 981750, 2028950, 2028950, 3010700, 3010700]
videos = ['/home/stephen_z/Desktop/test-videos/cam1_expLow.mjpeg', '/home/stephen_z/Desktop/test-videos/cam1_expHigh.mjpeg',
'/home/stephen_z/Desktop/test-videos/cam2_expLow.mjpeg', '/home/stephen_z/Desktop/test-videos/cam2_expHigh.mjpeg',
'/home/stephen_z/Desktop/test-videos/cam3_expLow.mjpeg', '/home/stephen_z/Desktop/test-videos/cam3_expHigh.mjpeg']
def record_video_mock(filename, framerate, recTime, exposure):
print("video_mock")
time = timestamps[idx[0]]
filename = videos[idx[0]]
idx[0] += 1
return (filename, time)
# Don't use record_video mock for software demo
#mocker.patch('core.opnav.record_video', side_effect=record_video_mock)
def record_gyro_mock(count):
print("gyro_mock")
return np.array([[0, 5, 0]])
mocker.patch('core.opnav.record_gyro', side_effect=record_gyro_mock)
# start opnav system
opnav.start(sql_path=sql_path, num_runs=1, gyro_count=2)
