Merge pull request #269 from lasp/refactor/193-coarseSunSensor-refactor

Refactor/193 coarse sun sensor refactor

src/simulation/sensors/coarseSunSensor/_UnitTest/test_customFovCss.py

@@ -0,0 +1,129 @@
+#
+#  ISC License
+#
+#  Copyright (c) 2024, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder
+#
+#  Permission to use, copy, modify, and/or distribute this software for any
+#  purpose with or without fee is hereby granted, provided that the above
+#  copyright notice and this permission notice appear in all copies.
+#
+#  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+#  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+#  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+#  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+#  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+#  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+#  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+#
+import inspect
+import os
+
+import numpy as np
+import pytest
+
+filename = inspect.getframeinfo(inspect.currentframe()).filename
+path = os.path.dirname(os.path.abspath(filename))
+
+
+from Basilisk.architecture import messaging
+from Basilisk.architecture import bskLogging
+from Basilisk.simulation import coarseSunSensor
+from Basilisk.utilities import astroFunctions
+from Basilisk.utilities import macros
+from Basilisk.utilities import SimulationBaseClass
+
+
+@pytest.mark.parametrize("xi", [np.pi/6, np.pi/3, np.pi/2])
+@pytest.mark.parametrize("eta", [np.pi/6, np.pi/3, np.pi/2])
+@pytest.mark.parametrize("zeta", [np.pi/6, np.pi/3, np.pi/2])
+@pytest.mark.parametrize("sunLocation", [0, 1, 2, 3])
+@pytest.mark.parametrize("accuracy", [1e-10])
+def test_customFovCss(show_plots, xi, eta, zeta, sunLocation, accuracy):
+
+    unitTaskName = "unitTask"
+    unitProcessName = "TestProcess"
+    bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING)
+
+    # Create a sim module as an empty container
+    unitTestSim = SimulationBaseClass.SimBaseClass()
+
+    # Create test thread
+    testProcessRate = macros.sec2nano(1.1)
+    testProc = unitTestSim.CreateNewProcess(unitProcessName)
+    testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate))
+
+    lHat_B = [1, 0, 0]
+    mHat_B = [0, 1, 0]
+    nHat_B = [0, 0, 1]
+
+    # Construct algorithm and associated C++ container
+    CSS = coarseSunSensor.CoarseSunSensor()
+    CSS.ModelTag = "coarseSunSensor"
+    CSS.scaleFactor = 1
+    CSS.lHat_B = lHat_B
+    CSS.mHat_B = mHat_B
+    CSS.nHat_B = nHat_B
+    CSS.customFov = True
+    CSS.fovXi = xi
+    CSS.fovEta = eta
+    CSS.fovZeta = zeta
+
+    # Add test module to runtime call list
+    unitTestSim.AddModelToTask(unitTaskName, CSS)
+
+    # Initialize the test module configuration data
+    # These will eventually become input messages
+
+    omega_BN_B = np.array([0.1, -0.2, 0.3])
+
+    # Create input navigation message
+    SCStatesMsgData = messaging.SCStatesMsgPayload()
+    SCStatesMsgData.r_BN_N = [0, 0, 0]
+    SCStatesMsgData.sigma_BN = [0, 0, 0]
+    SCStatesMsg = messaging.SCStatesMsg().write(SCStatesMsgData)
+    CSS.stateInMsg.subscribeTo(SCStatesMsg)
+
+    x = 0
+    y = 0
+    match sunLocation:
+        case 0:
+            y = np.sin(xi - accuracy)
+        case 1:
+            x = np.sin(zeta - accuracy)
+        case 2:
+            y = -np.sin(eta - accuracy)
+        case 3:
+            x = -np.sin(zeta - accuracy)
+    z = (1 - x*x - y*y)**0.5
+
+    # Create input Sun spice msg
+    SpicePlanetStateMsgData = messaging.SpicePlanetStateMsgPayload()
+    SpicePlanetStateMsgData.PositionVector = astroFunctions.AU * 1000 * np.array([x, y, z])
+    SpicePlanetStateMsg = messaging.SpicePlanetStateMsg().write(SpicePlanetStateMsgData)
+    CSS.sunInMsg.subscribeTo(SpicePlanetStateMsg)
+
+    # Setup logging on the test module output message so that we get all the writes to it
+    dataLog = CSS.cssDataOutMsg.recorder()
+    unitTestSim.AddModelToTask(unitTaskName, dataLog)
+
+    # Need to call the self-init and cross-init methods
+    unitTestSim.InitializeSimulation()
+
+    # Set the simulation time.
+    unitTestSim.ConfigureStopTime(macros.sec2nano(1.0))
+
+    # Begin the simulation time run set above
+    unitTestSim.ExecuteSimulation()
+
+    moduleOutput = dataLog.OutputData[0]
+
+    truth = np.dot(nHat_B, [x, y, z])
+
+    # set the filtered output truth states
+    np.testing.assert_allclose(moduleOutput, truth, rtol=0, atol=accuracy, verbose=True)
+
+    return
+
+
+if __name__ == "__main__":
+    test_customFovCss(False, np.pi/6, np.pi/3, np.pi/2, 0, 1e-10)

