From 8017c520f36bbaaeffcd2386450c1afdce440265 Mon Sep 17 00:00:00 2001 From: Riccardo Date: Tue, 12 Nov 2024 00:55:25 +0100 Subject: [PATCH 1/3] Updated module --- .../solarArrayReference/solarArrayReference.c | 64 ++----------------- .../solarArrayReference/solarArrayReference.h | 8 +-- 2 files changed, 10 insertions(+), 62 deletions(-) diff --git a/src/fswAlgorithms/effectorInterfaces/solarArrayReference/solarArrayReference.c b/src/fswAlgorithms/effectorInterfaces/solarArrayReference/solarArrayReference.c index 071074bcc7..8f2cb2f6c5 100644 --- a/src/fswAlgorithms/effectorInterfaces/solarArrayReference/solarArrayReference.c +++ b/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; -} diff --git a/src/fswAlgorithms/effectorInterfaces/solarArrayReference/solarArrayReference.h b/src/fswAlgorithms/effectorInterfaces/solarArrayReference/solarArrayReference.h index 808630f35b..8b7f2d4c87 100644 --- a/src/fswAlgorithms/effectorInterfaces/solarArrayReference/solarArrayReference.h +++ b/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 From 25e0dcb651ac0953dd0d00ed6d8c5c610b2a2f52 Mon Sep 17 00:00:00 2001 From: Riccardo Date: Tue, 24 Dec 2024 15:07:15 +0100 Subject: [PATCH 2/3] Updated UnitTest --- .../_UnitTest/test_solarArrayReference.py | 36 +++++++++---------- 1 file changed, 16 insertions(+), 20 deletions(-) diff --git a/src/fswAlgorithms/effectorInterfaces/solarArrayReference/_UnitTest/test_solarArrayReference.py b/src/fswAlgorithms/effectorInterfaces/solarArrayReference/_UnitTest/test_solarArrayReference.py index cafe16a232..7373f0f8a4 100644 --- a/src/fswAlgorithms/effectorInterfaces/solarArrayReference/_UnitTest/test_solarArrayReference.py +++ b/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): From 3b61a2ee82a7fd22b044fe03604858442df2080c Mon Sep 17 00:00:00 2001 From: Riccardo Date: Tue, 24 Dec 2024 15:57:21 +0100 Subject: [PATCH 3/3] Updated documentation and release notes --- docs/source/Support/bskReleaseNotes.rst | 1 + .../solarArrayReference.rst | 37 +++++++++---------- 2 files changed, 19 insertions(+), 19 deletions(-) diff --git a/docs/source/Support/bskReleaseNotes.rst b/docs/source/Support/bskReleaseNotes.rst index a9a29995fc..eecfcc6d63 100644 --- a/docs/source/Support/bskReleaseNotes.rst +++ b/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) diff --git a/src/fswAlgorithms/effectorInterfaces/solarArrayReference/solarArrayReference.rst b/src/fswAlgorithms/effectorInterfaces/solarArrayReference/solarArrayReference.rst index 6df7ee7359..186ed39670 100644 --- a/src/fswAlgorithms/effectorInterfaces/solarArrayReference/solarArrayReference.rst +++ b/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.