Fixes for Pointing Modes #851

Merged
meggert merged 24 commits from ptg-fixes into main 2024-01-29 17:05:46 +01:00
12 changed files with 115 additions and 107 deletions

View File

@ -16,6 +16,9 @@ will consitute of a breaking change warranting a new major release:
# [unreleased] # [unreleased]
- Bumped `eive-tmtc` to
- Bumped `eive-fsfw`
## Added ## Added
- Added new parameter for MPSoC which allows to skip SUPV commanding. - Added new parameter for MPSoC which allows to skip SUPV commanding.
@ -40,6 +43,13 @@ will consitute of a breaking change warranting a new major release:
`AcsController` being allowed sending invalid speed commands to the RW Handler, which `AcsController` being allowed sending invalid speed commands to the RW Handler, which
would then trigger FDIR and turning off the functioning device would then trigger FDIR and turning off the functioning device
- `RwHandler` returnvalues would use the `INTERFACE_ID` of the `DeviceHandlerBase` - `RwHandler` returnvalues would use the `INTERFACE_ID` of the `DeviceHandlerBase`
- The `AcsController` will reset its stored guidance values on mode change and lost
orientation.
- The nullspace controller will only be used if all RWs are available.
- Calculation of required rotation rate in pointing modes has been fixed to actual
calculation of rotation rate from two quaternions.
- Fixed alignment matrix and pseudo inverses of RWs, to match the wrong definition of
positive rotation.
# [v7.5.5] 2024-01-22 # [v7.5.5] 2024-01-22

2
fsfw

@ -1 +1 @@
Subproject commit e64e8b274d436502d5c5b87865b9006e52e4b1aa Subproject commit b5e7179af1da085b9be598b4897f2664012781af

View File