src/simulation/sensors/coarseSunSensor/coarseSunSensor.cpp

@@ -239,7 +239,40 @@ void CoarseSunSensor::computeTrueOutput()
 {
     // If sun heading is within sensor field of view, compute signal
     double signal = this->nHat_B.dot(this->sHat_B);
-    this->trueValue = signal >= cos(this->fov) ? signal : 0.0;
+    if (!this->customFov) {
+        this->trueValue = signal >= cos(this->fov) ? signal : 0.0;
+    }
+    else {
+        Eigen::Matrix3d BS;
+        BS << this->lHat_B, this->mHat_B, this->nHat_B;
+        Eigen::Matrix3d SB = BS.transpose();
+
+        Eigen::Vector3d sHat_S = SB * this->sHat_B;
+        double x = sHat_S[0];
+        double y = sHat_S[1];
+        double z = sHat_S[2];
+        double a = sin(this->fovZeta); // elliptical fov semimajor axis
+
+        if (z < 0 || fabs(x) > a) {
+            signal = 0.0;
+        }
+        else {
+            double b;       // elliptical fov semiminor axis
+            double yLim;    // boundary of elliptical fov
+            if (y > 0) {
+                b = sin(this->fovXi);
+                yLim = std::min(b * pow(1 - pow(x / a, this->n1), 1 / this->n1), pow(1-x*x, 0.5));
+            }
+            else {
+                b = sin(this->fovEta);
+                yLim = std::max(-b * pow(1 - pow(x / a, this->n2), 1 / this->n2), -pow(1-x*x, 0.5));
+            }
+            if (fabs(y) > fabs(yLim)) {
+                signal = 0.0;
+            }
+        }
+        this->trueValue = signal;
+    }
 
     // Define epsilon that will avoid dividing by a very small kelly factor, i.e 0.0.
     double eps = 1e-10;

src/simulation/sensors/coarseSunSensor/coarseSunSensor.h

@@ -64,7 +64,7 @@ class CoarseSunSensor: public SysModel {
     void scaleSensorValues();  //!< scale the sensor values
     void applySaturation();     //!< apply saturation effects to sensed output (floor and ceiling)
     void writeOutputMessages(uint64_t Clock); //!< @brief method to write the output message to the system
-    
+
 public:
     ReadFunctor<SpicePlanetStateMsgPayload> sunInMsg; //!< [-] input message for sun data
     ReadFunctor<SCStatesMsgPayload> stateInMsg;   //!< [-] input message for spacecraft state
@@ -77,8 +77,10 @@ class CoarseSunSensor: public SysModel {
     double              theta;                  //!< [rad] css azimuth angle, measured positive from the body +x axis around the +z axis
     double              phi;                    //!< [rad] css elevation angle, measured positive toward the body +z axis from the x-y plane
     double              B2P321Angles[3];        //!< [-] 321 Euler angles for body to platform
-    Eigen::Matrix3d     dcm_PB;                 //!< [-] DCM of platform frame P relative to body frame B
-    Eigen::Vector3d     nHat_B;                 //!< [-] css unit direction vector in body frame components
+    Eigen::Matrix3d     dcm_PB;                 //!< [-] DCM from platform frame P to body frame B
+    Eigen::Vector3d     lHat_B;                 //!< [-] css unit direction vector 1 in body frame components
+    Eigen::Vector3d     mHat_B;                 //!< [-] css unit direction vector 2 in body frame components
+    Eigen::Vector3d     nHat_B;                 //!< [-] css unit direction vector 3 in body frame components
     Eigen::Vector3d     sHat_B;                 //!< [-] unit vector to sun in B
     double              albedoValue = -1.0;     //!< [-] albedo irradiance measurement
     double              scaleFactor;            //!< [-] scale factor applied to sensor (common + individual multipliers)
@@ -87,8 +89,14 @@ class CoarseSunSensor: public SysModel {
     double              sensedValue;            //!< [-] solar irradiance measurement including perturbations
     double              kellyFactor;            //!< [-] Kelly curve fit for output cosine curve
     double              fov;                    //!< [-] rad, field of view half angle
+    bool                customFov = false;
+    double              fovXi;                  //!< [rad] half-angle fov along the +lHat direction
+    double              fovEta;                 //!< [rad] half-angle fov along the -lHat direction
+    double              fovZeta;                //!< [rad] half-angle fov along the ±mHat direction
+    double              n1 = 2;                 //!< [-] ellipse warping coefficient in the +lHat direction
+    double              n2 = 2;                 //!< [-] ellipse warping coefficient in the -lHat direction
     Eigen::Vector3d     r_B;                    //!< [m] position vector in body frame
-    Eigen::Vector3d     r_PB_B;                 //!< [m] misalignment of CSS platform wrt spacecraft body frame 
+    Eigen::Vector3d     r_PB_B;                 //!< [m] misalignment of CSS platform wrt spacecraft body frame
     double              senBias;                //!< [-] Sensor bias value
     double              senNoiseStd;            //!< [-] Sensor noise value
     double              faultNoiseStd;          //!< [-] Sensor noise value if CSSFAULT_RAND is triggered
@@ -110,8 +118,8 @@ class CoarseSunSensor: public SysModel {
 };
 
 //!@brief Constellation of coarse sun sensors for aggregating output information
-/*! This class is a thin container on top of the above coarse-sun sensor class.  
-It is used to aggregate the output messages of the coarse sun-sensors into a 
+/*! This class is a thin container on top of the above coarse-sun sensor class.
+It is used to aggregate the output messages of the coarse sun-sensors into a
 a single output for use by downstream models.*/
 class CSSConstellation: public SysModel {
  public:
@@ -120,7 +128,7 @@ class CSSConstellation: public SysModel {
     void Reset(uint64_t CurrentClock);          //!< Method for reseting the module
     void UpdateState(uint64_t CurrentSimNanos); //!< @brief [-] Main update method for CSS constellation
     void appendCSS(CoarseSunSensor *newSensor); //!< @brief [-] Method for adding sensor to list
-    
+
  public:
     Message<CSSArraySensorMsgPayload> constellationOutMsg;  //!< [-] CSS constellation output message
     std::vector<CoarseSunSensor *> sensorList;    //!< [-] List of coarse sun sensors in constellation

src/simulation/sensors/coarseSunSensor/coarseSunSensor.rst

@@ -15,9 +15,27 @@ The corruption types are outlined in this
 .. warning::
   Be careful when generating CSS objects in a loop or using a function! It is often convenient to initialize many CSS' with the same attributes using a loop with a function like ``setupCSS(CSS)`` where ``setupCSS`` initializes the field of view, sensor noise, min / max outputs, etc. If you do this, be sure to disown the memory in Python so the object doesn't get accidentally garbage collected or freed for use. You can disown the memory though ``cssObject.this.disown()``.
 
-The coarse sun sensor module also supports user-enabled faulty behavior by assigning a values to the ``.faultState`` member of a given sensor. An example of such call would is ``cssSensor.faultState = coarse_sun_sensor.CSSFAULT_OFF`` where ``cssSensor`` is an instantiated sensor, and ``coarse_sun_sensor`` is the imported module. 
+It is possible to define an asymmetrical field of view for the CSS. This is useful when trying to simulate baffles that partially limit the field of view of the sensor in a certain direction, usually to avoid reflected sunlight fo enter the view of the CSS. This requires not only the definition of the CSS boresight vector :math:`\boldsymbol{\hat{n}}`, but also two additional unit vectors :math:`\boldsymbol{\hat{l}}` and :math:`\boldsymbol{\hat{m}}` that define a full CSS frame. This custom field of view is modeled as the combination of two pseudo-ellipses in the :math:`(l,m)` plane. The resulting planar, closed curve is projected onto the unit sphere surface in the positive :math:`\boldsymbol{\hat{n}}` direction. The custom field of view can be set up as follows::
 
-The module currently supports the following faults: 
+    CSS = coarseSunSensor.CoarseSunSensor()
+    CSS.ModelTag = "coarseSunSensor"
+    CSS.scaleFactor = 1
+    CSS.lHat_B = lHat_B
+    CSS.mHat_B = mHat_B
+    CSS.nHat_B = nHat_B
+    CSS.customFov = True
+    CSS.fovXi = xi
+    CSS.fovEta = eta
+    CSS.fovZeta = zeta
+    CSS.n1 = n1
+    CSS.n2 = n2
+
+
+where :math:`\xi` indicates the half-angle field of view in the :math:`+\boldsymbol{\hat{m}}` direction, :math:`\eta` indicates the half-angle field of view in the :math:`-\boldsymbol{\hat{m}}` direction, and :math:`\zeta` indicates the half-angle field of view in the positive :math:`\pm \boldsymbol{\hat{l}}` direction. By design, the field of view is symmetric with respect to the :math:`(l,n)` plane. The coefficients :math:`n_1` and :math:`n_2` are warping coefficients that allow to obtain more rectangular shapes for the two half ellipsoids as :math:`n_1, n_2 >> 2`. Setting :math:`n_1 = n_2 = 2` ensures that the half ellipsoids are, in fact, half ellipses; setting :math:`\xi = \eta = \zeta` returns a regular conical field of view.
+
+The coarse sun sensor module also supports user-enabled faulty behavior by assigning a values to the ``.faultState`` member of a given sensor. An example of such call would is ``cssSensor.faultState = coarse_sun_sensor.CSSFAULT_OFF`` where ``cssSensor`` is an instantiated sensor, and ``coarse_sun_sensor`` is the imported module.
+
+The module currently supports the following faults:
 
 .. list-table:: ``CoarseSunSensor`` Fault Types
     :widths: 35 50
@@ -32,9 +50,9 @@ The module currently supports the following faults:
     * - ``CSSFAULT_STUCK_MAX``
       - CSS signal is stuck on the maximum value
     * - ``CSSFAULT_STUCK_RAND``
-      - CSS signal is stuck on a random value 
+      - CSS signal is stuck on a random value
     * - ``CSSFAULT_RAND``
-      - CSS produces random values 
+      - CSS produces random values