Skip to content

Commit

Permalink
Merge pull request #260 from AVSLab/refactor/bsk-244-definitionsAndTo…
Browse files Browse the repository at this point in the history 
…leranceRefactor

Refactor/bsk 244 definitions and tolerance refactor
  • Loading branch information
@rcalaon
rcalaon authored Mar 23, 2023
2 parents 453a0db + 86e673e commit 994e131
Show file tree
Hide file tree
Showing 7 changed files with 54 additions and 50 deletions.
14 changes: 10 additions & 4 deletions ...fswAlgorithms/attGuidance/oneAxisSolarArrayPoint/_UnitTest/test_oneAxisSolarArrayPoint.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,17 +64,18 @@ def computeGamma(alpha, delta):

return gamma

# The 'ang' array spans the interval from 0 to pi. 0 and pi are excluded because
# the code is less accurate around those points; it still provides accurate results at 1e-6
ang = np.linspace(0, np.pi, 5, endpoint=True)

# The 'ang' array spans the interval from 0 to pi. pi is excluded because
# the code is less accurate around this point;
ang = np.linspace(0, np.pi, 5, endpoint=False)
ang = list(ang)

@pytest.mark.parametrize("alpha", ang)
@pytest.mark.parametrize("delta", ang)
@pytest.mark.parametrize("bodyAxisInput", [0,1])
@pytest.mark.parametrize("inertialAxisInput", [0,1,2])
@pytest.mark.parametrize("alignmentPriority", [0,1])
@pytest.mark.parametrize("accuracy", [1e-9])
@pytest.mark.parametrize("accuracy", [1e-12])