@ -49,7 +49,7 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
case SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL: { case SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL: {
ReturnValue_t result = guidance.solarArrayDeploymentComplete(); ReturnValue_t result = guidance.solarArrayDeploymentComplete();
if (result == returnvalue::FAILED) { if (result == returnvalue::FAILED) {
return FILE_DELETION_FAILED; return acsctrl::FILE_DELETION_FAILED;
} }
return HasActionsIF::EXECUTION_FINISHED; return HasActionsIF::EXECUTION_FINISHED;
} }
@ -360,6 +360,7 @@ void AcsController::performPointingCtrl() {
triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION); triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION);
ptgCtrlLostCounter = 0; ptgCtrlLostCounter = 0;
} }
guidance.resetValues();
updateCtrlValData(ptgCtrlStrat); updateCtrlValData(ptgCtrlStrat);
updateActuatorCmdData(ZERO_VEC4, cmdSpeedRws, ZERO_VEC3_INT16); updateActuatorCmdData(ZERO_VEC4, cmdSpeedRws, ZERO_VEC3_INT16);
commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
@ -393,9 +394,10 @@ void AcsController::performPointingCtrl() {
break; break;
} }
bool allRwAvailable = true;
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
if (result == returnvalue::FAILED) { if (result == acsctrl::MULTIPLE_RW_UNAVAILABLE) {
if (multipleRwUnavailableCounter >= if (multipleRwUnavailableCounter >=
acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) { acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) {
triggerEvent(acs::MULTIPLE_RW_INVALID); triggerEvent(acs::MULTIPLE_RW_INVALID);
@ -403,8 +405,10 @@ void AcsController::performPointingCtrl() {
} }
multipleRwUnavailableCounter++; multipleRwUnavailableCounter++;
return; return;
} else { }
multipleRwUnavailableCounter = 0; multipleRwUnavailableCounter = 0;
if (result == acsctrl::SINGLE_RW_UNAVAILABLE) {
allRwAvailable = false;
} }
// Variables required for guidance // Variables required for guidance
@ -422,7 +426,7 @@ void AcsController::performPointingCtrl() {
errorSatRotRate, errorAngle); errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate, ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters, ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.idleModeControllerParameters,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
rwTrqNs); rwTrqNs);
@ -445,7 +449,7 @@ void AcsController::performPointingCtrl() {
errorSatRotRate, errorAngle); errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate, ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters, ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.targetModeControllerParameters,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
rwTrqNs); rwTrqNs);
@ -465,7 +469,7 @@ void AcsController::performPointingCtrl() {
errorSatRotRate, errorAngle); errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate, ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters, ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.gsTargetModeControllerParameters,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
rwTrqNs); rwTrqNs);
@ -488,7 +492,7 @@ void AcsController::performPointingCtrl() {
errorSatRotRate, errorAngle); errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate, ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters, ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.nadirModeControllerParameters,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
rwTrqNs); rwTrqNs);
@ -510,7 +514,7 @@ void AcsController::performPointingCtrl() {
errorSatRotRate, errorAngle); errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate, ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters, ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.inertialModeControllerParameters,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
rwTrqNs); rwTrqNs);
@ -899,6 +903,7 @@ void AcsController::modeChanged(Mode_t mode, Submode_t submode) {
rw4SpeedSet.setRwSpeed(0, 10); rw4SpeedSet.setRwSpeed(0, 10);
} }
} }
guidance.resetValues();
return ExtendedControllerBase::modeChanged(mode, submode); return ExtendedControllerBase::modeChanged(mode, submode);
} }
@ -1095,7 +1100,7 @@ ReturnValue_t AcsController::writeTleToFs(const uint8_t *tle) {
tleFile << "\n"; tleFile << "\n";
tleFile.write(reinterpret_cast<const char *>(tle + 69), 69); tleFile.write(reinterpret_cast<const char *>(tle + 69), 69);
} else { } else {
return WRITE_FILE_FAILED; return acsctrl::WRITE_FILE_FAILED;
} }
tleFile.close(); tleFile.close();
return returnvalue::OK; return returnvalue::OK;
@ -1119,12 +1124,12 @@ ReturnValue_t AcsController::readTleFromFs(uint8_t *line1, uint8_t *line2) {
std::memcpy(line2, tleLine2.c_str(), 69); std::memcpy(line2, tleLine2.c_str(), 69);
} else { } else {
triggerEvent(acs::TLE_FILE_READ_FAILED); triggerEvent(acs::TLE_FILE_READ_FAILED);
return READ_FILE_FAILED; return acsctrl::READ_FILE_FAILED;
} }
tleFile.close(); tleFile.close();
} else { } else {
triggerEvent(acs::TLE_FILE_READ_FAILED); triggerEvent(acs::TLE_FILE_READ_FAILED);
return READ_FILE_FAILED; return acsctrl::READ_FILE_FAILED;
} }
return returnvalue::OK; return returnvalue::OK;
} }

View File

@ -112,14 +112,6 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
static const DeviceCommandId_t UPDATE_TLE = 0x3; static const DeviceCommandId_t UPDATE_TLE = 0x3;
static const DeviceCommandId_t READ_TLE = 0x4; static const DeviceCommandId_t READ_TLE = 0x4;
static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL;
//! [EXPORT] : [COMMENT] File deletion failed and at least one file is still existent.
static constexpr ReturnValue_t FILE_DELETION_FAILED = MAKE_RETURN_CODE(0);
//! [EXPORT] : [COMMENT] Writing the TLE to the file has failed.
static constexpr ReturnValue_t WRITE_FILE_FAILED = MAKE_RETURN_CODE(1);
//! [EXPORT] : [COMMENT] Reading the TLE to the file has failed.
static constexpr ReturnValue_t READ_FILE_FAILED = MAKE_RETURN_CODE(2);
ReturnValue_t initialize() override; ReturnValue_t initialize() override;
ReturnValue_t handleCommandMessage(CommandMessage* message) override; ReturnValue_t handleCommandMessage(CommandMessage* message) override;
void performControlOperation() override; void performControlOperation() override;

