diff --git a/src/fswAlgorithms/effectorInterfaces/thrMomentumDumping/_UnitTest/test_thrMomentumDumping.py b/src/fswAlgorithms/effectorInterfaces/thrMomentumDumping/_UnitTest/test_thrMomentumDumping.py index 66f022812..005ef4f62 100755 --- a/src/fswAlgorithms/effectorInterfaces/thrMomentumDumping/_UnitTest/test_thrMomentumDumping.py +++ b/src/fswAlgorithms/effectorInterfaces/thrMomentumDumping/_UnitTest/test_thrMomentumDumping.py @@ -25,6 +25,7 @@ import inspect import os +import numpy as np import pytest @@ -46,23 +47,17 @@ # Provide a unique test method name, starting with 'test_'. # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("resetCheck, largeMinFireTime", [ - (False, False) - ,(True, False) - ,(False, True) +@pytest.mark.parametrize("resetCheck, largeMinFireTime, maxNumOfDtFiringTimes", [ + (False, False, False) + ,(True, False, False) + ,(False, True, False) + ,(False, False, True) ]) # update "module" in this function name to reflect the module name -def test_thrMomentumDumping(show_plots, resetCheck, largeMinFireTime): +def test_thrMomentumDumping(show_plots, resetCheck, largeMinFireTime, maxNumOfDtFiringTimes): """Module Unit Test""" # each test method requires a single assert method to be called - [testResults, testMessage] = thrMomentumDumpingTestFunction(show_plots, resetCheck, largeMinFireTime) - assert testResults < 1, testMessage - - -def thrMomentumDumpingTestFunction(show_plots, resetCheck, largeMinFireTime): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages unitTaskName = "unitTask" # arbitrary name (don't change) unitProcessName = "TestProcess" # arbitrary name (don't change) @@ -70,7 +65,12 @@ def thrMomentumDumpingTestFunction(show_plots, resetCheck, largeMinFireTime): unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread - testProcessRate = macros.sec2nano(0.5) # update process rate update time + if maxNumOfDtFiringTimes: + testProcessRate = macros.sec2nano(0.1) # update process rate update times + simTime = 2.0 + else: + testProcessRate = macros.sec2nano(0.5) # update process rate update time + simTime = 3.0 testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -83,7 +83,12 @@ def thrMomentumDumpingTestFunction(show_plots, resetCheck, largeMinFireTime): unitTestSim.AddModelToTask(unitTaskName, module) # Initialize the test module configuration data - module.maxCounterValue = 2 + if maxNumOfDtFiringTimes: + module.maxCounterValue = 5 + module.maxNumOfDtFiringTimes = 3 + else: + module.maxCounterValue = 2 + if largeMinFireTime: module.thrMinFireTime = 0.200 # seconds else: @@ -149,7 +154,7 @@ def thrMomentumDumpingTestFunction(show_plots, resetCheck, largeMinFireTime): # write the input Delta_H message deltaHInMsg.write(DeltaHInMsgData, macros.sec2nano(0.5)) - unitTestSim.ConfigureStopTime(macros.sec2nano(3.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(simTime)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -197,7 +202,7 @@ def thrMomentumDumpingTestFunction(show_plots, resetCheck, largeMinFireTime): [0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.3, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] ] - else: + elif not largeMinFireTime and not maxNumOfDtFiringTimes: trueVector = [ [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], @@ -207,29 +212,33 @@ def thrMomentumDumpingTestFunction(show_plots, resetCheck, largeMinFireTime): [0.1, 0.0, 0.0, 0.3, 0.1, 0.0, 0.3, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] ] + else: + trueVector = [ + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.3, 0.1, 0.0, 0.3, 0.3, 0.1, 0.3, 0.0], + [0.3, 0.0, 0.0, 0.3, 0.3, 0.0, 0.3, 0.0], + [0.3, 0.0, 0.0, 0.3, 0.3, 0.0, 0.3, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.3, 0.0, 0.0, 0.3, 0.3, 0.0, 0.3, 0.0], + [0.2, 0.0, 0.0, 0.3, 0.2, 0.0, 0.3, 0.0], + [0.1, 0.0, 0.0, 0.3, 0.1, 0.0, 0.3, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.2, 0.0], + [0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.1, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + ] # compare the module results to the truth values - accuracy = 1e-12 - unitTestSupport.writeTeXSnippet("toleranceValue", str(accuracy), path) - - testFailCount, testMessages = unitTestSupport.compareArray(trueVector, moduleOutput, accuracy, - "OnTimeRequest", testFailCount, testMessages) - - snippentName = "passFail" + str(resetCheck) + str(largeMinFireTime) - if testFailCount == 0: - colorText = 'ForestGreen' - print("PASSED: " + module.modelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' - else: - colorText = 'Red' - print("Failed: " + module.modelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' - unitTestSupport.writeTeXSnippet(snippentName, passedText, path) - - # each test method requires a single assert method to be called - # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] - + np.testing.assert_allclose(moduleOutput, trueVector, rtol=0, atol=1e-12, verbose=True) # # This statement below ensures that the unitTestScript can be run as a @@ -239,5 +248,6 @@ def thrMomentumDumpingTestFunction(show_plots, resetCheck, largeMinFireTime): test_thrMomentumDumping( # update "module" in function name True, False, # resetCheck - False # largeMinFireTime + False, # largeMinFireTime + True ) diff --git a/src/fswAlgorithms/effectorInterfaces/thrMomentumDumping/thrMomentumDumping.cpp b/src/fswAlgorithms/effectorInterfaces/thrMomentumDumping/thrMomentumDumping.cpp old mode 100755 new mode 100644 index cb75b4dfc..9be1b6512 --- a/src/fswAlgorithms/effectorInterfaces/thrMomentumDumping/thrMomentumDumping.cpp +++ b/src/fswAlgorithms/effectorInterfaces/thrMomentumDumping/thrMomentumDumping.cpp @@ -22,20 +22,21 @@ */ #include "fswAlgorithms/effectorInterfaces/thrMomentumDumping/thrMomentumDumping.h" -#include "architecture/utilities/macroDefinitions.h" -#include "architecture/utilities/linearAlgebra.h" + #include +#include "architecture/utilities/linearAlgebra.h" +#include "architecture/utilities/macroDefinitions.h" + /*! This method performs a complete reset of the module. Local module variables that retain time varying states between function calls are reset to their default values. @return void @param callTime The clock time at which the function was called (nanoseconds) */ -void ThrMomentumDumping::reset(uint64_t callTime) -{ - THRArrayConfigMsgPayload localThrusterData; /* local copy of the thruster data message */ - CmdTorqueBodyMsgPayload DeltaHInMsg; - int i; +void ThrMomentumDumping::reset(uint64_t callTime) { + THRArrayConfigMsgPayload localThrusterData; /* local copy of the thruster data message */ + CmdTorqueBodyMsgPayload DeltaHInMsg; + int i; /*! - reset the prior time flag state. If set to zero, the control time step is not evaluated on the first function call */ @@ -55,7 +56,7 @@ void ThrMomentumDumping::reset(uint64_t callTime) /*! - read in number of thrusters installed and maximum thrust values */ localThrusterData = this->thrusterConfInMsg(); this->numThrusters = localThrusterData.numThrusters; - for (i=0;inumThrusters;i++) { + for (i = 0; i < this->numThrusters; i++) { this->thrMaxForce[i] = localThrusterData.thrusters[i].maxThrust; } @@ -77,9 +78,12 @@ void ThrMomentumDumping::reset(uint64_t callTime) /*! - perform sanity check that the module maxCounterValue value is set to a positive value */ if (this->maxCounterValue < 1) { - this->bskLogger.bskLog(BSK_WARNING,"The maxCounterValue flag must be set to a positive value."); + this->bskLogger.bskLog(BSK_WARNING, "The maxCounterValue flag must be set to a positive value."); + } + /*! - perform sanity check that the module maxCounterValue value is set to a correct value */ + if (this->maxCounterValue < this->maxNumOfDtFiringTimes) { + this->bskLogger.bskLog(BSK_ERROR, "The maxCounterValue must be greater than maxNumOfDtFiringTimes."); } - } /*! This method reads in the requested thruster impulse message. If it is a new message then a fresh @@ -88,26 +92,27 @@ void ThrMomentumDumping::reset(uint64_t callTime) @return void @param callTime The clock time at which the function was called (nanoseconds) */ -void ThrMomentumDumping::updateState(uint64_t callTime) -{ - double dt; /* [s] control update period */ - double *Delta_P_input; /* [] pointer to vector of requested net thruster impulses */ - double *tOnOut; /* pointer to vector of requested thruster on times per dumping cycle */ - THRArrayOnTimeCmdMsgPayload thrOnTimeOut = {}; /* [] output message container */ - THRArrayCmdForceMsgPayload thrusterImpulseInMsg; /* [] thruster inpulse input message */ - CmdTorqueBodyMsgPayload DeltaHInMsg; /* [] commanded Delta_H input message */ - uint64_t timeOfDeltaHMsg; - int i; +void ThrMomentumDumping::updateState(uint64_t callTime) { + double dt; /* [s] control update period */ + double *Delta_P_input; /* [] pointer to vector of requested net thruster impulses */ + double *tOnOut; /* pointer to vector of requested thruster on times per dumping cycle */ + THRArrayOnTimeCmdMsgPayload thrOnTimeOut = {}; /* [] output message container */ + THRArrayCmdForceMsgPayload thrusterImpulseInMsg; /* [] thruster inpulse input message */ + CmdTorqueBodyMsgPayload DeltaHInMsg; /* [] commanded Delta_H input message */ + uint64_t timeOfDeltaHMsg; + int i; /*! - zero the output array of on-time values */ tOnOut = thrOnTimeOut.OnTimeRequest; /*! - check if this is the first call after reset. If yes, write zero output message and exit */ - if (this->priorTime != 0) { /* don't compute dt if this is the first call after a reset */ + if (this->priorTime != 0) { /* don't compute dt if this is the first call after a reset */ /* - compute control update time */ - dt = (callTime - this->priorTime)*NANO2SEC; - if (dt < 0.0) {dt = 0.0;} /* ensure no negative numbers are used */ + dt = (callTime - this->priorTime) * NANO2SEC; + if (dt < 0.0) { + dt = 0.0; + } /* ensure no negative numbers are used */ /*! - Read the requester thruster impulse input message */ thrusterImpulseInMsg = this->thrusterImpulseInMsg(); @@ -117,56 +122,60 @@ void ThrMomentumDumping::updateState(uint64_t callTime) with current momentum dumping) */ DeltaHInMsg = this->deltaHInMsg(); timeOfDeltaHMsg = this->deltaHInMsg.timeWritten(); - if (this->lastDeltaHInMsgTime == timeOfDeltaHMsg){ + if (this->lastDeltaHInMsgTime == timeOfDeltaHMsg) { /* identical net thruster impulse request case, continue with existing RW momentum dumping */ - if (this->thrDumpingCounter <= 0) { + if (this->thrDumpingCounter <= 0 || + this->thrDumpingCounter > (this->maxCounterValue - this->maxNumOfDtFiringTimes + 1)) { /* time to fire thrusters again */ mCopy(this->thrOnTimeRemaining, 1, this->numThrusters, tOnOut); /* subtract next control period from remaining impulse time */ - for (i=0;inumThrusters;i++) { - if (this->thrOnTimeRemaining[i] >0.0){ + for (i = 0; i < this->numThrusters; i++) { + if (this->thrOnTimeRemaining[i] > 0.0) { this->thrOnTimeRemaining[i] -= dt; } } /* reset the dumping counter */ - this->thrDumpingCounter = this->maxCounterValue; + if (this->thrDumpingCounter <= 0) { + this->thrDumpingCounter = this->maxCounterValue; + } else { + this->thrDumpingCounter -= 1; + } } else { /* no thrusters are firing, giving RWs time to settle attitude */ this->thrDumpingCounter -= 1; } - } else { /* new net thruster impulse request case */ this->lastDeltaHInMsgTime = timeOfDeltaHMsg; mCopy(Delta_P_input, 1, this->numThrusters, this->Delta_p); /* store current Delta_p */ - for (i=0;inumThrusters;i++) { + for (i = 0; i < this->numThrusters; i++) { /* compute net time required to implement requested thruster impulse */ - this->thrOnTimeRemaining[i] = this->Delta_p[i]/this->thrMaxForce[i]; + this->thrOnTimeRemaining[i] = this->Delta_p[i] / this->thrMaxForce[i]; } /* set thruster on time to requested impulse time */ mCopy(this->thrOnTimeRemaining, 1, this->numThrusters, tOnOut); /* reset the dumping counter */ - this->thrDumpingCounter = this->maxCounterValue; + this->thrDumpingCounter = maxCounterValue; /* subtract next control period from remaining impulse time */ - for (i=0;inumThrusters;i++) { + for (i = 0; i < this->numThrusters; i++) { this->thrOnTimeRemaining[i] -= dt; } } /*! - check for negative, saturated firing times or negative remaining times */ - for (i=0;inumThrusters;i++) { + for (i = 0; i < this->numThrusters; i++) { /* if thruster on time is less than the minimum firing time, set thrust time command to zero */ - if (tOnOut[i] < this->thrMinFireTime){ + if (tOnOut[i] < this->thrMinFireTime) { tOnOut[i] = 0.0; } /* if the thruster time remainder is negative, zero out the remainder */ - if (this->thrOnTimeRemaining[i] < 0.0){ + if (this->thrOnTimeRemaining[i] < 0.0) { this->thrOnTimeRemaining[i] = 0.0; } /* if the thruster on time is larger than the control period, set it equal to control period */ - if (tOnOut[i] > dt){ - tOnOut[i] = dt; + if (tOnOut[i] > maxNumOfDtFiringTimes * dt) { + tOnOut[i] = maxNumOfDtFiringTimes * dt; } } } diff --git a/src/fswAlgorithms/effectorInterfaces/thrMomentumDumping/thrMomentumDumping.h b/src/fswAlgorithms/effectorInterfaces/thrMomentumDumping/thrMomentumDumping.h old mode 100755 new mode 100644 index 0db4e4fc7..622812035 --- a/src/fswAlgorithms/effectorInterfaces/thrMomentumDumping/thrMomentumDumping.h +++ b/src/fswAlgorithms/effectorInterfaces/thrMomentumDumping/thrMomentumDumping.h @@ -24,41 +24,41 @@ #include "architecture/_GeneralModuleFiles/sys_model.h" #include "architecture/messaging/messaging.h" -#include "architecture/msgPayloadDefC/THRArrayConfigMsgPayload.h" +#include "architecture/msgPayloadDefC/CmdTorqueBodyMsgPayload.h" #include "architecture/msgPayloadDefC/THRArrayCmdForceMsgPayload.h" +#include "architecture/msgPayloadDefC/THRArrayConfigMsgPayload.h" #include "architecture/msgPayloadDefC/THRArrayOnTimeCmdMsgPayload.h" -#include "architecture/msgPayloadDefC/CmdTorqueBodyMsgPayload.h" - #include "architecture/utilities/bskLogging.h" - - /*! @brief thruster force momentum dumping module configuration message */ class ThrMomentumDumping : public SysModel { -public: + public: void reset(uint64_t callTime) override; void updateState(uint64_t callTime) override; /* declare module private variables */ - int32_t thrDumpingCounter; //!< counter to specify after how many contro period a thruster firing should occur. - double Delta_p[MAX_EFF_CNT]; //!< vector of desired total thruster impulses - uint64_t lastDeltaHInMsgTime; //!< time tag of the last momentum change input message - double thrOnTimeRemaining[MAX_EFF_CNT]; //!< vector of remaining thruster on times - uint64_t priorTime; //!< [ns] Last time the attitude control is called - int numThrusters; //!< number of thrusters installed - double thrMaxForce[MAX_EFF_CNT]; //!< [N] vector of maximum thruster forces + int32_t + thrDumpingCounter; //!< counter to specify after how many contro period a thruster firing should occur. + double Delta_p[MAX_EFF_CNT]; //!< vector of desired total thruster impulses + uint64_t lastDeltaHInMsgTime; //!< time tag of the last momentum change input message + double thrOnTimeRemaining[MAX_EFF_CNT]; //!< vector of remaining thruster on times + uint64_t priorTime; //!< [ns] Last time the attitude control is called + int numThrusters; //!< number of thrusters installed + double thrMaxForce[MAX_EFF_CNT]; //!< [N] vector of maximum thruster forces /* declare module public variables */ - int maxCounterValue; //!< this variable must be set to a non-zero value, indicating how many control periods to wait until the thrusters fire again to dump RW momentum - double thrMinFireTime; //!< [s] smallest thruster firing time + int maxCounterValue; //!< this variable must be set to a non-zero value, indicating how many control periods + //!< to wait until the thrusters fire again to dump RW momentum + double thrMinFireTime; //!< [s] smallest thruster firing time + int maxNumOfDtFiringTimes = 1; /* declare module IO interfaces */ - Message thrusterOnTimeOutMsg; //!< thruster on time output message name - ReadFunctor thrusterImpulseInMsg; //!< desired thruster impulse input message name - ReadFunctor thrusterConfInMsg; //!< The name of the thruster configuration Input message - ReadFunctor deltaHInMsg; //!< The name of the requested momentum change input message + Message thrusterOnTimeOutMsg; //!< thruster on time output message name + ReadFunctor thrusterImpulseInMsg; //!< desired thruster impulse input message name + ReadFunctor thrusterConfInMsg; //!< The name of the thruster configuration Input message + ReadFunctor deltaHInMsg; //!< The name of the requested momentum change input message - BSKLogger bskLogger={}; //!< BSK Logging + BSKLogger bskLogger = {}; //!< BSK Logging }; #endif