Refactor/358 fix dcm facet srp dynamic effector

src/simulation/dynamics/facetSRPDynamicEffector/_UnitTest/test_unitFacetSRPDynamicEffector.py

@@ -291,11 +291,11 @@ def checkFacetSRPForce(index, facetRotAngle1, facetRotAngle2, area, specCoeff, d
 
     # Rotate the articulated facet normal vectors
     if (index == 6 or index == 7):
-        prv_BB0 = facetRotAngle1 * facetRotAxis
+        prv_BB0 = -facetRotAngle1 * facetRotAxis
         dcm_BB0 = rbk.PRV2C(prv_BB0)
         facetNormal = np.matmul(dcm_BB0, facetNormal)
     if (index == 8 or index == 9):
-        prv_BB0 = facetRotAngle2 * facetRotAxis
+        prv_BB0 = -facetRotAngle2 * facetRotAxis
         dcm_BB0 = rbk.PRV2C(prv_BB0)
         facetNormal = np.matmul(dcm_BB0, facetNormal)
 

src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.cpp

@@ -18,13 +18,15 @@
  */
 
 #include "facetSRPDynamicEffector.h"
-#include "architecture/utilities/rigidBodyKinematics.h"
-#include "architecture/utilities/avsEigenSupport.h"
+
 #include <cmath>
 
+#include "architecture/utilities/avsEigenSupport.h"
+#include "architecture/utilities/rigidBodyKinematics.h"
+
 const double speedLight = 299792458.0;  // [m/s] Speed of light
-const double AstU = 149597870700.0;  // [m] Astronomical unit
-const double solarRadFlux = 1368.0;  // [W/m^2] Solar radiation flux at 1 AU
+const double AstU = 149597870700.0;     // [m] Astronomical unit
+const double solarRadFlux = 1368.0;     // [W/m^2] Solar radiation flux at 1 AU
 
 /*! The constructor */
 FacetSRPDynamicEffector::FacetSRPDynamicEffector() {
@@ -36,8 +38,7 @@ FacetSRPDynamicEffector::FacetSRPDynamicEffector() {
 }
 
 /*! The destructor */
-FacetSRPDynamicEffector::~FacetSRPDynamicEffector() {
-}
+FacetSRPDynamicEffector::~FacetSRPDynamicEffector() {}
 
 /*! The reset method
  @return void
@@ -77,7 +78,7 @@ articulatedFacetDataInMsgs input message
  @return void
  @param tmpMsg  hingedRigidBody input message containing facet articulation angle data
 */
-void FacetSRPDynamicEffector::addArticulatedFacet(Message<HingedRigidBodyMsgPayload> *tmpMsg) {
+void FacetSRPDynamicEffector::addArticulatedFacet(Message<HingedRigidBodyMsgPayload>* tmpMsg) {
     this->articulatedFacetDataInMsgs.push_back(tmpMsg->addSubscriber());
 }
 
@@ -95,8 +96,7 @@ void FacetSRPDynamicEffector::linkInStates(DynParamManager& states) {
 the articulation angle messages are also read
  @return void
 */
-void FacetSRPDynamicEffector::ReadMessages()
-{
+void FacetSRPDynamicEffector::ReadMessages() {
     // Read the Sun state input message
     if (this->sunInMsg.isLinked() && this->sunInMsg.isWritten()) {
         SpicePlanetStateMsgPayload sunMsgBuffer;
@@ -111,10 +111,10 @@ void FacetSRPDynamicEffector::ReadMessages()
         this->facetArticulationAngleList.clear();
         for (int i = 0; i < this->numArticulatedFacets; i++) {
             if (this->articulatedFacetDataInMsgs[i].isLinked() && this->articulatedFacetDataInMsgs[i].isWritten()) {
-                    facetAngleMsg = this->articulatedFacetDataInMsgs[i]();
-                    this->facetArticulationAngleList.push_back(facetAngleMsg.theta);
-                    this->facetAngleMsgRead = true;
-                } else {
+                facetAngleMsg = this->articulatedFacetDataInMsgs[i]();
+                this->facetArticulationAngleList.push_back(facetAngleMsg.theta);
+                this->facetAngleMsgRead = true;
+            } else {
                 this->facetAngleMsgRead = false;
             }
         }
@@ -176,9 +176,9 @@ void FacetSRPDynamicEffector::computeForceTorque(double callTime, double timeSte
             double articulationAngle = facetArticulationAngleList.at(articulatedIndex);
 
             // Determine the required DCM that rotates the facet normal vector through the articulation angle
-            double prv_BB0[3] = {articulationAngle * scGeometry.facetRotAxes_B[i][0],
-                                 articulationAngle * scGeometry.facetRotAxes_B[i][1],
-                                 articulationAngle * scGeometry.facetRotAxes_B[i][2]};
+            double prv_BB0[3] = {-articulationAngle * scGeometry.facetRotAxes_B[i][0],
+                                 -articulationAngle * scGeometry.facetRotAxes_B[i][1],
+                                 -articulationAngle * scGeometry.facetRotAxes_B[i][2]};
             PRV2C(prv_BB0, dcmBB0);
             dcm_BB0 = c2DArray2EigenMatrix3d(dcmBB0);
 
@@ -192,11 +192,11 @@ void FacetSRPDynamicEffector::computeForceTorque(double callTime, double timeSte
 
         // Compute the SRP force and torque acting on the facet only if the facet is in view of the Sun
         if (projectedArea > 0.0) {
-            facetSRPForcePntB_B = -SRPPressure * projectedArea
-                                  * ((1 - this->scGeometry.facetSpecCoeffs[i])
-                                  * sHat + 2 * ((this->scGeometry.facetDiffCoeffs[i] / 3)
-                                  + this->scGeometry.facetSpecCoeffs[i] * cosTheta)
-                                  * this->scGeometry.facetNormals_B[i]);
+            facetSRPForcePntB_B =
+                -SRPPressure * projectedArea *
+                ((1 - this->scGeometry.facetSpecCoeffs[i]) * sHat +
+                 2 * ((this->scGeometry.facetDiffCoeffs[i] / 3) + this->scGeometry.facetSpecCoeffs[i] * cosTheta) *
+                     this->scGeometry.facetNormals_B[i]);
             facetSRPTorquePntB_B = this->scGeometry.facetLocationsPntB_B[i].cross(facetSRPForcePntB_B);
 
             // Add the facet contribution to the total SRP force and torque acting on the spacecraft

src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.h

@@ -22,55 +22,59 @@
 
 #include <Eigen/Dense>
 #include <vector>
-#include "simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h"
+
 #include "architecture/_GeneralModuleFiles/sys_model.h"
-#include "architecture/utilities/bskLogging.h"
 #include "architecture/messaging/messaging.h"
-#include "architecture/msgPayloadDefC/SpicePlanetStateMsgPayload.h"
 #include "architecture/msgPayloadDefC/HingedRigidBodyMsgPayload.h"
-#include "simulation/dynamics/_GeneralModuleFiles/stateData.h"
-#include "architecture/utilities/avsEigenSupport.h"
+#include "architecture/msgPayloadDefC/SpicePlanetStateMsgPayload.h"
 #include "architecture/utilities/avsEigenMRP.h"
+#include "architecture/utilities/avsEigenSupport.h"
+#include "architecture/utilities/bskLogging.h"
+#include "simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h"
+#include "simulation/dynamics/_GeneralModuleFiles/stateData.h"
 
 /*! @brief Spacecraft Geometry Data */
 typedef struct {
-    std::vector<double> facetAreas;                                   //!< [m^2] Vector of facet areas
-    std::vector<double> facetSpecCoeffs;                              //!< Vector of facet spectral reflection optical coefficients
-    std::vector<double> facetDiffCoeffs;                              //!< Vector of facet diffuse reflection optical coefficients
-    std::vector<Eigen::Vector3d> facetNormals_B;                      //!< Vector of facet normals expressed in B frame components
-    std::vector<Eigen::Vector3d> facetLocationsPntB_B;                //!< [m] Vector of facet COP locations wrt point B expressed in B frame components
-    std::vector<Eigen::Vector3d> facetRotAxes_B;                      //!< [m] Vector of facet rotation axes expressed in B frame components
-}FacetedSRPSpacecraftGeometryData;
+    std::vector<double> facetAreas;               //!< [m^2] Vector of facet areas
+    std::vector<double> facetSpecCoeffs;          //!< Vector of facet spectral reflection optical coefficients
+    std::vector<double> facetDiffCoeffs;          //!< Vector of facet diffuse reflection optical coefficients
+    std::vector<Eigen::Vector3d> facetNormals_B;  //!< Vector of facet normals expressed in B frame components
+    std::vector<Eigen::Vector3d>
+        facetLocationsPntB_B;  //!< [m] Vector of facet COP locations wrt point B expressed in B frame components
+    std::vector<Eigen::Vector3d> facetRotAxes_B;  //!< [m] Vector of facet rotation axes expressed in B frame components
+} FacetedSRPSpacecraftGeometryData;
 
 /*! @brief Faceted Solar Radiation Pressure Dynamic Effector */
-class FacetSRPDynamicEffector: public SysModel, public DynamicEffector {
-public:
-    FacetSRPDynamicEffector();                                                           //!< The module constructor
-    ~FacetSRPDynamicEffector();                                                          //!< The module destructor
-    void linkInStates(DynParamManager& states) override;                                 //!< Method for giving the effector access to the hub states
-    void computeForceTorque(double callTime, double timeStep) override;                  //!< Method for computing the SRP force and torque about point B
-    void reset(uint64_t currentSimNanos) override;                                       //!< Reset method
+class FacetSRPDynamicEffector : public SysModel, public DynamicEffector {
+   public:
+    FacetSRPDynamicEffector();                            //!< The module constructor
+    ~FacetSRPDynamicEffector();                           //!< The module destructor
+    void linkInStates(DynParamManager &states) override;  //!< Method for giving the effector access to the hub states
+    void computeForceTorque(double callTime,
+                            double timeStep) override;  //!< Method for computing the SRP force and torque about point B
+    void reset(uint64_t currentSimNanos) override;      //!< Reset method
     void addFacet(double area,
                   double specCoeff,
                   double diffCoeff,
                   Eigen::Vector3d normal_B,
                   Eigen::Vector3d locationPntB_B,
-                  Eigen::Vector3d rotAxis_B);                                            //!< Method for adding facets to the spacecraft geometry structure
+                  Eigen::Vector3d rotAxis_B);  //!< Method for adding facets to the spacecraft geometry structure
     void addArticulatedFacet(Message<HingedRigidBodyMsgPayload> *tmpMsg);
     void ReadMessages();
 
-    uint64_t numFacets;                                                                  //!< Total number of spacecraft facets
-    uint64_t numArticulatedFacets;                                                       //!< Number of articulated facets
-    ReadFunctor<SpicePlanetStateMsgPayload> sunInMsg;                                    //!< Sun spice ephemeris input message
+    uint64_t numFacets;                                //!< Total number of spacecraft facets
+    uint64_t numArticulatedFacets;                     //!< Number of articulated facets
+    ReadFunctor<SpicePlanetStateMsgPayload> sunInMsg;  //!< Sun spice ephemeris input message
 
-private:
-    std::vector<ReadFunctor<HingedRigidBodyMsgPayload>> articulatedFacetDataInMsgs;      //!< Articulated facet angle data input message
-    std::vector<double> facetArticulationAngleList;                                      //!< [rad] Vector of facet rotation angles
-    FacetedSRPSpacecraftGeometryData scGeometry;                                         //!< Spacecraft facet data structure
-    Eigen::Vector3d r_SN_N;                                                              //!< [m] Sun inertial position vector
-    StateData *hubPosition;                                                              //!< [m] Hub inertial position vector
-    StateData *hubSigma;                                                                 //!< Hub MRP inertial attitude
-    bool facetAngleMsgRead;                                                              //!< Boolean variable signaling that the facet articulation messages are read
+   private:
+    std::vector<ReadFunctor<HingedRigidBodyMsgPayload>>
+        articulatedFacetDataInMsgs;                  //!< Articulated facet angle data input message
+    std::vector<double> facetArticulationAngleList;  //!< [rad] Vector of facet rotation angles
+    FacetedSRPSpacecraftGeometryData scGeometry;     //!< Spacecraft facet data structure
+    Eigen::Vector3d r_SN_N;                          //!< [m] Sun inertial position vector
+    StateData *hubPosition;                          //!< [m] Hub inertial position vector
+    StateData *hubSigma;                             //!< Hub MRP inertial attitude
+    bool facetAngleMsgRead;  //!< Boolean variable signaling that the facet articulation messages are read
 };
 
 #endif

src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.rst

@@ -48,7 +48,7 @@ required to rotate the given facet normal vector through the current facet artic
 principal rotation vector (PRV) transformation where:
 
 .. math::
-    [\mathcal{B}_0\mathcal{B}] = \text{PRV2C}(\phi, \boldsymbol{\hat{a}})
+    [\mathcal{B}\mathcal{B}_0] = \text{PRV2C}(-\phi, \boldsymbol{\hat{a}})
 
 and