View File

@ -333,16 +333,16 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->setMatrix(rwMatrices.pseudoInverse); parameterWrapper->setMatrix(rwMatrices.pseudoInverse);
break; break;
case 0x2: case 0x2:
parameterWrapper->setMatrix(rwMatrices.without1); parameterWrapper->setMatrix(rwMatrices.pseudoInverseWithoutRW1);
break; break;
case 0x3: case 0x3:
parameterWrapper->setMatrix(rwMatrices.without2); parameterWrapper->setMatrix(rwMatrices.pseudoInverseWithoutRW2);
break; break;
case 0x4: case 0x4:
parameterWrapper->setMatrix(rwMatrices.without3); parameterWrapper->setMatrix(rwMatrices.pseudoInverseWithoutRW3);
break; break;
case 0x5: case 0x5:
parameterWrapper->setMatrix(rwMatrices.without4); parameterWrapper->setMatrix(rwMatrices.pseudoInverseWithoutRW4);
break; break;
case 0x6: case 0x6:
parameterWrapper->setVector(rwMatrices.nullspaceVector); parameterWrapper->setVector(rwMatrices.nullspaceVector);

View File

@ -812,19 +812,19 @@ class AcsParameters : public HasParametersIF {
} rwHandlingParameters; } rwHandlingParameters;
struct RwMatrices { struct RwMatrices {
double alignmentMatrix[3][4] = {{0.9205, 0.0000, -0.9205, 0.0000}, double alignmentMatrix[3][4] = {{-0.9205, 0.0000, 0.9205, 0.0000},
{0.0000, -0.9205, 0.0000, 0.9205}, {0.0000, 0.9205, 0.0000, -0.9205},
{0.3907, 0.3907, 0.3907, 0.3907}}; {-0.3907, -0.3907, -0.3907, -0.3907}};
double pseudoInverse[4][3] = { double pseudoInverse[4][3] = {
{0.5432, 0, 0.6398}, {0, -0.5432, 0.6398}, {-0.5432, 0, 0.6398}, {0, 0.5432, 0.6398}}; {-0.5432, 0, -0.6399}, {0, 0.5432, -0.6399}, {0.5432, 0, -0.6399}, {0, -0.5432, -0.6399}};
double without1[4][3] = { double pseudoInverseWithoutRW1[4][3] = {
{0, 0, 0}, {0.5432, -0.5432, 1.2797}, {-1.0864, 0, 0}, {0.5432, 0.5432, 1.2797}}; {0, 0, 0}, {-0.5432, 0.5432, -1.2798}, {1.0864, 0, 0}, {-0.5432, -0.5432, -1.2798}};
double without2[4][3] = { double pseudoInverseWithoutRW2[4][3] = {
{0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 1.0864, 0}}; {-0.5432, 0.5432, -1.2798}, {0, 0, 0}, {0.5432, 0.5432, -1.2798}, {0, -1.0864, 0}};
double without3[4][3] = { double pseudoInverseWithoutRW3[4][3] = {
{1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}}; {-1.0864, 0, 0}, {0.5432, 0.5432, -1.2798}, {0, 0, 0}, {0.5432, -0.5432, -1.2798}};
double without4[4][3] = { double pseudoInverseWithoutRW4[4][3] = {
{0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}}; {-0.5432, -0.5432, -1.2798}, {0, 1.0864, 0}, {0.5432, -0.5432, -1.2798}, {0, 0, 0}};
double nullspaceVector[4] = {-1, 1, -1, 1}; double nullspaceVector[4] = {-1, 1, -1, 1};
} rwMatrices; } rwMatrices;

View File

