From c808a55366884c7b9913aaa76d5778d704f2d038 Mon Sep 17 00:00:00 2001 From: thetazero Date: Wed, 3 Aug 2022 15:42:22 -0400 Subject: [PATCH 01/14] Comment --- state_machine/drivers/pycubedmini/lib/pycubed.py | 1 + 1 file changed, 1 insertion(+) diff --git a/state_machine/drivers/pycubedmini/lib/pycubed.py b/state_machine/drivers/pycubedmini/lib/pycubed.py index 27ef3e2c..3373e26f 100644 --- a/state_machine/drivers/pycubedmini/lib/pycubed.py +++ b/state_machine/drivers/pycubedmini/lib/pycubed.py @@ -239,6 +239,7 @@ class _Satellite: c_logfail = multiBitFlag(register=_LOGFAIL, lowest_bit=0, num_bits=8) # Set hardware attributes to None + # Needed for things not to crash _i2c1, _i2c2, _i2c3, _spi, _sd, _neopixel = None, None, None, None, None, None _imu, _radio, _sun_yn, _sun_zn, _sun_xn = None, None, None, None, None, None _sun_yp, _sun_zp, _sun_xp, _drv_x, _drv_y = None, None, None, None, None From b733ce9b0ec5abaf9f86155414db337e0c04d230 Mon Sep 17 00:00:00 2001 From: thetazero Date: Wed, 3 Aug 2022 15:43:38 -0400 Subject: [PATCH 02/14] New default value --- state_machine/drivers/emulation/lib/pycubed.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/state_machine/drivers/emulation/lib/pycubed.py b/state_machine/drivers/emulation/lib/pycubed.py index c9370e73..5581e513 100644 --- a/state_machine/drivers/emulation/lib/pycubed.py +++ b/state_machine/drivers/emulation/lib/pycubed.py @@ -55,7 +55,7 @@ def __init__(self): # to provide more interesting output from the b-cross controller. self._accel = [1.0, 2.0, 3.0] self._mag = [4.0, 3.0, 1.0] - self._gyro = [0.0, 0.0, 0.0] + self._gyro = [3.0, 5.0, -0.3] self._torque = [0, 0, 0] self.sim = False From e6e3cc43b9691451ba7ecb2798021845475d8450 Mon Sep 17 00:00:00 2001 From: thetazero Date: Wed, 3 Aug 2022 15:43:56 -0400 Subject: [PATCH 03/14] GNC test --- state_machine/applications/flight/Tasks/gnc.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/state_machine/applications/flight/Tasks/gnc.py b/state_machine/applications/flight/Tasks/gnc.py index b1ba13e7..51c47814 100644 --- a/state_machine/applications/flight/Tasks/gnc.py +++ b/state_machine/applications/flight/Tasks/gnc.py @@ -21,7 +21,7 @@ class task(Task): rgb_on = False last = None - r_eci = array([6871, -6571, -7071]) + eci_state = array([6871, -6571, -7071, 2, -10, 3]) async def main_task(self): if self.last is None: @@ -35,12 +35,12 @@ async def main_task(self): w = cubesat.gyro() br_mag = cubesat.magnetic() br_sun = cubesat.sun_vector() - nr_mag = igrf_eci(t, self.r_eci) + nr_mag = igrf_eci(t, self.eci_state[0:3]) nr_sun = approx_sun_position_ECI(t) mekf.step(w, delta_t, nr_mag, nr_sun, br_mag, br_sun) # propogate ECI position - self.r_eci = orbital_mechanics.propogate(self.r_eci, delta_t, integration_step=5) + self.eci_state = orbital_mechanics.propogate(self.eci_state, delta_t, integration_step=5) # compute control m = bcross(cubesat.magnetic(), cubesat.gyro()) From 749474c544e4c6059d39834a5597d1ea418ee7db Mon Sep 17 00:00:00 2001 From: thetazero Date: Wed, 3 Aug 2022 15:44:37 -0400 Subject: [PATCH 04/14] document eci_state --- state_machine/applications/flight/Tasks/gnc.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/state_machine/applications/flight/Tasks/gnc.py b/state_machine/applications/flight/Tasks/gnc.py index 51c47814..39212082 100644 --- a/state_machine/applications/flight/Tasks/gnc.py +++ b/state_machine/applications/flight/Tasks/gnc.py @@ -21,7 +21,7 @@ class task(Task): rgb_on = False last = None - eci_state = array([6871, -6571, -7071, 2, -10, 3]) + eci_state = array([6871, -6571, -7071, 2, -10, 3]) # [x, y, z, vx, vy, vz] async def main_task(self): if self.last is None: From 04e36042c128927fa413a0e403b001c3313ca6a7 Mon Sep 17 00:00:00 2001 From: thetazero Date: Wed, 3 Aug 2022 15:45:28 -0400 Subject: [PATCH 05/14] Update None's --- state_machine/drivers/pycubedmini/lib/pycubed.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/state_machine/drivers/pycubedmini/lib/pycubed.py b/state_machine/drivers/pycubedmini/lib/pycubed.py index 308addd2..102f1fe9 100644 --- a/state_machine/drivers/pycubedmini/lib/pycubed.py +++ b/state_machine/drivers/pycubedmini/lib/pycubed.py @@ -241,7 +241,7 @@ class _Satellite: # Set hardware attributes to None # Needed for things not to crash _i2c1, _i2c2, _i2c3, _spi, _sd, _neopixel = None, None, None, None, None, None - _imu, _radio, _sun_yn, _sun_zn, _sun_xn = None, None, None, None, None, None + _imu, _radio, _sun_yn, _sun_zn, _sun_xn = None, None, None, None, None _sun_yp, _sun_zp, _sun_xp, _drv_x, _drv_y = None, None, None, None, None _drv_z, _burnwire1, _burnwire2 = None, None, None From 911e1e321e8cd44ad97c2127c9c53c38a185052c Mon Sep 17 00:00:00 2001 From: thetazero Date: Wed, 3 Aug 2022 15:45:42 -0400 Subject: [PATCH 06/14] Lint --- state_machine/drivers/pycubedmini/lib/pycubed.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/state_machine/drivers/pycubedmini/lib/pycubed.py b/state_machine/drivers/pycubedmini/lib/pycubed.py index 102f1fe9..33ced20b 100644 --- a/state_machine/drivers/pycubedmini/lib/pycubed.py +++ b/state_machine/drivers/pycubedmini/lib/pycubed.py @@ -241,7 +241,7 @@ class _Satellite: # Set hardware attributes to None # Needed for things not to crash _i2c1, _i2c2, _i2c3, _spi, _sd, _neopixel = None, None, None, None, None, None - _imu, _radio, _sun_yn, _sun_zn, _sun_xn = None, None, None, None, None + _imu, _radio, _sun_yn, _sun_zn, _sun_xn = None, None, None, None, None _sun_yp, _sun_zp, _sun_xp, _drv_x, _drv_y = None, None, None, None, None _drv_z, _burnwire1, _burnwire2 = None, None, None From b813209658dc8fce8823316c29f33c0ab40017ea Mon Sep 17 00:00:00 2001 From: thetazero Date: Wed, 3 Aug 2022 15:51:53 -0400 Subject: [PATCH 07/14] Don't crash on no sun sensors --- state_machine/applications/flight/Tasks/gnc.py | 9 ++++++++- state_machine/drivers/emulation/lib/pycubed.py | 7 +++++++ 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/state_machine/applications/flight/Tasks/gnc.py b/state_machine/applications/flight/Tasks/gnc.py index 39212082..a97a6dd9 100644 --- a/state_machine/applications/flight/Tasks/gnc.py +++ b/state_machine/applications/flight/Tasks/gnc.py @@ -22,6 +22,7 @@ class task(Task): rgb_on = False last = None eci_state = array([6871, -6571, -7071, 2, -10, 3]) # [x, y, z, vx, vy, vz] + sun_sensor_failed = False async def main_task(self): if self.last is None: @@ -34,7 +35,13 @@ async def main_task(self): # update mekf w = cubesat.gyro() br_mag = cubesat.magnetic() - br_sun = cubesat.sun_vector() + try: + br_sun = cubesat.sun_vector() + except cubesat.HardwareInitException as e: + if not self.sun_sensor_failed: + self.debug('Something went wrong trying to read from a sun sensor') + self.debug(f'Error: {e}') + self.sun_sensor_failed = True nr_mag = igrf_eci(t, self.eci_state[0:3]) nr_sun = approx_sun_position_ECI(t) mekf.step(w, delta_t, nr_mag, nr_sun, br_mag, br_sun) diff --git a/state_machine/drivers/emulation/lib/pycubed.py b/state_machine/drivers/emulation/lib/pycubed.py index 5581e513..6fe9100a 100644 --- a/state_machine/drivers/emulation/lib/pycubed.py +++ b/state_machine/drivers/emulation/lib/pycubed.py @@ -124,6 +124,13 @@ def sun_vector(): return array([0, 0, 0]) +""" +Define HardwareInitException +""" +class HardwareInitException(Exception): + pass + + """ Radio related functions """ From be928470724c9188434e062b09fa11958bdf7042 Mon Sep 17 00:00:00 2001 From: thetazero Date: Wed, 3 Aug 2022 16:09:38 -0400 Subject: [PATCH 08/14] Doesn't crash on mainboard --- state_machine/applications/flight/Tasks/gnc.py | 8 ++++++-- state_machine/applications/flight/lib/IGRF.py | 2 +- state_machine/applications/flight/lib/frames.py | 5 +++-- 3 files changed, 10 insertions(+), 5 deletions(-) diff --git a/state_machine/applications/flight/Tasks/gnc.py b/state_machine/applications/flight/Tasks/gnc.py index a97a6dd9..600c21d7 100644 --- a/state_machine/applications/flight/Tasks/gnc.py +++ b/state_machine/applications/flight/Tasks/gnc.py @@ -25,10 +25,11 @@ class task(Task): sun_sensor_failed = False async def main_task(self): + failed = False + # start calculating time steps if self.last is None: self.last = time.monotonic() return - t = time.monotonic() delta_t = t - self.last @@ -42,9 +43,12 @@ async def main_task(self): self.debug('Something went wrong trying to read from a sun sensor') self.debug(f'Error: {e}') self.sun_sensor_failed = True + failed = True nr_mag = igrf_eci(t, self.eci_state[0:3]) nr_sun = approx_sun_position_ECI(t) - mekf.step(w, delta_t, nr_mag, nr_sun, br_mag, br_sun) + + if not failed: + mekf.step(w, delta_t, nr_mag, nr_sun, br_mag, br_sun) # propogate ECI position self.eci_state = orbital_mechanics.propogate(self.eci_state, delta_t, integration_step=5) diff --git a/state_machine/applications/flight/lib/IGRF.py b/state_machine/applications/flight/lib/IGRF.py index fc1d531f..5144b749 100644 --- a/state_machine/applications/flight/lib/IGRF.py +++ b/state_machine/applications/flight/lib/IGRF.py @@ -3,7 +3,7 @@ # Fifth order approximatin. try: - import ulab as np + import ulab.numpy as np except ImportError: import numpy as np import math diff --git a/state_machine/applications/flight/lib/frames.py b/state_machine/applications/flight/lib/frames.py index 3416f114..fab02dd0 100644 --- a/state_machine/applications/flight/lib/frames.py +++ b/state_machine/applications/flight/lib/frames.py @@ -1,9 +1,10 @@ try: import ulab.numpy as np - from ulab.numpy import cos, sin, pi, arctan2, sqrt, array + from ulab.numpy import cos, sin, pi, sqrt, array except ImportError: import numpy as np - from numpy import cos, sin, pi, arctan2, sqrt, array + from numpy import cos, sin, pi, sqrt, array +from math import atan2 as arctan2 J2000 = 946684800 # unix timestamp for the Julian date 2000-01-01 MJD_ZERO = 2400000.5 # Offset of Modified Julian Days representation with respect to Julian Days. From 6fec0bad612fdba69f1c2b147d1096c79e3eb304 Mon Sep 17 00:00:00 2001 From: thetazero Date: Thu, 4 Aug 2022 09:15:00 -0400 Subject: [PATCH 09/14] Use ImportError --- state_machine/applications/flight/lib/control.py | 2 +- state_machine/applications/flight/lib/frames.py | 2 +- state_machine/applications/flight/lib/mathutils.py | 2 +- state_machine/applications/flight/lib/mekf.py | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/state_machine/applications/flight/lib/control.py b/state_machine/applications/flight/lib/control.py index afb924dc..83be26bc 100644 --- a/state_machine/applications/flight/lib/control.py +++ b/state_machine/applications/flight/lib/control.py @@ -1,6 +1,6 @@ try: from ulab.numpy import eye as identity, array, linalg, cross, dot as matmul, isfinite, all -except Exception: +except ImportError: from numpy import identity, array, linalg, cross, matmul, isfinite, all def bcross(b, ω, k=7e-4): diff --git a/state_machine/applications/flight/lib/frames.py b/state_machine/applications/flight/lib/frames.py index fab02dd0..ca23417f 100644 --- a/state_machine/applications/flight/lib/frames.py +++ b/state_machine/applications/flight/lib/frames.py @@ -4,7 +4,7 @@ except ImportError: import numpy as np from numpy import cos, sin, pi, sqrt, array -from math import atan2 as arctan2 +from math import atan2 as arctan2 J2000 = 946684800 # unix timestamp for the Julian date 2000-01-01 MJD_ZERO = 2400000.5 # Offset of Modified Julian Days representation with respect to Julian Days. diff --git a/state_machine/applications/flight/lib/mathutils.py b/state_machine/applications/flight/lib/mathutils.py index bbe2c3eb..dc29346f 100644 --- a/state_machine/applications/flight/lib/mathutils.py +++ b/state_machine/applications/flight/lib/mathutils.py @@ -1,6 +1,6 @@ try: from ulab.numpy import array, ndarray, zeros, eye as I, dot as matmul # noqa: E741 (I is not ambiguous) -except Exception: +except ImportError: from numpy import array, ndarray, zeros, eye as I, matmul # noqa: E741 (I is not ambiguous) def block(S): diff --git a/state_machine/applications/flight/lib/mekf.py b/state_machine/applications/flight/lib/mekf.py index c87ab56e..fa6375f6 100644 --- a/state_machine/applications/flight/lib/mekf.py +++ b/state_machine/applications/flight/lib/mekf.py @@ -12,7 +12,7 @@ """ try: from ulab.numpy import dot as matmul, eye as I, zeros, array, linalg, concatenate as concat # noqa: E741 -except Exception: +except ImportError: from numpy import linalg, matmul, eye as I, zeros, array, concatenate as concat # noqa: E741 from lib.mathutils import quaternion_mul, quaternion_to_left_matrix, hat, block, quaternion_to_rotation_matrix from math import cos, sin From 9593bdba7e28b2b43969c9a17294ad882216b5ea Mon Sep 17 00:00:00 2001 From: thetazero Date: Thu, 4 Aug 2022 09:32:30 -0400 Subject: [PATCH 10/14] Reorganize into gnc folder --- state_machine/applications/flight/lib/{ => gnc}/IGRF.py | 2 +- state_machine/applications/flight/lib/{ => gnc}/control.py | 0 state_machine/applications/flight/lib/{ => gnc}/frames.py | 0 state_machine/applications/flight/lib/{ => gnc}/mathutils.py | 0 state_machine/applications/flight/lib/{ => gnc}/mekf.py | 2 +- state_machine/applications/flight/lib/{ => gnc}/sun_position.py | 0 state_machine/unit_tests/test_control.py | 2 +- state_machine/unit_tests/test_frames.py | 2 +- state_machine/unit_tests/test_igrf.py | 2 +- state_machine/unit_tests/test_mathutils.py | 2 +- state_machine/unit_tests/test_mekf.py | 2 +- state_machine/unit_tests/test_orbital_mechanics.py | 2 +- state_machine/unit_tests/test_sun_position.py | 2 +- 13 files changed, 9 insertions(+), 9 deletions(-) rename state_machine/applications/flight/lib/{ => gnc}/IGRF.py (99%) rename state_machine/applications/flight/lib/{ => gnc}/control.py (100%) rename state_machine/applications/flight/lib/{ => gnc}/frames.py (100%) rename state_machine/applications/flight/lib/{ => gnc}/mathutils.py (100%) rename state_machine/applications/flight/lib/{ => gnc}/mekf.py (96%) rename state_machine/applications/flight/lib/{ => gnc}/sun_position.py (100%) diff --git a/state_machine/applications/flight/lib/IGRF.py b/state_machine/applications/flight/lib/gnc/IGRF.py similarity index 99% rename from state_machine/applications/flight/lib/IGRF.py rename to state_machine/applications/flight/lib/gnc/IGRF.py index 5144b749..19bcfded 100644 --- a/state_machine/applications/flight/lib/IGRF.py +++ b/state_machine/applications/flight/lib/gnc/IGRF.py @@ -8,7 +8,7 @@ import numpy as np import math -import lib.frames as frames +import lib.gnc.frames as frames def reset_array(input_array): for i in range(len(input_array)): diff --git a/state_machine/applications/flight/lib/control.py b/state_machine/applications/flight/lib/gnc/control.py similarity index 100% rename from state_machine/applications/flight/lib/control.py rename to state_machine/applications/flight/lib/gnc/control.py diff --git a/state_machine/applications/flight/lib/frames.py b/state_machine/applications/flight/lib/gnc/frames.py similarity index 100% rename from state_machine/applications/flight/lib/frames.py rename to state_machine/applications/flight/lib/gnc/frames.py diff --git a/state_machine/applications/flight/lib/mathutils.py b/state_machine/applications/flight/lib/gnc/mathutils.py similarity index 100% rename from state_machine/applications/flight/lib/mathutils.py rename to state_machine/applications/flight/lib/gnc/mathutils.py diff --git a/state_machine/applications/flight/lib/mekf.py b/state_machine/applications/flight/lib/gnc/mekf.py similarity index 96% rename from state_machine/applications/flight/lib/mekf.py rename to state_machine/applications/flight/lib/gnc/mekf.py index fa6375f6..97e09302 100644 --- a/state_machine/applications/flight/lib/mekf.py +++ b/state_machine/applications/flight/lib/gnc/mekf.py @@ -14,7 +14,7 @@ from ulab.numpy import dot as matmul, eye as I, zeros, array, linalg, concatenate as concat # noqa: E741 except ImportError: from numpy import linalg, matmul, eye as I, zeros, array, concatenate as concat # noqa: E741 -from lib.mathutils import quaternion_mul, quaternion_to_left_matrix, hat, block, quaternion_to_rotation_matrix +from lib.gnc.mathutils import quaternion_mul, quaternion_to_left_matrix, hat, block, quaternion_to_rotation_matrix from math import cos, sin q = array([0., 0., 0., 0.]) # Quaternion attitude vector diff --git a/state_machine/applications/flight/lib/sun_position.py b/state_machine/applications/flight/lib/gnc/sun_position.py similarity index 100% rename from state_machine/applications/flight/lib/sun_position.py rename to state_machine/applications/flight/lib/gnc/sun_position.py diff --git a/state_machine/unit_tests/test_control.py b/state_machine/unit_tests/test_control.py index 822089c9..34e0474b 100644 --- a/state_machine/unit_tests/test_control.py +++ b/state_machine/unit_tests/test_control.py @@ -6,7 +6,7 @@ sys.path.insert(0, './state_machine/applications/flight') sys.path.insert(0, './state_machine/frame') -from lib.control import bcross +from lib.gnc.control import bcross class BCrossTest(unittest.TestCase): diff --git a/state_machine/unit_tests/test_frames.py b/state_machine/unit_tests/test_frames.py index a8b61841..1e8098a1 100644 --- a/state_machine/unit_tests/test_frames.py +++ b/state_machine/unit_tests/test_frames.py @@ -6,7 +6,7 @@ sys.path.insert(0, './state_machine/applications/flight') -import lib.frames as frames +import lib.gnc.frames as frames EARTH_RADIUS = 6371 # Earth radius (km) LEO = 2000 # Low Earth Orbit Altitude Limit (km) diff --git a/state_machine/unit_tests/test_igrf.py b/state_machine/unit_tests/test_igrf.py index 9d0c757f..ca05995c 100644 --- a/state_machine/unit_tests/test_igrf.py +++ b/state_machine/unit_tests/test_igrf.py @@ -5,7 +5,7 @@ sys.path.insert(0, './state_machine/applications/flight') -from lib.IGRF import igrf, igrf_eci +from lib.gnc.IGRF import igrf, igrf_eci EARTH_RADIUS = 6.378136300e3 # km diff --git a/state_machine/unit_tests/test_mathutils.py b/state_machine/unit_tests/test_mathutils.py index a3bd347f..31abbb93 100644 --- a/state_machine/unit_tests/test_mathutils.py +++ b/state_machine/unit_tests/test_mathutils.py @@ -4,7 +4,7 @@ sys.path.insert(0, './state_machine/applications/flight') -from lib.mathutils import hat, quaternion_to_left_matrix, block, quaternion_to_rotation_matrix, quaternion_mul +from lib.gnc.mathutils import hat, quaternion_to_left_matrix, block, quaternion_to_rotation_matrix, quaternion_mul class HatTests(unittest.TestCase): diff --git a/state_machine/unit_tests/test_mekf.py b/state_machine/unit_tests/test_mekf.py index 8e26feb0..39549329 100644 --- a/state_machine/unit_tests/test_mekf.py +++ b/state_machine/unit_tests/test_mekf.py @@ -4,7 +4,7 @@ sys.path.insert(0, './state_machine/applications/flight') -import lib.mekf as mekf +import lib.gnc.mekf as mekf class PropogationTest(unittest.TestCase): diff --git a/state_machine/unit_tests/test_orbital_mechanics.py b/state_machine/unit_tests/test_orbital_mechanics.py index 5667339b..7ded0870 100644 --- a/state_machine/unit_tests/test_orbital_mechanics.py +++ b/state_machine/unit_tests/test_orbital_mechanics.py @@ -6,7 +6,7 @@ sys.path.insert(0, './state_machine/applications/flight') -from lib.orbital_mechanics import rk4, propogate, d_state, MEAN_RADIUS +from lib.gnc.orbital_mechanics import rk4, propogate, d_state, MEAN_RADIUS class TestRK4(unittest.TestCase): diff --git a/state_machine/unit_tests/test_sun_position.py b/state_machine/unit_tests/test_sun_position.py index 1644917f..950ff207 100644 --- a/state_machine/unit_tests/test_sun_position.py +++ b/state_machine/unit_tests/test_sun_position.py @@ -4,7 +4,7 @@ sys.path.insert(0, './state_machine/applications/flight') -from lib.sun_position import approx_sun_position_ECI +from lib.gnc.sun_position import approx_sun_position_ECI class TestSunPosition(unittest.TestCase): From 2e6e4e76296069d898afec2a41fd8018dad4c79e Mon Sep 17 00:00:00 2001 From: thetazero Date: Thu, 4 Aug 2022 09:36:25 -0400 Subject: [PATCH 11/14] move oribtal mechanics --- .../applications/flight/lib/{ => gnc}/orbital_mechanics.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename state_machine/applications/flight/lib/{ => gnc}/orbital_mechanics.py (100%) diff --git a/state_machine/applications/flight/lib/orbital_mechanics.py b/state_machine/applications/flight/lib/gnc/orbital_mechanics.py similarity index 100% rename from state_machine/applications/flight/lib/orbital_mechanics.py rename to state_machine/applications/flight/lib/gnc/orbital_mechanics.py From a05017c573e891e4e0f4056571881baea1e0ce98 Mon Sep 17 00:00:00 2001 From: thetazero Date: Thu, 4 Aug 2022 09:37:18 -0400 Subject: [PATCH 12/14] Fix imports --- state_machine/applications/flight/Tasks/gnc.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/state_machine/applications/flight/Tasks/gnc.py b/state_machine/applications/flight/Tasks/gnc.py index 600c21d7..edcfb776 100644 --- a/state_machine/applications/flight/Tasks/gnc.py +++ b/state_machine/applications/flight/Tasks/gnc.py @@ -1,10 +1,10 @@ from lib.template_task import Task -from lib.control import bcross import lib.pycubed as cubesat -from lib.IGRF import igrf_eci -from lib.sun_position import approx_sun_position_ECI -import lib.orbital_mechanics as orbital_mechanics -import lib.mekf as mekf +from lib.gnc.control import bcross +from lib.gnc.IGRF import igrf_eci +from lib.gnc.sun_position import approx_sun_position_ECI +import lib.gnc.orbital_mechanics as orbital_mechanics +import lib.gnc.mekf as mekf import time try: from ulab.numpy import array From 37b16d53fc14d633a7f79d25673d2034b9665b7c Mon Sep 17 00:00:00 2001 From: thetazero Date: Thu, 4 Aug 2022 09:51:25 -0400 Subject: [PATCH 13/14] Add gnc_state --- state_machine/applications/flight/Tasks/gnc.py | 10 +++------- state_machine/applications/flight/lib/gnc/gnc_state.py | 6 ++++++ 2 files changed, 9 insertions(+), 7 deletions(-) create mode 100644 state_machine/applications/flight/lib/gnc/gnc_state.py diff --git a/state_machine/applications/flight/Tasks/gnc.py b/state_machine/applications/flight/Tasks/gnc.py index edcfb776..dd769a5f 100644 --- a/state_machine/applications/flight/Tasks/gnc.py +++ b/state_machine/applications/flight/Tasks/gnc.py @@ -5,11 +5,8 @@ from lib.gnc.sun_position import approx_sun_position_ECI import lib.gnc.orbital_mechanics as orbital_mechanics import lib.gnc.mekf as mekf +import lib.gnc.gnc_state as gnc_state import time -try: - from ulab.numpy import array -except ImportError: - from numpy import array def toStr(arr): @@ -21,7 +18,6 @@ class task(Task): rgb_on = False last = None - eci_state = array([6871, -6571, -7071, 2, -10, 3]) # [x, y, z, vx, vy, vz] sun_sensor_failed = False async def main_task(self): @@ -44,14 +40,14 @@ async def main_task(self): self.debug(f'Error: {e}') self.sun_sensor_failed = True failed = True - nr_mag = igrf_eci(t, self.eci_state[0:3]) + nr_mag = igrf_eci(t, gnc_state.eci_state[0:3]) nr_sun = approx_sun_position_ECI(t) if not failed: mekf.step(w, delta_t, nr_mag, nr_sun, br_mag, br_sun) # propogate ECI position - self.eci_state = orbital_mechanics.propogate(self.eci_state, delta_t, integration_step=5) + gnc_state.eci_state = orbital_mechanics.propogate(gnc_state.eci_state, delta_t, integration_step=5) # compute control m = bcross(cubesat.magnetic(), cubesat.gyro()) diff --git a/state_machine/applications/flight/lib/gnc/gnc_state.py b/state_machine/applications/flight/lib/gnc/gnc_state.py new file mode 100644 index 00000000..d7eff7b3 --- /dev/null +++ b/state_machine/applications/flight/lib/gnc/gnc_state.py @@ -0,0 +1,6 @@ +try: + from ulab.numpy import array +except ImportError: + from numpy import array + +eci_state = array([6871, -6571, -7071, 2, -10, 3]) # [x, y, z, vx, vy, vz] From d3414b9153a2dfbafee3e87a35bb1369aba6ab8d Mon Sep 17 00:00:00 2001 From: thetazero Date: Thu, 4 Aug 2022 10:02:16 -0400 Subject: [PATCH 14/14] Multithreading from numpy cuasing issues --- state_machine/drivers/emulation/lib/pycubed.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/state_machine/drivers/emulation/lib/pycubed.py b/state_machine/drivers/emulation/lib/pycubed.py index 6fe9100a..fb99bf43 100644 --- a/state_machine/drivers/emulation/lib/pycubed.py +++ b/state_machine/drivers/emulation/lib/pycubed.py @@ -1,3 +1,8 @@ +# force numpy to use one thread +import os +os.environ["OPENBLAS_NUM_THREADS"] = "1" +os.environ["MKL_NUM_THREADS"] = "1" + import time import tasko