Skip to content

Commit 4193636

Browse files
leahkinerleahkiner
authored
Merge pull request #359 from lasp/refactor/358-fix-dcm-facetSRPDynamicEffector
Refactor/358 fix dcm facet srp dynamic effector
2 parents d87af3b + e74f938 commit 4193636

4 files changed

Lines changed: 59 additions & 55 deletions

File tree

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -291,11 +291,11 @@ def checkFacetSRPForce(index, facetRotAngle1, facetRotAngle2, area, specCoeff, d
291291

292292
# Rotate the articulated facet normal vectors
293293
if (index == 6 or index == 7):
294-
prv_BB0 = facetRotAngle1 * facetRotAxis
294+
prv_BB0 = -facetRotAngle1 * facetRotAxis
295295
dcm_BB0 = rbk.PRV2C(prv_BB0)
296296
facetNormal = np.matmul(dcm_BB0, facetNormal)
297297
if (index == 8 or index == 9):
298-
prv_BB0 = facetRotAngle2 * facetRotAxis
298+
prv_BB0 = -facetRotAngle2 * facetRotAxis
299299
dcm_BB0 = rbk.PRV2C(prv_BB0)
300300
facetNormal = np.matmul(dcm_BB0, facetNormal)
301301

‎src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.cpp‎

Lines changed: 21 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -18,13 +18,15 @@
1818
*/
1919

2020
#include "facetSRPDynamicEffector.h"
21-
#include "architecture/utilities/rigidBodyKinematics.h"
22-
#include "architecture/utilities/avsEigenSupport.h"
21+
2322
#include <cmath>
2423

24+
#include "architecture/utilities/avsEigenSupport.h"
25+
#include "architecture/utilities/rigidBodyKinematics.h"
26+
2527
const double speedLight = 299792458.0; // [m/s] Speed of light
26-
const double AstU = 149597870700.0; // [m] Astronomical unit
27-
const double solarRadFlux = 1368.0; // [W/m^2] Solar radiation flux at 1 AU
28+
const double AstU = 149597870700.0; // [m] Astronomical unit
29+
const double solarRadFlux = 1368.0; // [W/m^2] Solar radiation flux at 1 AU
2830

2931
/*! The constructor */
3032
FacetSRPDynamicEffector::FacetSRPDynamicEffector() {
@@ -36,8 +38,7 @@ FacetSRPDynamicEffector::FacetSRPDynamicEffector() {
3638
}
3739

3840
/*! The destructor */
39-
FacetSRPDynamicEffector::~FacetSRPDynamicEffector() {
40-
}
41+
FacetSRPDynamicEffector::~FacetSRPDynamicEffector() {}
4142

4243
/*! The reset method
4344
@return void
@@ -77,7 +78,7 @@ articulatedFacetDataInMsgs input message
7778
@return void
7879
@param tmpMsg hingedRigidBody input message containing facet articulation angle data
7980
*/
80-
void FacetSRPDynamicEffector::addArticulatedFacet(Message<HingedRigidBodyMsgPayload> *tmpMsg) {
81+
void FacetSRPDynamicEffector::addArticulatedFacet(Message<HingedRigidBodyMsgPayload>* tmpMsg) {
8182
this->articulatedFacetDataInMsgs.push_back(tmpMsg->addSubscriber());
8283
}
8384

@@ -95,8 +96,7 @@ void FacetSRPDynamicEffector::linkInStates(DynParamManager& states) {
9596
the articulation angle messages are also read
9697
@return void
9798
*/
98-
void FacetSRPDynamicEffector::ReadMessages()
99-
{
99+
void FacetSRPDynamicEffector::ReadMessages() {
100100
// Read the Sun state input message
101101
if (this->sunInMsg.isLinked() && this->sunInMsg.isWritten()) {
102102
SpicePlanetStateMsgPayload sunMsgBuffer;
@@ -111,10 +111,10 @@ void FacetSRPDynamicEffector::ReadMessages()
111111
this->facetArticulationAngleList.clear();
112112
for (int i = 0; i < this->numArticulatedFacets; i++) {
113113
if (this->articulatedFacetDataInMsgs[i].isLinked() && this->articulatedFacetDataInMsgs[i].isWritten()) {
114-
facetAngleMsg = this->articulatedFacetDataInMsgs[i]();
115-
this->facetArticulationAngleList.push_back(facetAngleMsg.theta);
116-
this->facetAngleMsgRead = true;
117-
} else {
114+
facetAngleMsg = this->articulatedFacetDataInMsgs[i]();
115+
this->facetArticulationAngleList.push_back(facetAngleMsg.theta);
116+
this->facetAngleMsgRead = true;
117+
} else {
118118
this->facetAngleMsgRead = false;
119119
}
120120
}
@@ -176,9 +176,9 @@ void FacetSRPDynamicEffector::computeForceTorque(double callTime, double timeSte
176176
double articulationAngle = facetArticulationAngleList.at(articulatedIndex);
177177

178178
// Determine the required DCM that rotates the facet normal vector through the articulation angle
179-
double prv_BB0[3] = {articulationAngle * scGeometry.facetRotAxes_B[i][0],
180-
articulationAngle * scGeometry.facetRotAxes_B[i][1],
181-
articulationAngle * scGeometry.facetRotAxes_B[i][2]};
179+
double prv_BB0[3] = {-articulationAngle * scGeometry.facetRotAxes_B[i][0],
180+
-articulationAngle * scGeometry.facetRotAxes_B[i][1],
181+
-articulationAngle * scGeometry.facetRotAxes_B[i][2]};
182182
PRV2C(prv_BB0, dcmBB0);
183183
dcm_BB0 = c2DArray2EigenMatrix3d(dcmBB0);
184184

@@ -192,11 +192,11 @@ void FacetSRPDynamicEffector::computeForceTorque(double callTime, double timeSte
192192

193193
// Compute the SRP force and torque acting on the facet only if the facet is in view of the Sun
194194
if (projectedArea > 0.0) {
195-
facetSRPForcePntB_B = -SRPPressure * projectedArea
196-
* ((1 - this->scGeometry.facetSpecCoeffs[i])
197-
* sHat + 2 * ((this->scGeometry.facetDiffCoeffs[i] / 3)
198-
+ this->scGeometry.facetSpecCoeffs[i] * cosTheta)
199-
* this->scGeometry.facetNormals_B[i]);
195+
facetSRPForcePntB_B =
196+
-SRPPressure * projectedArea *
197+
((1 - this->scGeometry.facetSpecCoeffs[i]) * sHat +
198+
2 * ((this->scGeometry.facetDiffCoeffs[i] / 3) + this->scGeometry.facetSpecCoeffs[i] * cosTheta) *
199+
this->scGeometry.facetNormals_B[i]);
200200
facetSRPTorquePntB_B = this->scGeometry.facetLocationsPntB_B[i].cross(facetSRPForcePntB_B);
201201

202202
// Add the facet contribution to the total SRP force and torque acting on the spacecraft

‎src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.h‎

Lines changed: 35 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -22,55 +22,59 @@
2222

2323
#include <Eigen/Dense>
2424
#include <vector>
25-
#include "simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h"
25+
2626
#include "architecture/_GeneralModuleFiles/sys_model.h"
27-
#include "architecture/utilities/bskLogging.h"
2827
#include "architecture/messaging/messaging.h"
29-
#include "architecture/msgPayloadDefC/SpicePlanetStateMsgPayload.h"
3028
#include "architecture/msgPayloadDefC/HingedRigidBodyMsgPayload.h"
31-
#include "simulation/dynamics/_GeneralModuleFiles/stateData.h"
32-
#include "architecture/utilities/avsEigenSupport.h"
29+
#include "architecture/msgPayloadDefC/SpicePlanetStateMsgPayload.h"
3330
#include "architecture/utilities/avsEigenMRP.h"
31+
#include "architecture/utilities/avsEigenSupport.h"
32+
#include "architecture/utilities/bskLogging.h"
33+
#include "simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h"
34+
#include "simulation/dynamics/_GeneralModuleFiles/stateData.h"
3435

3536
/*! @brief Spacecraft Geometry Data */
3637
typedef struct {
37-
std::vector<double> facetAreas; //!< [m^2] Vector of facet areas
38-
std::vector<double> facetSpecCoeffs; //!< Vector of facet spectral reflection optical coefficients
39-
std::vector<double> facetDiffCoeffs; //!< Vector of facet diffuse reflection optical coefficients
40-
std::vector<Eigen::Vector3d> facetNormals_B; //!< Vector of facet normals expressed in B frame components
41-
std::vector<Eigen::Vector3d> facetLocationsPntB_B; //!< [m] Vector of facet COP locations wrt point B expressed in B frame components
42-
std::vector<Eigen::Vector3d> facetRotAxes_B; //!< [m] Vector of facet rotation axes expressed in B frame components
43-
}FacetedSRPSpacecraftGeometryData;
38+
std::vector<double> facetAreas; //!< [m^2] Vector of facet areas
39+
std::vector<double> facetSpecCoeffs; //!< Vector of facet spectral reflection optical coefficients
40+
std::vector<double> facetDiffCoeffs; //!< Vector of facet diffuse reflection optical coefficients
41+
std::vector<Eigen::Vector3d> facetNormals_B; //!< Vector of facet normals expressed in B frame components
42+
std::vector<Eigen::Vector3d>
43+
facetLocationsPntB_B; //!< [m] Vector of facet COP locations wrt point B expressed in B frame components
44+
std::vector<Eigen::Vector3d> facetRotAxes_B; //!< [m] Vector of facet rotation axes expressed in B frame components
45+
} FacetedSRPSpacecraftGeometryData;
4446

4547
/*! @brief Faceted Solar Radiation Pressure Dynamic Effector */
46-
class FacetSRPDynamicEffector: public SysModel, public DynamicEffector {
47-
public:
48-
FacetSRPDynamicEffector(); //!< The module constructor
49-
~FacetSRPDynamicEffector(); //!< The module destructor
50-
void linkInStates(DynParamManager& states) override; //!< Method for giving the effector access to the hub states
51-
void computeForceTorque(double callTime, double timeStep) override; //!< Method for computing the SRP force and torque about point B
52-
void reset(uint64_t currentSimNanos) override; //!< Reset method
48+
class FacetSRPDynamicEffector : public SysModel, public DynamicEffector {
49+
public:
50+
FacetSRPDynamicEffector(); //!< The module constructor
51+
~FacetSRPDynamicEffector(); //!< The module destructor
52+
void linkInStates(DynParamManager &states) override; //!< Method for giving the effector access to the hub states
53+
void computeForceTorque(double callTime,
54+
double timeStep) override; //!< Method for computing the SRP force and torque about point B
55+
void reset(uint64_t currentSimNanos) override; //!< Reset method
5356
void addFacet(double area,
5457
double specCoeff,
5558
double diffCoeff,
5659
Eigen::Vector3d normal_B,
5760
Eigen::Vector3d locationPntB_B,
58-
Eigen::Vector3d rotAxis_B); //!< Method for adding facets to the spacecraft geometry structure
61+
Eigen::Vector3d rotAxis_B); //!< Method for adding facets to the spacecraft geometry structure
5962
void addArticulatedFacet(Message<HingedRigidBodyMsgPayload> *tmpMsg);
6063
void ReadMessages();
6164

62-
uint64_t numFacets; //!< Total number of spacecraft facets
63-
uint64_t numArticulatedFacets; //!< Number of articulated facets
64-
ReadFunctor<SpicePlanetStateMsgPayload> sunInMsg; //!< Sun spice ephemeris input message
65+
uint64_t numFacets; //!< Total number of spacecraft facets
66+
uint64_t numArticulatedFacets; //!< Number of articulated facets
67+
ReadFunctor<SpicePlanetStateMsgPayload> sunInMsg; //!< Sun spice ephemeris input message
6568

66-
private:
67-
std::vector<ReadFunctor<HingedRigidBodyMsgPayload>> articulatedFacetDataInMsgs; //!< Articulated facet angle data input message
68-
std::vector<double> facetArticulationAngleList; //!< [rad] Vector of facet rotation angles
69-
FacetedSRPSpacecraftGeometryData scGeometry; //!< Spacecraft facet data structure
70-
Eigen::Vector3d r_SN_N; //!< [m] Sun inertial position vector
71-
StateData *hubPosition; //!< [m] Hub inertial position vector
72-
StateData *hubSigma; //!< Hub MRP inertial attitude
73-
bool facetAngleMsgRead; //!< Boolean variable signaling that the facet articulation messages are read
69+
private:
70+
std::vector<ReadFunctor<HingedRigidBodyMsgPayload>>
71+
articulatedFacetDataInMsgs; //!< Articulated facet angle data input message
72+
std::vector<double> facetArticulationAngleList; //!< [rad] Vector of facet rotation angles
73+
FacetedSRPSpacecraftGeometryData scGeometry; //!< Spacecraft facet data structure
74+
Eigen::Vector3d r_SN_N; //!< [m] Sun inertial position vector
75+
StateData *hubPosition; //!< [m] Hub inertial position vector
76+
StateData *hubSigma; //!< Hub MRP inertial attitude
77+
bool facetAngleMsgRead; //!< Boolean variable signaling that the facet articulation messages are read
7478
};
7579

7680
#endif

‎src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.rst‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,7 @@ required to rotate the given facet normal vector through the current facet artic
4848
principal rotation vector (PRV) transformation where:
4949

5050
.. math::
51-
[\mathcal{B}_0\mathcal{B}] = \text{PRV2C}(\phi, \boldsymbol{\hat{a}})
51+
[\mathcal{B}\mathcal{B}_0] = \text{PRV2C}(-\phi, \boldsymbol{\hat{a}})
5252
5353
and
5454

0 commit comments

Comments
 (0)