@ -1,15 +1,5 @@
#include "Guidance.h" #include "Guidance.h"
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <mission/controller/acs/util/MathOperations.h>
#include <cmath>
#include <filesystem>
#include <string>
Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
Guidance::~Guidance() {} Guidance::~Guidance() {}
@ -431,7 +421,11 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
errorAngle = QuaternionOperations::getAngle(errorQuat, true); errorAngle = QuaternionOperations::getAngle(errorQuat, true);
// Calculate error satellite rotational rate // Calculate error satellite rotational rate
// First combine the target and reference satellite rotational rates // Convert target rotational rate into body RF
double errorQuatInv[4] = {0, 0, 0, 0}, targetSatRotRateB[3] = {0, 0, 0};
QuaternionOperations::inverse(errorQuat, errorQuatInv);
QuaternionOperations::multiplyVector(errorQuatInv, targetSatRotRate, targetSatRotRateB);
// Combine the target and reference satellite rotational rates
double combinedRefSatRotRate[3] = {0, 0, 0}; double combinedRefSatRotRate[3] = {0, 0, 0};
VectorOperations<double>::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3); VectorOperations<double>::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3);
// Then subtract the combined required satellite rotational rates from the actual rate // Then subtract the combined required satellite rotational rates from the actual rate
@ -455,44 +449,16 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
} }
void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double timeDelta, void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double timeDelta,
double quatInertialTarget[4], double *refSatRate) { double quatIX[4], double *refSatRate) {
//------------------------------------------------------------------------------------- if (VectorOperations<double>::norm(quatIXprev, 4) == 0) {
// Calculation of target rotation rate std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
//-------------------------------------------------------------------------------------
if (VectorOperations<double>::norm(savedQuaternion, 4) == 0) {
std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
} }
if (timeDelta < timeElapsedMax and timeDelta != 0.0) { if (timeDelta != 0.0) {
double q[4] = {0, 0, 0, 0}, qS[4] = {0, 0, 0, 0}; QuaternionOperations::rotationFromQuaternions(quatIX, quatIXprev, timeDelta, refSatRate);
QuaternionOperations::inverse(quatInertialTarget, q);
QuaternionOperations::inverse(savedQuaternion, qS);
double qDiff[4] = {0, 0, 0, 0};
VectorOperations<double>::subtract(q, qS, qDiff, 4);
VectorOperations<double>::mulScalar(qDiff, 1. / timeDelta, qDiff, 4);
double tgtQuatVec[3] = {q[0], q[1], q[2]};
double qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0};
VectorOperations<double>::cross(tgtQuatVec, qDiffVec, sum1);
VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
VectorOperations<double>::mulScalar(qDiffVec, q[3], sum3, 3);
VectorOperations<double>::add(sum1, sum2, sum, 3);
VectorOperations<double>::subtract(sum, sum3, sum, 3);
double omegaRefNew[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(sum, -2, omegaRefNew, 3);
VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3);
VectorOperations<double>::subtract(refSatRate, omegaRefSaved, refSatRate, 3);
omegaRefSaved[0] = omegaRefNew[0];
omegaRefSaved[1] = omegaRefNew[1];
omegaRefSaved[2] = omegaRefNew[2];
} else { } else {
refSatRate[0] = 0; std::memcpy(refSatRate, ZERO_VEC3, 3 * sizeof(double));
refSatRate[1] = 0;
refSatRate[2] = 0;
} }
std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
} }
ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
@ -506,22 +472,27 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double)); std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double));
return returnvalue::OK; return returnvalue::OK;
} else if (not rw1valid and rw2valid and rw3valid and rw4valid) { } else if (not rw1valid and rw2valid and rw3valid and rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without1, 12 * sizeof(double)); std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW1,
return returnvalue::OK; 12 * sizeof(double));
return acsctrl::SINGLE_RW_UNAVAILABLE;
} else if (rw1valid and not rw2valid and rw3valid and rw4valid) { } else if (rw1valid and not rw2valid and rw3valid and rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without2, 12 * sizeof(double)); std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW2,
return returnvalue::OK; 12 * sizeof(double));
return acsctrl::SINGLE_RW_UNAVAILABLE;
} else if (rw1valid and rw2valid and not rw3valid and rw4valid) { } else if (rw1valid and rw2valid and not rw3valid and rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without3, 12 * sizeof(double)); std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW3,
return returnvalue::OK; 12 * sizeof(double));
return acsctrl::SINGLE_RW_UNAVAILABLE;
} else if (rw1valid and rw2valid and rw3valid and not rw4valid) { } else if (rw1valid and rw2valid and rw3valid and not rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double)); std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW4,
return returnvalue::OK; 12 * sizeof(double));
} else { return acsctrl::SINGLE_RW_UNAVAILABLE;
return returnvalue::FAILED;
} }
return acsctrl::MULTIPLE_RW_UNAVAILABLE;
} }
void Guidance::resetValues() { std::memcpy(quatIXprev, ZERO_VEC4, sizeof(quatIXprev)); }
void Guidance::getTargetParamsSafe(double sunTargetSafe[3]) { void Guidance::getTargetParamsSafe(double sunTargetSafe[3]) {
std::error_code e; std::error_code e;
if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or

View File

@ -1,11 +1,19 @@
#ifndef GUIDANCE_H_ #ifndef GUIDANCE_H_
#define GUIDANCE_H_ #define GUIDANCE_H_
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/SensorValues.h>
#include <mission/controller/acs/util/MathOperations.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
#include <time.h> #include <time.h>
#include "../controllerdefinitions/AcsCtrlDefinitions.h" #include <cmath>
#include "AcsParameters.h" #include <filesystem>
#include "SensorValues.h" #include <string>
class Guidance { class Guidance {
public: public:
@ -14,6 +22,7 @@ class Guidance {
void getTargetParamsSafe(double sunTargetSafe[3]); void getTargetParamsSafe(double sunTargetSafe[3]);
ReturnValue_t solarArrayDeploymentComplete(); ReturnValue_t solarArrayDeploymentComplete();
void resetValues();
// Function to get the target quaternion and reference rotation rate from gps position and // Function to get the target quaternion and reference rotation rate from gps position and
// position of the ground station // position of the ground station
@ -59,9 +68,11 @@ class Guidance {
private: private:
const AcsParameters *acsParameters; const AcsParameters *acsParameters;
static constexpr double ZERO_VEC3[3] = {0, 0, 0};
static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
bool strBlindAvoidFlag = false; bool strBlindAvoidFlag = false;
double savedQuaternion[4] = {0, 0, 0, 0}; double quatIXprev[4] = {0, 0, 0, 0};
double omegaRefSaved[3] = {0, 0, 0};
static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/acsDeploymentConfirm"; static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/acsDeploymentConfirm";
static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/acsDeploymentConfirm"; static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/acsDeploymentConfirm";

View File

@ -39,7 +39,7 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
double qError[3] = {errorQuat[0], errorQuat[1], errorQuat[2]}; double qError[3] = {errorQuat[0], errorQuat[1], errorQuat[2]};
double cInt = 2 * om * zeta; double cInt = 2 * om * zeta;
double kInt = 2 * pow(om, 2); double kInt = 2 * om * om;
Review

sad pow sounds

sad pow sounds
double qErrorLaw[3] = {0, 0, 0}; double qErrorLaw[3] = {0, 0, 0};
@ -111,9 +111,13 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
VectorOperations<double>::mulScalar(torqueRws, -1, torqueRws, 4); VectorOperations<double>::mulScalar(torqueRws, -1, torqueRws, 4);
} }
void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, void PtgCtrl::ptgNullspace(const bool allRwAvabilable,
AcsParameters::PointingLawParameters *pointingLawParameters,
const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
const int32_t speedRw3, double *rwTrqNs) { const int32_t speedRw3, double *rwTrqNs) {
if (not allRwAvabilable) {
return;
}
// concentrate RW speeds as vector and convert to double // concentrate RW speeds as vector and convert to double
double speedRws[4] = {static_cast<double>(speedRw0), static_cast<double>(speedRw1), double speedRws[4] = {static_cast<double>(speedRw0), static_cast<double>(speedRw1),
static_cast<double>(speedRw2), static_cast<double>(speedRw3)}; static_cast<double>(speedRw2), static_cast<double>(speedRw3)};

View File

@ -33,7 +33,8 @@ class PtgCtrl {
void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError, void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
const double *deltaRate, const double *rwPseudoInv, double *torqueRws); const double *deltaRate, const double *rwPseudoInv, double *torqueRws);
void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, void ptgNullspace(const bool allRwAvabilable,
AcsParameters::PointingLawParameters *pointingLawParameters,
const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
const int32_t speedRw3, double *rwTrqNs); const int32_t speedRw3, double *rwTrqNs);

View File

@ -4,15 +4,12 @@
#include <fsfw/src/fsfw/globalfunctions/constants.h> #include <fsfw/src/fsfw/globalfunctions/constants.h>
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h> #include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/src/fsfw/globalfunctions/sign.h> #include <fsfw/src/fsfw/globalfunctions/sign.h>
#include <stdint.h> #include <fsfw/src/fsfw/serviceinterface.h>
#include <string.h>
#include <sys/time.h>
#include <cmath> #include <cmath>
#include <cstring>
#include <iostream> #include <iostream>
#include "fsfw/serviceinterface.h"
template <typename T1, typename T2 = T1> template <typename T1, typename T2 = T1>
class MathOperations { class MathOperations {
public: public:
@ -46,7 +43,7 @@ class MathOperations {
static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize, uint8_t colSize) { static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize, uint8_t colSize) {
int min_idx; int min_idx;
T1 temp; T1 temp;
memcpy(result, matrix, rowSize * colSize * sizeof(*result)); std::memcpy(result, matrix, rowSize * colSize * sizeof(*result));
// One by one move boundary of unsorted subarray // One by one move boundary of unsorted subarray
for (int k = 0; k < rowSize; k++) { for (int k = 0; k < rowSize; k++) {
for (int i = 0; i < colSize - 1; i++) { for (int i = 0; i < colSize - 1; i++) {

View File

@ -1,6 +1,7 @@
#ifndef MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ #ifndef MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_
#define MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ #define MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_
#include <common/config/eive/resultClassIds.h>
#include <fsfw/datapoollocal/StaticLocalDataSet.h> #include <fsfw/datapoollocal/StaticLocalDataSet.h>
#include <fsfw/datapoollocal/localPoolDefinitions.h> #include <fsfw/datapoollocal/localPoolDefinitions.h>
@ -8,6 +9,22 @@
namespace acsctrl { namespace acsctrl {
static const uint8_t IF_ACS_CTRL_ID = CLASS_ID::ACS_CTRL;
//! [EXPORT] : [COMMENT] File deletion failed and at least one file is still existent.
static constexpr ReturnValue_t FILE_DELETION_FAILED =
returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 0);
//! [EXPORT] : [COMMENT] Writing the TLE to the file has failed.
static constexpr ReturnValue_t WRITE_FILE_FAILED =
returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 1);
//! [EXPORT] : [COMMENT] Reading the TLE to the file has failed.
static constexpr ReturnValue_t READ_FILE_FAILED = returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 2);
//! [EXPORT] : [COMMENT] A single RW has failed.
static constexpr ReturnValue_t SINGLE_RW_UNAVAILABLE =
returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 3);
//! [EXPORT] : [COMMENT] Multiple RWs have failed.
static constexpr ReturnValue_t MULTIPLE_RW_UNAVAILABLE =
returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 4);
enum SetIds : uint32_t { enum SetIds : uint32_t {
MGM_SENSOR_DATA, MGM_SENSOR_DATA,
MGM_PROCESSED_DATA, MGM_PROCESSED_DATA,