Feature/743 solar array reference fix

docs/source/Support/bskReleaseNotes.rst

@@ -103,6 +103,7 @@ Version  |release|
 
 - Update CI Linux build with ``opNav`` to use Ubuntu 22.04, not latest (i.e. 24.02).  The latter does not
   support directly Python 3.11, and Basilisk does not support Python 3.13 yet.
+- Updated :ref:`solarArrayReference` to correct the wrong assumption of reflective solar arrays for momentum management pointing mode.
 
 
 Version 2.5.0 (Sept. 30, 2024)

src/fswAlgorithms/effectorInterfaces/solarArrayReference/_UnitTest/test_solarArrayReference.py

@@ -65,22 +65,6 @@ def computeSunReferenceAngle(sigma_RN, rHat_SB_N, a1Hat_B, a2Hat_B, theta0):
 
     return theta
 
-def computeSrpReferenceAngle(sHat, hHat, thetaSunR):
-
-    s2 = sHat[1]
-    s3 = sHat[2]
-    h2 = hHat[1]
-    h3 = hHat[2]
-    Delta = 8 * ((s2*h2)**2 + (s3*h3)**2 + (s2*h3)**2 + (s3*h2)**2) + (s2*h2 + s3*h3)**2
-    t = np.arctan( (3*(s3*h3 - s2*h2) - Delta**0.5) / (4*s2*h3 + 2*s3*h2) )
-    yHat = np.array([0.0, np.cos(t), np.sin(t)])
-    if np.dot(sHat, yHat) < 0:
-        yHat = -1 * yHat
-    theta = np.arctan2(yHat[2], yHat[1])
-    f = np.dot(hHat, yHat) * (np.dot(sHat, yHat))**2
-
-    return thetaSunR - f * (theta - thetaSunR)
-
 # Uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed.
 # @pytest.mark.skipif(conditionstring)
 # Uncomment this line if this test has an expected failure, adjust message as needed.
