Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Feature/1412 remove limit on thruster on time #380

Merged
merged 4 commits into from
Apr 2, 2025
Merged

Feature/1412 remove limit on thruster on time #380

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
a261a8b
Pre-commit changes
Mar 14, 2025
0641fa3
Add logic to increase fire time
Mar 17, 2025
d0c5d76
Replace custom functions with numpy testing
Mar 17, 2025
5486f1a
Add test for maxNumOfDtFiringTimes
Mar 17, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
86 changes: 48 additions & 38 deletions src/fswAlgorithms/effectorInterfaces/thrMomentumDumping/_UnitTest/test_thrMomentumDumping.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@

import inspect
import os
import numpy as np

import pytest

Expand All @@ -46,31 +47,30 @@
# 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)

# Create a sim module as an empty container
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))

Expand All @@ -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:
Expand Down Expand Up @@ -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()
Expand Down Expand Up @@ -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],
Expand All @@ -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
Expand All @@ -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
)
85 changes: 47 additions & 38 deletions src/fswAlgorithms/effectorInterfaces/thrMomentumDumping/thrMomentumDumping.cpp
100755 → 100644
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,20 +22,21 @@
*/

#include "fswAlgorithms/effectorInterfaces/thrMomentumDumping/thrMomentumDumping.h"
#include "architecture/utilities/macroDefinitions.h"
#include "architecture/utilities/linearAlgebra.h"

#include <string.h>

#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 */
Expand All @@ -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;i<this->numThrusters;i++) {
for (i = 0; i < this->numThrusters; i++) {
this->thrMaxForce[i] = localThrusterData.thrusters[i].maxThrust;
}

Expand All @@ -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
Expand All @@ -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();
Expand All @@ -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;i<this->numThrusters;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;i<this->numThrusters;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;i<this->numThrusters;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;i<this->numThrusters;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;
}
}
}
Expand Down
40 changes: 20 additions & 20 deletions src/fswAlgorithms/effectorInterfaces/thrMomentumDumping/thrMomentumDumping.h
100755 → 100644
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<THRArrayOnTimeCmdMsgPayload> thrusterOnTimeOutMsg; //!< thruster on time output message name
ReadFunctor<THRArrayCmdForceMsgPayload> thrusterImpulseInMsg; //!< desired thruster impulse input message name
ReadFunctor<THRArrayConfigMsgPayload> thrusterConfInMsg; //!< The name of the thruster configuration Input message
ReadFunctor<CmdTorqueBodyMsgPayload> deltaHInMsg; //!< The name of the requested momentum change input message
Message<THRArrayOnTimeCmdMsgPayload> thrusterOnTimeOutMsg; //!< thruster on time output message name
ReadFunctor<THRArrayCmdForceMsgPayload> thrusterImpulseInMsg; //!< desired thruster impulse input message name
ReadFunctor<THRArrayConfigMsgPayload> thrusterConfInMsg; //!< The name of the thruster configuration Input message
ReadFunctor<CmdTorqueBodyMsgPayload> deltaHInMsg; //!< The name of the requested momentum change input message

BSKLogger bskLogger={}; //!< BSK Logging
BSKLogger bskLogger = {}; //!< BSK Logging
};

#endif