def test_oneAxisSolarArrayPointTestFunction(show_plots, alpha, delta, bodyAxisInput, inertialAxisInput, alignmentPriority, accuracy):
r"""
Expand Down Expand Up @@ -247,6 +248,11 @@ def oneAxisSolarArrayPointTestFunction(show_plots, alpha, delta, bodyAxisInput,
"bodyAxisInput = {}, inertialAxisInput = {} and priorityFlag = {}".format(
bodyAxisInput, inertialAxisInput, alignmentPriority))

if testFailCount:
print(testMessages)
else:
print("Unit Test Passed")

return [testFailCount, ''.join(testMessages)]


Expand Down
49 changes: 25 additions & 24 deletions src/fswAlgorithms/attGuidance/oneAxisSolarArrayPoint/oneAxisSolarArrayPoint.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@
#include "architecture/utilities/astroConstants.h"
#include "architecture/utilities/macroDefinitions.h"

const double epsilon = 1e-12; // module tolerance for zero

/*! This method initializes the output messages for this module.
@return void
Expand Down Expand Up @@ -56,7 +57,7 @@ void Reset_oneAxisSolarArrayPoint(OneAxisSolarArrayPointConfig *configData, uint
if (BodyHeadingMsg_C_isLinked(&configData->bodyHeadingInMsg)) {
configData->bodyAxisInput = inputBodyHeadingMsg;
}
else if (v3Norm(configData->h1Hat_B) > ONE_AXIS_SA_EPS) {
else if (v3Norm(configData->h1Hat_B) > epsilon) {
configData->bodyAxisInput = inputBodyHeadingParameter;
}
else {
Expand All @@ -79,7 +80,7 @@ void Reset_oneAxisSolarArrayPoint(OneAxisSolarArrayPointConfig *configData, uint
}
}
else {
if (v3Norm(configData->hHat_N) > ONE_AXIS_SA_EPS) {
if (v3Norm(configData->hHat_N) > epsilon) {
configData->inertialAxisInput = inputInertialHeadingParameter;
}
else {
Expand Down Expand Up @@ -141,7 +142,7 @@ void Update_oneAxisSolarArrayPoint(OneAxisSolarArrayPointConfig *configData, uin

/*! get the second body frame direction */
double a2Hat_B[3];
if (v3Norm(configData->a2Hat_B) > ONE_AXIS_SA_EPS) {
if (v3Norm(configData->a2Hat_B) > epsilon) {
v3Normalize(configData->a2Hat_B, a2Hat_B);
}
else {
Expand All @@ -158,7 +159,7 @@ void Update_oneAxisSolarArrayPoint(OneAxisSolarArrayPointConfig *configData, uin

/*! compute the total rotation DCM */
double RN[3][3];
computeFinalRotation(configData->alignmentPriority, BN, rHat_SB_B, hRefHat_B, hReqHat_B, a1Hat_B, a2Hat_B, RN);
oasapComputeFinalRotation(configData->alignmentPriority, BN, rHat_SB_B, hRefHat_B, hReqHat_B, a1Hat_B, a2Hat_B, RN);

/*! compute the relative rotation DCM and Sun direction in relative frame */
double RB[3][3];
Expand All @@ -170,9 +171,9 @@ void Update_oneAxisSolarArrayPoint(OneAxisSolarArrayPointConfig *configData, uin
double sigma_RN[3];
C2MRP(RN, sigma_RN);

if (v3Norm(configData->h2Hat_B) > ONE_AXIS_SA_EPS) {
if (v3Norm(configData->h2Hat_B) > epsilon) {
// compute second reference frame
computeFinalRotation(configData->alignmentPriority, BN, rHat_SB_B, configData->h2Hat_B, hReqHat_B, a1Hat_B, a2Hat_B, RN);
oasapComputeFinalRotation(configData->alignmentPriority, BN, rHat_SB_B, configData->h2Hat_B, hReqHat_B, a1Hat_B, a2Hat_B, RN);

// compute the relative rotation DCM and Sun direction in relative frame
m33MultM33t(RN, BN, RB);
Expand All @@ -181,7 +182,7 @@ void Update_oneAxisSolarArrayPoint(OneAxisSolarArrayPointConfig *configData, uin

double dotP_1 = v3Dot(rHat_SB_R1, a2Hat_B);
double dotP_2 = v3Dot(rHat_SB_R2, a2Hat_B);
if (dotP_2 > dotP_1 && fabs(dotP_2 - dotP_1) > ONE_AXIS_SA_EPS) {
if (dotP_2 > dotP_1 && fabs(dotP_2 - dotP_1) > epsilon) {
// if second reference frame gives a better result, save this as reference MRP set
C2MRP(RN, sigma_RN);
}
Expand Down Expand Up @@ -259,18 +260,18 @@ void Update_oneAxisSolarArrayPoint(OneAxisSolarArrayPointConfig *configData, uin
}

/*! This helper function computes the first rotation that aligns the body heading with the inertial heading */
void computeFirstRotation(double hRefHat_B[3], double hReqHat_B[3], double R1B[3][3])
void oasapComputeFirstRotation(double hRefHat_B[3], double hReqHat_B[3], double R1B[3][3])
{
/*! compute principal rotation angle (phi) and vector (e_phi) for the first rotation */
double phi = acos( fmin( fmax( v3Dot(hRefHat_B, hReqHat_B), -1 ), 1 ) );
double e_phi[3];
v3Cross(hRefHat_B, hReqHat_B, e_phi);
// If phi = PI, e_phi can be any vector perpendicular to hRefHat_B
if (fabs(phi-MPI) < ONE_AXIS_SA_EPS) {
if (fabs(phi-MPI) < epsilon) {
phi = MPI;
v3Perpendicular(hRefHat_B, e_phi);
}
else if (fabs(phi) < ONE_AXIS_SA_EPS) {
else if (fabs(phi) < epsilon) {
phi = 0;
}
// normalize e_phi
Expand All @@ -283,7 +284,7 @@ void computeFirstRotation(double hRefHat_B[3], double hReqHat_B[3], double R1B[3
}

/*! This helper function computes the second rotation that achieves the best incidence on the solar arrays maintaining the heading alignment */
void computeSecondRotation(double hRefHat_B[3], double rHat_SB_R1[3], double a1Hat_B[3], double a2Hat_B[3], double R2R1[3][3])
void oasapComputeSecondRotation(double hRefHat_B[3], double rHat_SB_R1[3], double a1Hat_B[3], double a2Hat_B[3], double R2R1[3][3])
{
/*! define second rotation vector to coincide with the thrust direction in B coordinates */
double e_psi[3];
Expand All @@ -304,8 +305,8 @@ void computeSecondRotation(double hRefHat_B[3], double rHat_SB_R1[3], double a1H

/*! compute exact solution or best solution depending on Delta */
double t, t1, t2, y, y1, y2, psi;
if (fabs(A) < ONE_AXIS_SA_EPS) {
if (fabs(B) < ONE_AXIS_SA_EPS) {
if (fabs(A) < epsilon) {
if (fabs(B) < epsilon) {
// zero-th order equation has no solution
// the solution of the minimum problem is psi = MPI
psi = MPI;
Expand All @@ -320,7 +321,7 @@ void computeSecondRotation(double hRefHat_B[3], double rHat_SB_R1[3], double a1H
if (Delta < 0) {
// second order equation has no solution
// the solution of the minimum problem is found
if (fabs(B) < ONE_AXIS_SA_EPS) {
if (fabs(B) < epsilon) {
t = 0.0;
}
else {
Expand Down Expand Up @@ -350,10 +351,10 @@ void computeSecondRotation(double hRefHat_B[3], double rHat_SB_R1[3], double a1H

// choose between t1 and t2 according to a2Hat
t = t1;
if (fabs(v3Dot(hRefHat_B, a2Hat_B)-1) > ONE_AXIS_SA_EPS) {
if (fabs(v3Dot(hRefHat_B, a2Hat_B)-1) > epsilon) {
y1 = (E*t1*t1 + F*t1 + G) / (1 + t1*t1);
y2 = (E*t2*t2 + F*t2 + G) / (1 + t2*t2);
if (y2 - y1 > ONE_AXIS_SA_EPS) {
if (y2 - y1 > epsilon) {
t = t2;
}
}
Expand All @@ -368,7 +369,7 @@ void computeSecondRotation(double hRefHat_B[3], double rHat_SB_R1[3], double a1H
}

/*! This helper function computes the third rotation that breaks the heading alignment if needed, to achieve maximum incidence on solar arrays */
void computeThirdRotation(int alignmentPriority, double hRefHat_B[3], double rHat_SB_R2[3], double a1Hat_B[3], double R3R2[3][3])
void oasapComputeThirdRotation(int alignmentPriority, double hRefHat_B[3], double rHat_SB_R2[3], double a1Hat_B[3], double R3R2[3][3])
{
double PRV_theta[3];

Expand All @@ -380,7 +381,7 @@ void computeThirdRotation(int alignmentPriority, double hRefHat_B[3], double rHa
else {
double sTheta = v3Dot(rHat_SB_R2, a1Hat_B);
double theta = asin( fmin( fmax( fabs(sTheta), -1 ), 1 ) );
if (fabs(theta) < ONE_AXIS_SA_EPS) {
if (fabs(theta) < epsilon) {
// if Sun direction and solar array drive are already perpendicular, third rotation is null
for (int i = 0; i < 3; i++) {
PRV_theta[i] = 0;
Expand All @@ -389,7 +390,7 @@ void computeThirdRotation(int alignmentPriority, double hRefHat_B[3], double rHa
else {
// if Sun direction and solar array drive are not perpendicular, project solar array drive a1Hat_B onto perpendicular plane (aPHat_B) and compute third rotation
double e_theta[3], aPHat_B[3];
if (fabs(fabs(theta)-MPI/2) > ONE_AXIS_SA_EPS) {
if (fabs(fabs(theta)-MPI/2) > epsilon) {
for (int i = 0; i < 3; i++) {
aPHat_B[i] = (a1Hat_B[i] - sTheta * rHat_SB_R2[i]) / (1 - sTheta * sTheta);
}
Expand All @@ -398,7 +399,7 @@ void computeThirdRotation(int alignmentPriority, double hRefHat_B[3], double rHa
else {
// rotate about the axis that minimizes variation in hRefHat_B direction
v3Cross(rHat_SB_R2, hRefHat_B, aPHat_B);
if (v3Norm(aPHat_B) < ONE_AXIS_SA_EPS) {
if (v3Norm(aPHat_B) < epsilon) {
v3Perpendicular(rHat_SB_R2, aPHat_B);
}
v3Cross(a1Hat_B, aPHat_B, e_theta);
Expand All @@ -413,27 +414,27 @@ void computeThirdRotation(int alignmentPriority, double hRefHat_B[3], double rHa
}

/*! This helper function computes the final rotation as a product of the first three DCMs */
void computeFinalRotation(int alignmentPriority, double BN[3][3], double rHat_SB_B[3], double hRefHat_B[3], double hReqHat_B[3], double a1Hat_B[3], double a2Hat_B[3], double RN[3][3])
void oasapComputeFinalRotation(int alignmentPriority, double BN[3][3], double rHat_SB_B[3], double hRefHat_B[3], double hReqHat_B[3], double a1Hat_B[3], double a2Hat_B[3], double RN[3][3])
{
/*! compute the first rotation DCM */
double R1B[3][3];
computeFirstRotation(hRefHat_B, hReqHat_B, R1B);
oasapComputeFirstRotation(hRefHat_B, hReqHat_B, R1B);

/*! compute Sun direction vector in R1 frame coordinates */
double rHat_SB_R1[3];
m33MultV3(R1B, rHat_SB_B, rHat_SB_R1);

/*! compute the second rotation DCM */
double R2R1[3][3];
computeSecondRotation(hRefHat_B, rHat_SB_R1, a1Hat_B, a2Hat_B, R2R1);
oasapComputeSecondRotation(hRefHat_B, rHat_SB_R1, a1Hat_B, a2Hat_B, R2R1);

/* compute Sun direction in R2 frame components */
double rHat_SB_R2[3];
m33MultV3(R2R1, rHat_SB_R1, rHat_SB_R2);

/*! compute the third rotation DCM */
double R3R2[3][3];
computeThirdRotation(alignmentPriority, hRefHat_B, rHat_SB_R2, a1Hat_B, R3R2);
oasapComputeThirdRotation(alignmentPriority, hRefHat_B, rHat_SB_R2, a1Hat_B, R3R2);

/*! compute reference frames w.r.t inertial frame */
double R1N[3][3], R2N[3][3];
Expand Down
9 changes: 4 additions & 5 deletions src/fswAlgorithms/attGuidance/oneAxisSolarArrayPoint/oneAxisSolarArrayPoint.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,6 @@
#include "cMsgCInterface/EphemerisMsg_C.h"
#include "cMsgCInterface/NavAttMsg_C.h"

#define ONE_AXIS_SA_EPS 1e-12 //!< accuracy to check if a variable is zero

typedef enum alignmentPriority{
prioritizeAxisAlignment = 0,
Expand Down Expand Up @@ -87,10 +86,10 @@ extern "C" {
void Reset_oneAxisSolarArrayPoint(OneAxisSolarArrayPointConfig *configData, uint64_t callTime, int64_t moduleID);
void Update_oneAxisSolarArrayPoint(OneAxisSolarArrayPointConfig *configData, uint64_t callTime, int64_t moduleID);

void computeFirstRotation(double hRefHat_B[3], double hReqHat_B[3], double R1B[3][3]);
void computeSecondRotation(double hRefHat_B[3], double rHat_SB_R1[3], double a1Hat_B[3], double a2Hat_B[3], double R2R1[3][3]);
void computeThirdRotation(int alignmentPriority, double hRefHat_B[3], double rHat_SB_R2[3], double a1Hat_B[3], double R3R2[3][3]);
void computeFinalRotation(int alignmentPriority, double BN[3][3], double rHat_SB_B[3], double hRefHat_B[3], double hReqHat_B[3], double a1Hat_B[3], double a2Hat_B[3], double RN[3][3]);
void oasapComputeFirstRotation(double hRefHat_B[3], double hReqHat_B[3], double R1B[3][3]);
void oasapComputeSecondRotation(double hRefHat_B[3], double rHat_SB_R1[3], double a1Hat_B[3], double a2Hat_B[3], double R2R1[3][3]);
void oasapComputeThirdRotation(int alignmentPriority, double hRefHat_B[3], double rHat_SB_R2[3], double a1Hat_B[3], double R3R2[3][3]);
void oasapComputeFinalRotation(int alignmentPriority, double BN[3][3], double rHat_SB_B[3], double hRefHat_B[3], double hReqHat_B[3], double a1Hat_B[3], double a2Hat_B[3], double RN[3][3]);

#ifdef __cplusplus
}
Expand Down
5 changes: 2 additions & 3 deletions src/fswAlgorithms/effectorInterfaces/solarArrayReference/solarArrayReference.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,13 +22,12 @@
#include "string.h"
#include <math.h>


/* Support files. Be sure to use the absolute path relative to Basilisk directory. */
#include "architecture/utilities/linearAlgebra.h"
#include "architecture/utilities/rigidBodyKinematics.h"
#include "architecture/utilities/astroConstants.h"
#include "architecture/utilities/macroDefinitions.h"

const double epsilon = 1e-12; // module tolerance for zero

/*! This method initializes the output messages for this module.
@return void
Expand Down Expand Up @@ -124,7 +123,7 @@ void Update_solarArrayReference(solarArrayReferenceConfig *configData, uint64_t
double thetaC = atan2(sinThetaC, cosThetaC); // clip theta current between 0 and 2*pi

/*! compute reference angle and store in buffer msg */
if (v3Norm(a2Hat_R) < SA_REF_EPS) {
if (v3Norm(a2Hat_R) < epsilon) {
// if norm(a2Hat_R) = 0, reference coincides with current angle
hingedRigidBodyRefOut.theta = hingedRigidBodyIn.theta;
}
Expand Down
1 change: 0 additions & 1 deletion src/fswAlgorithms/effectorInterfaces/solarArrayReference/solarArrayReference.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,6 @@
#include "cMsgCInterface/AttRefMsg_C.h"
#include "cMsgCInterface/HingedRigidBodyMsg_C.h"

#define SA_REF_EPS 1e-12 //!< accuracy to check if a variable is zero

enum attitudeFrame{
referenceFrame = 0,
Expand Down
18 changes: 9 additions & 9 deletions src/fswAlgorithms/effectorInterfaces/thrusterPlatformReference/thrusterPlatformReference.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ void Update_thrusterPlatformReference(ThrusterPlatformReferenceConfig *configDat
v3Add(configData->r_FM_F, configData->r_TF_F, r_TM_F); // position of T w.r.t. M in F-frame coordinates

double FM[3][3];
computeFinalRotation(r_CM_M, r_TM_F, configData->T_F, FM);
tprComputeFinalRotation(r_CM_M, r_TM_F, configData->T_F, FM);

/*! compute net RW momentum */
double vec3[3], hs_B[3], hs_M[3];
Expand All @@ -115,7 +115,7 @@ void Update_thrusterPlatformReference(ThrusterPlatformReferenceConfig *configDat
/*! recompute thrust direction and FM matrix based on offset */
double r_CMd_M[3];
v3Add(r_CM_M, d_M, r_CMd_M);
computeFinalRotation(r_CMd_M, r_TM_F, configData->T_F, FM);
tprComputeFinalRotation(r_CMd_M, r_TM_F, configData->T_F, FM);

/*! extract theta1 and theta2 angles */
hingedRigidBodyRef1Out.theta = atan2(FM[1][2], FM[1][1]);
Expand Down Expand Up @@ -151,7 +151,7 @@ void Update_thrusterPlatformReference(ThrusterPlatformReferenceConfig *configDat
CmdTorqueBodyMsg_C_write(&thrusterTorqueOut, &configData->thrusterTorqueOutMsg, moduleID, callTime);
}

void computeFirstRotation(double THat_F[3], double rHat_CM_F[3], double F1M[3][3])
void tprComputeFirstRotation(double THat_F[3], double rHat_CM_F[3], double F1M[3][3])
{
// compute principal rotation angle phi
double phi = acos( fmin( fmax( v3Dot(THat_F, rHat_CM_F), -1 ), 1 ) );
Expand Down Expand Up @@ -190,7 +190,7 @@ void computeFirstRotation(double THat_F[3], double rHat_CM_F[3], double F1M[3][3
PRV2C(PRV_phi, F1M);
}

void computeSecondRotation(double r_CM_F[3], double r_TM_F[3], double r_CT_F[3], double THat_F[3], double F2F1[3][3])
void tprComputeSecondRotation(double r_CM_F[3], double r_TM_F[3], double r_CT_F[3], double THat_F[3], double F2F1[3][3])
{
// define offset vector aVec
double aVec[3];
Expand Down Expand Up @@ -231,7 +231,7 @@ void computeSecondRotation(double r_CM_F[3], double r_TM_F[3], double r_CT_F[3],
PRV2C(PRV_psi, F2F1);
}

void computeThirdRotation(double e_theta[3], double F2M[3][3], double F3F2[3][3])
void tprComputeThirdRotation(double e_theta[3], double F2M[3][3], double F3F2[3][3])
{
double e1 = e_theta[0];
double e2 = e_theta[1];
Expand Down Expand Up @@ -301,7 +301,7 @@ void computeThirdRotation(double e_theta[3], double F2M[3][3], double F3F2[3][3]
PRV2C(PRV_theta, F3F2);
}

void computeFinalRotation(double r_CM_M[3], double r_TM_F[3], double T_F[3], double FM[3][3])
void tprComputeFinalRotation(double r_CM_M[3], double r_TM_F[3], double T_F[3], double FM[3][3])
{
/*! define unit vectors of CM direction in M coordinates and thrust direction in F coordinates */
double rHat_CM_M[3];
Expand All @@ -313,7 +313,7 @@ void computeFinalRotation(double r_CM_M[3], double r_TM_F[3], double T_F[3], dou

/*! compute first rotation to make T_F parallel to r_CM */
double F1M[3][3];
computeFirstRotation(THat_F, rHat_CM_F, F1M);
tprComputeFirstRotation(THat_F, rHat_CM_F, F1M);

/*! rotate r_CM_F */
double r_CM_F[3];
Expand All @@ -325,7 +325,7 @@ void computeFinalRotation(double r_CM_M[3], double r_TM_F[3], double T_F[3], dou

/*! compute second rotation to zero the offset between T_F and r_CT_F */
double F2F1[3][3];
computeSecondRotation(r_CM_F, r_TM_F, r_CT_F, THat_F, F2F1);
tprComputeSecondRotation(r_CM_F, r_TM_F, r_CT_F, THat_F, F2F1);

/*! define intermediate platform rotation F2M */
double F2M[3][3];
Expand All @@ -336,7 +336,7 @@ void computeFinalRotation(double r_CM_M[3], double r_TM_F[3], double T_F[3], dou
m33MultV3(F2M, r_CM_M, e_theta);
v3Normalize(e_theta, e_theta);
double F3F2[3][3];
computeThirdRotation(e_theta, F2M, F3F2);
tprComputeThirdRotation(e_theta, F2M, F3F2);

/*! define final platform rotation FM */
m33MultM33(F3F2, F2M, FM);
Expand Down
8 changes: 4 additions & 4 deletions src/fswAlgorithms/effectorInterfaces/thrusterPlatformReference/thrusterPlatformReference.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,10 +72,10 @@ extern "C" {
void Reset_thrusterPlatformReference(ThrusterPlatformReferenceConfig *configData, uint64_t callTime, int64_t moduleID);
void Update_thrusterPlatformReference(ThrusterPlatformReferenceConfig *configData, uint64_t callTime, int64_t moduleID);

void computeFirstRotation(double THat_F[3], double rHat_CM_F[3], double F1M[3][3]);
void computeSecondRotation(double r_CM_F[3], double r_TM_F[3], double r_CT_F[3], double T_F_hat[3], double FF1[3][3]);
void computeThirdRotation(double e_theta[3], double F2M[3][3], double F3F2[3][3]);
void computeFinalRotation(double r_CM_M[3], double r_TM_F[3], double T_F[3], double FM[3][3]);
void tprComputeFirstRotation(double THat_F[3], double rHat_CM_F[3], double F1M[3][3]);
void tprComputeSecondRotation(double r_CM_F[3], double r_TM_F[3], double r_CT_F[3], double T_F_hat[3], double FF1[3][3]);
void tprComputeThirdRotation(double e_theta[3], double F2M[3][3], double F3F2[3][3]);
void tprComputeFinalRotation(double r_CM_M[3], double r_TM_F[3], double T_F[3], double FM[3][3]);

#ifdef __cplusplus
}
Expand Down

0 comments on commit 994e131

Please sign in to comment.