@@ -169,6 +153,9 @@ def solarArrayRotationTestFunction(show_plots, rHat_SB_N, sigma_BN, sigma_RN, at
     solarArray.r_AB_B = r_AB_B
     solarArray.pointingMode = pointingMode
     solarArray.attitudeFrame = attitudeFrame
+    solarArray.ThetaMax = np.pi/2
+    solarArray.sigma = 1
+    solarArray.n = 1
 
     # Create input attitude navigation message
     natAttInMsgData = messaging.NavAttMsgPayload()
@@ -247,13 +234,22 @@ def solarArrayRotationTestFunction(show_plots, rHat_SB_N, sigma_BN, sigma_RN, at
             RB = np.matmul(RN, BN.transpose())
             sHat = np.matmul(RB, rHat_SB_B)
             H_R = np.matmul(RB, H_B)
-            hHat = np.cross(r_AB_B, H_R)
         else:
             sHat = rHat_SB_B
-            hHat = np.cross(r_AB_B, H_B)
-        hHat = hHat / np.linalg.norm(hHat)
+            H_R = H_B
+
+        F_R = np.cross(sHat, r_AB_B)
+        f = np.dot(F_R, H_R)
+
+        if f > 0:
+            thetaR = thetaSunR + 2 * solarArray.ThetaMax / np.pi * np.arctan(solarArray.sigma * f**(solarArray.n))
+        else:
+            thetaR = thetaSunR
 
-        thetaR = computeSrpReferenceAngle(sHat, hHat, thetaSunR)
+    if thetaR-thetaC > np.pi:
+        thetaR -= 2*np.pi
+    elif thetaR-thetaC < -np.pi:
+        thetaR += 2*np.pi
 
     # compare the module results to the truth values
     if not unitTestSupport.isDoubleEqual(dataLog.theta[0], thetaR, accuracy):

src/fswAlgorithms/effectorInterfaces/solarArrayReference/solarArrayReference.c

@@ -182,12 +182,14 @@ void Update_solarArrayReference(solarArrayReferenceConfig *configData, uint64_t
             m33MultV3(RB, hs_B, hs_R);
         }
 
-        double thetaSrpR;   // reference solar array angle that maximizes SRP dumping torque
+        double F_R[3];
+        v3Cross(rHat_SB_R, r_AC_B, F_R);
         double f;
-        computeSrpArrayNormal(a1Hat_B, a2Hat_B, a3Hat_B, rHat_SB_R, r_AC_B, hs_R, &thetaSrpR, &f);
+        f = v3Dot(F_R, hs_R);
 
-        // bias the reference angle towards thetaSrpR more if there is more potential to dump momentum (f = -1)
-        thetaR = thetaSunR - f * (thetaSrpR - thetaSunR);
+        if (f > 0) {
+            thetaR += 2 * configData->ThetaMax / M_PI * atan(configData->sigma * pow(f, configData->n));
+        }
     }
 
     // always make the absolute difference |thetaR-thetaC| smaller than 2*pi
@@ -218,57 +220,3 @@ void Update_solarArrayReference(solarArrayReferenceConfig *configData, uint64_t
     /* write output message */
     HingedRigidBodyMsg_C_write(&hingedRigidBodyRefOut, &configData->hingedRigidBodyRefOutMsg, moduleID, callTime);
 }
-
-/*! This method computes the reference angle for the arrays that maximizes SRP torque in the direction
- * opposite to current RW momentum (thetaSrpR). It also outputs a coefficient f that is proportional to the projection
- * of the SRP torque along the RW net momentum.
- @return void
- */
-void computeSrpArrayNormal(double a1Hat_B[3], double a2Hat_B[3], double a3Hat_B[3],
-                           double sHat_R[3], double r_B[3], double H_B[3], double *thetaR, double *f)
-{
-    /*! Define map between body frame and array frame A */
-    double BA[3][3];        // DCM mapping between body frame and array frame
-    for (int i=0; i<3; ++i) {
-        BA[i][0] = a1Hat_B[i];
-        BA[i][1] = a2Hat_B[i];
-        BA[i][2] = a3Hat_B[i];
-    }
-
-    /*! Map vectors to array frame A */
-    double sHat_A[3];       // Sun heading in array frame
-    v3tMultM33(sHat_R, BA, sHat_A);
-    double hHat_B[3];       // h vector (see documentation) in body frame
-    v3Cross(r_B, H_B, hHat_B);
-    v3Normalize(hHat_B, hHat_B);
-    double hHat_A[3];       // h vector (see documentation) in array frame
-    v3tMultM33(hHat_B, BA, hHat_A);
-
-    /*! Define vector components in the local x-y plane */
-    double s2 = sHat_A[1];
-    double s3 = sHat_A[2];
-    double h2 = hHat_A[1];
-    double h3 = hHat_A[2];
-
-    // Discriminant of characteristic equation
-    double Delta = 8 * (pow(s2*h2, 2) + pow(s3*h3, 2) + pow(s2*h3, 2) + pow(s3*h2, 2)) + pow(s2*h2+s3*h3, 2);
-
-    // Solution that produces SRP torque opposite to current RW momentum
-    double t = atan( (3*(s3*h3 - s2*h2) - pow(Delta, 0.5)) / (4*s2*h3 + 2*s3*h2) );
-
-    // SA normal direction to maximize momentum dumping
-    double a2SrpHat_A[3] = {0.0, cos(t), sin(t)};
-
-    // Choose normal direction that is also power-positive
-    double dotS = v3Dot(sHat_A, a2SrpHat_A);
-    if (dotS < 0) {
-        v3Scale(-1, a2SrpHat_A, a2SrpHat_A);
-    }
-
-    double theta = atan2(a2SrpHat_A[2], a2SrpHat_A[1]);
-    double fVal = dotS * dotS * v3Dot(hHat_A, a2SrpHat_A);
-
-    // Return the corresponding solar array reference angle and momentum dumping coefficient
-    *thetaR = theta;
-    *f = fVal;
-}

src/fswAlgorithms/effectorInterfaces/solarArrayReference/solarArrayReference.h

@@ -43,11 +43,14 @@ typedef enum pointingMode{
 /*! @brief Top level structure for the sub-module routines. */
 typedef struct {
 
-    /* declare these user-defined quantities */
+    /*! declare these user-defined quantities */
     double a1Hat_B[3];              //!< solar array drive axis in body frame coordinates
     double a2Hat_B[3];              //!< solar array surface normal at zero rotation
     AttitudeFrame attitudeFrame;    //!< attitudeFrame = 1: compute theta reference based on body frame instead of reference frame
     double r_AB_B[3];               //!< location of the array center of pressure in body frame coordinates
+    double ThetaMax;
+    double sigma;
+    double n;
 
     /*! declare these variables for internal computations */
     int                       count;                     //!< counter variable for finite differences
@@ -77,9 +80,6 @@ extern "C" {
     void Reset_solarArrayReference(solarArrayReferenceConfig *configData, uint64_t callTime, int64_t moduleID);
     void Update_solarArrayReference(solarArrayReferenceConfig *configData, uint64_t callTime, int64_t moduleID);
 
-    void computeSrpArrayNormal(double a1Hat_B[3], double a2Hat_B[3], double a3Hat_B[3],
-                               double sHat_R[3], double r_B[3], double H_B[3], double *thetaR, double *f);
-
 #ifdef __cplusplus
 }
 #endif

src/fswAlgorithms/effectorInterfaces/solarArrayReference/solarArrayReference.rst

@@ -73,36 +73,26 @@ and then normalizing to obtain :math:`{}^\mathcal{R}\boldsymbol{\hat{a}}_2`. The
     \theta_{\text{Sun,}R} = \arccos ({}^\mathcal{B}\boldsymbol{\hat{a}}_2 \cdot {}^\mathcal{R}\boldsymbol{\hat{a}}_2).
 
 
-Maximum Momentum Dumping
+Momentum Dumping
 +++++++++++++++++++++++++++
-In this pointing mode, the reference angle is computed in order to leverage solar radiation pressure (SRP) to produce a torque whose component in the opposite direction to the local net reaction wheel momentum (:math:`\boldsymbol{H}`) is maximum. Because the array can only rotate about the :math:`\boldsymbol{\hat{a}}_1` axis, the desired solar array normal :math:`\boldsymbol{\hat{y}}` can be expressed as follows, in the solar array frame :math:`\mathcal{A}`:
+In this pointing mode, the reference angle is computed in order to leverage solar radiation pressure (SRP) to produce a torque on the spacecraft opposing the local net reaction wheel momentum (:math:`\boldsymbol{H}`). This functionality applies to a set of two rotating solar arrays whose rotation axes are opposite to one another, and it consists in articulating the two arrays differentially in order to generate a net SRP torque.
 
-.. math::
-    {}^\mathcal{A}\boldsymbol{\hat{y}} = \{0, \cos t, \sin t \}^T.
-
-The dot product between the solar array torque and the net wheel momentum is the function to minimize:
+With respect to a ''zero rotation'' configuration, where zero rotation consists in having the power-generating surface of the array directly facing the Sun, the desire is to drive one of the two arrays at an offset inclination angle :math:`\theta` with respect to the Sun direction. This offset is defined as:
 
 .. math::
-    f(t) = \boldsymbol{L} \cdot \boldsymbol{H} = (\boldsymbol{\hat{s}} \cdot \boldsymbol{\hat{y}})^2 (\boldsymbol{r} \times (-\boldsymbol{\hat{y}})) \cdot \boldsymbol{H} = (\boldsymbol{\hat{s}} \cdot \boldsymbol{\hat{y}})^2 (\boldsymbol{h} \cdot \boldsymbol{\hat{y}})
-
-where :math:`\boldsymbol{h} = \boldsymbol{r} \times \boldsymbol{H}` and :math:`\boldsymbol{r}` is the position of the array center of pressure with respect to the system's center of mass. Taking the derivative with respect to :math:`t` and equating it to zero results in the third-order equation in :math:`\tan t`:
+    \theta = \left\{ \begin{array}[c|c|c] \\ \Theta_\text{max} \cdot \arctan(\sigma \nu^n) & \text{if} & \nu \geq 0 \\ 0 & \text{if} & \nu < 0 \\  \end{array} \right.
 
-.. math::
-    (s_2 \cos t + s_3 \sin t) \left[(2s_2h_3+s_3h_2) \tan^2 t -3(s_3h_3-s_2h_2) \tan t - (2s_3h_2+s_2h_3) \right] = 0
 
-whose desired solution is:
+where :math:`\Theta_\text{max}`, :math:`\sigma`, and :math:`n` are user-defined parameters, and :math:`\nu` is defined as:
 
 .. math::
-    \theta_{\text{Srp,}R} = t = \frac{3(s_3h_3-s_2h_2) - \sqrt{\Delta}}{4s_3h_2+2s_2h_3}.
-
-In the presence of two solar arrays, it is not desirable to maneuver both to the angle that minimizes :math:`f(t)`. This is because one array will always reach an edge-on configuration, thus resulting in a SRP torque imbalance between the arrays. For this reason, the array(s) are maneuvered to the reference angle
+    \nu = - (\boldsymbol{r}_{O/C} \times \boldsymbol{\hat{r}}_\text{S}) \cdot \boldsymbol{H}_\text{RW}
 
-.. math::
-    \theta_R = \theta_{\text{Sun,}R} - f(t) \left( \theta_{\text{Srp,}R} - \theta_{\text{Sun,}R} \right)
+where :math:`\boldsymbol{r}_{O/C}` is the location of the array center of pressure with recpect to the system CM, :math:`\boldsymbol{\hat{r}}_\text{S}` is the direction of the Sun wirh respect to the spacecraft, and :math:`\boldsymbol{H}_\text{RW}` is the net wheel momentum.
 
-to ensure that both arrays remain, on average, pointed at the Sun. When one array has the ability to generate a lot of dumping torque (:math:`f(t) \rightarrow -1`), its reference angle is skewed towards :math:`\theta_{\text{Srp,}R}`. Conversely, when :math:`f(t) \rightarrow 0` and there is not much momentum dumping capability, the array remains close to the maximum power-generating angle :math:`\theta_{\text{Sun,}R}`.
+The parameter :math:`\Theta_\text{max}` allows to specify a maximum deflection of the array with respect to the Sun, in order to prevent it from ever reaching an edge-on configuration. The parameter :math:`\sigma` is a tuning gain, whether the exponent :math:`n > 1` allows to introduce a deadband around the zero rotation to avoid chattering.
 
-For more details on the mathematical derivation, see R. Calaon, C. Allard and H. Schaub, "Momentum Management of a Spacecraft equipped with a Dual-Gimballed Electric Thruster", currently in preparation for submission to the Journal of Spacecraft and Rockets.
+For more details on the mathematical derivation and stability considerations, see R. Calaon, C. Allard and H. Schaub, "Momentum Management of a Spacecraft equipped with a Dual-Gimballed Electric Thruster", currently in preparation for submission to the Journal of Spacecraft and Rockets.
 
 
 User Guide
@@ -115,6 +105,9 @@ The required module configuration is::
     saReference.a2Hat_B = [0, 0, 1]
     saReference.attitudeFrame = 0
     saReference.pointingMode = 0
+    saReference.ThetaMax = np.pi/2
+    saReference.sigma = 1
+    saReference.n = 1
     unitTestSim.AddModelToTask(unitTaskName, saReference)
 
 The module is configurable with the following parameters:
@@ -133,3 +126,9 @@ The module is configurable with the following parameters:
      - 0 for reference angle computed w.r.t reference frame; 1 for reference angle computed w.r.t. body frame; defaults to 0 if not specified
    * - ``pointingMode``
      - 0 for maximum power generation; 1 maximum momentum dumping; defaults to 0 if not specified
+   * - ``ThetaMax``
+     - between 0 and Pi
+   * - ``sigma``
+     - tuning gain; setting to zero removes momentum management capability
+   * - ``n``
+     - n > 1 introduces a deadband around zero rotation.