Merge pull request 'Fixes for Pointing Modes' (#851) from ptg-fixes into main
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Reviewed-on: #851
This commit is contained in:
commit
ac06947078
10
CHANGELOG.md
10
CHANGELOG.md
@ -16,6 +16,9 @@ will consitute of a breaking change warranting a new major release:
|
||||
|
||||
# [unreleased]
|
||||
|
||||
- Bumped `eive-tmtc` to
|
||||
- Bumped `eive-fsfw`
|
||||
|
||||
## Added
|
||||
|
||||
- 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
|
||||
would then trigger FDIR and turning off the functioning device
|
||||
- `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
|
||||
|
||||
|
2
fsfw
2
fsfw
@ -1 +1 @@
|
||||
Subproject commit e64e8b274d436502d5c5b87865b9006e52e4b1aa
|
||||
Subproject commit b5e7179af1da085b9be598b4897f2664012781af
|
@ -49,7 +49,7 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
|
||||
case SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL: {
|
||||
ReturnValue_t result = guidance.solarArrayDeploymentComplete();
|
||||
if (result == returnvalue::FAILED) {
|
||||
return FILE_DELETION_FAILED;
|
||||
return acsctrl::FILE_DELETION_FAILED;
|
||||
}
|
||||
return HasActionsIF::EXECUTION_FINISHED;
|
||||
}
|
||||
@ -360,6 +360,7 @@ void AcsController::performPointingCtrl() {
|
||||
triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION);
|
||||
ptgCtrlLostCounter = 0;
|
||||
}
|
||||
guidance.resetValues();
|
||||
updateCtrlValData(ptgCtrlStrat);
|
||||
updateActuatorCmdData(ZERO_VEC4, cmdSpeedRws, ZERO_VEC3_INT16);
|
||||
commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
|
||||
@ -393,9 +394,10 @@ void AcsController::performPointingCtrl() {
|
||||
break;
|
||||
}
|
||||
|
||||
bool allRwAvailable = true;
|
||||
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
|
||||
if (result == returnvalue::FAILED) {
|
||||
if (result == acsctrl::MULTIPLE_RW_UNAVAILABLE) {
|
||||
if (multipleRwUnavailableCounter >=
|
||||
acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) {
|
||||
triggerEvent(acs::MULTIPLE_RW_INVALID);
|
||||
@ -403,8 +405,10 @@ void AcsController::performPointingCtrl() {
|
||||
}
|
||||
multipleRwUnavailableCounter++;
|
||||
return;
|
||||
} else {
|
||||
}
|
||||
multipleRwUnavailableCounter = 0;
|
||||
if (result == acsctrl::SINGLE_RW_UNAVAILABLE) {
|
||||
allRwAvailable = false;
|
||||
}
|
||||
|
||||
// Variables required for guidance
|
||||
@ -422,7 +426,7 @@ void AcsController::performPointingCtrl() {
|
||||
errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters,
|
||||
ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.idleModeControllerParameters,
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
||||
rwTrqNs);
|
||||
@ -445,7 +449,7 @@ void AcsController::performPointingCtrl() {
|
||||
errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters,
|
||||
ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.targetModeControllerParameters,
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
||||
rwTrqNs);
|
||||
@ -465,7 +469,7 @@ void AcsController::performPointingCtrl() {
|
||||
errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters,
|
||||
ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.gsTargetModeControllerParameters,
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
||||
rwTrqNs);
|
||||
@ -488,7 +492,7 @@ void AcsController::performPointingCtrl() {
|
||||
errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters,
|
||||
ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.nadirModeControllerParameters,
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
||||
rwTrqNs);
|
||||
@ -510,7 +514,7 @@ void AcsController::performPointingCtrl() {
|
||||
errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters,
|
||||
ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.inertialModeControllerParameters,
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
||||
rwTrqNs);
|
||||
@ -899,6 +903,7 @@ void AcsController::modeChanged(Mode_t mode, Submode_t submode) {
|
||||
rw4SpeedSet.setRwSpeed(0, 10);
|
||||
}
|
||||
}
|
||||
guidance.resetValues();
|
||||
return ExtendedControllerBase::modeChanged(mode, submode);
|
||||
}
|
||||
|
||||
@ -1095,7 +1100,7 @@ ReturnValue_t AcsController::writeTleToFs(const uint8_t *tle) {
|
||||
tleFile << "\n";
|
||||
tleFile.write(reinterpret_cast<const char *>(tle + 69), 69);
|
||||
} else {
|
||||
return WRITE_FILE_FAILED;
|
||||
return acsctrl::WRITE_FILE_FAILED;
|
||||
}
|
||||
tleFile.close();
|
||||
return returnvalue::OK;
|
||||
@ -1119,12 +1124,12 @@ ReturnValue_t AcsController::readTleFromFs(uint8_t *line1, uint8_t *line2) {
|
||||
std::memcpy(line2, tleLine2.c_str(), 69);
|
||||
} else {
|
||||
triggerEvent(acs::TLE_FILE_READ_FAILED);
|
||||
return READ_FILE_FAILED;
|
||||
return acsctrl::READ_FILE_FAILED;
|
||||
}
|
||||
tleFile.close();
|
||||
} else {
|
||||
triggerEvent(acs::TLE_FILE_READ_FAILED);
|
||||
return READ_FILE_FAILED;
|
||||
return acsctrl::READ_FILE_FAILED;
|
||||
}
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
@ -112,14 +112,6 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
||||
static const DeviceCommandId_t UPDATE_TLE = 0x3;
|
||||
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 handleCommandMessage(CommandMessage* message) override;
|
||||
void performControlOperation() override;
|
||||
|
@ -333,16 +333,16 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
parameterWrapper->setMatrix(rwMatrices.pseudoInverse);
|
||||
break;
|
||||
case 0x2:
|
||||
parameterWrapper->setMatrix(rwMatrices.without1);
|
||||
parameterWrapper->setMatrix(rwMatrices.pseudoInverseWithoutRW1);
|
||||
break;
|
||||
case 0x3:
|
||||
parameterWrapper->setMatrix(rwMatrices.without2);
|
||||
parameterWrapper->setMatrix(rwMatrices.pseudoInverseWithoutRW2);
|
||||
break;
|
||||
case 0x4:
|
||||
parameterWrapper->setMatrix(rwMatrices.without3);
|
||||
parameterWrapper->setMatrix(rwMatrices.pseudoInverseWithoutRW3);
|
||||
break;
|
||||
case 0x5:
|
||||
parameterWrapper->setMatrix(rwMatrices.without4);
|
||||
parameterWrapper->setMatrix(rwMatrices.pseudoInverseWithoutRW4);
|
||||
break;
|
||||
case 0x6:
|
||||
parameterWrapper->setVector(rwMatrices.nullspaceVector);
|
||||
|
@ -812,19 +812,19 @@ class AcsParameters : public HasParametersIF {
|
||||
} rwHandlingParameters;
|
||||
|
||||
struct RwMatrices {
|
||||
double alignmentMatrix[3][4] = {{0.9205, 0.0000, -0.9205, 0.0000},
|
||||
{0.0000, -0.9205, 0.0000, 0.9205},
|
||||
{0.3907, 0.3907, 0.3907, 0.3907}};
|
||||
double alignmentMatrix[3][4] = {{-0.9205, 0.0000, 0.9205, 0.0000},
|
||||
{0.0000, 0.9205, 0.0000, -0.9205},
|
||||
{-0.3907, -0.3907, -0.3907, -0.3907}};
|
||||
double pseudoInverse[4][3] = {
|
||||
{0.5432, 0, 0.6398}, {0, -0.5432, 0.6398}, {-0.5432, 0, 0.6398}, {0, 0.5432, 0.6398}};
|
||||
double without1[4][3] = {
|
||||
{0, 0, 0}, {0.5432, -0.5432, 1.2797}, {-1.0864, 0, 0}, {0.5432, 0.5432, 1.2797}};
|
||||
double without2[4][3] = {
|
||||
{0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 1.0864, 0}};
|
||||
double without3[4][3] = {
|
||||
{1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}};
|
||||
double without4[4][3] = {
|
||||
{0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}};
|
||||
{-0.5432, 0, -0.6399}, {0, 0.5432, -0.6399}, {0.5432, 0, -0.6399}, {0, -0.5432, -0.6399}};
|
||||
double pseudoInverseWithoutRW1[4][3] = {
|
||||
{0, 0, 0}, {-0.5432, 0.5432, -1.2798}, {1.0864, 0, 0}, {-0.5432, -0.5432, -1.2798}};
|
||||
double pseudoInverseWithoutRW2[4][3] = {
|
||||
{-0.5432, 0.5432, -1.2798}, {0, 0, 0}, {0.5432, 0.5432, -1.2798}, {0, -1.0864, 0}};
|
||||
double pseudoInverseWithoutRW3[4][3] = {
|
||||
{-1.0864, 0, 0}, {0.5432, 0.5432, -1.2798}, {0, 0, 0}, {0.5432, -0.5432, -1.2798}};
|
||||
double pseudoInverseWithoutRW4[4][3] = {
|
||||
{-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};
|
||||
} rwMatrices;
|
||||
|
||||
|
@ -1,15 +1,5 @@
|
||||
#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() {}
|
||||
@ -431,7 +421,11 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
|
||||
errorAngle = QuaternionOperations::getAngle(errorQuat, true);
|
||||
|
||||
// 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};
|
||||
VectorOperations<double>::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3);
|
||||
// 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,
|
||||
double quatInertialTarget[4], double *refSatRate) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target rotation rate
|
||||
//-------------------------------------------------------------------------------------
|
||||
if (VectorOperations<double>::norm(savedQuaternion, 4) == 0) {
|
||||
std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
|
||||
double quatIX[4], double *refSatRate) {
|
||||
if (VectorOperations<double>::norm(quatIXprev, 4) == 0) {
|
||||
std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
|
||||
}
|
||||
if (timeDelta < timeElapsedMax and timeDelta != 0.0) {
|
||||
double q[4] = {0, 0, 0, 0}, qS[4] = {0, 0, 0, 0};
|
||||
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];
|
||||
if (timeDelta != 0.0) {
|
||||
QuaternionOperations::rotationFromQuaternions(quatIX, quatIXprev, timeDelta, refSatRate);
|
||||
} else {
|
||||
refSatRate[0] = 0;
|
||||
refSatRate[1] = 0;
|
||||
refSatRate[2] = 0;
|
||||
std::memcpy(refSatRate, ZERO_VEC3, 3 * sizeof(double));
|
||||
}
|
||||
|
||||
std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
|
||||
std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
|
||||
}
|
||||
|
||||
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));
|
||||
return returnvalue::OK;
|
||||
} else if (not rw1valid and rw2valid and rw3valid and rw4valid) {
|
||||
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without1, 12 * sizeof(double));
|
||||
return returnvalue::OK;
|
||||
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW1,
|
||||
12 * sizeof(double));
|
||||
return acsctrl::SINGLE_RW_UNAVAILABLE;
|
||||
} else if (rw1valid and not rw2valid and rw3valid and rw4valid) {
|
||||
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without2, 12 * sizeof(double));
|
||||
return returnvalue::OK;
|
||||
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW2,
|
||||
12 * sizeof(double));
|
||||
return acsctrl::SINGLE_RW_UNAVAILABLE;
|
||||
} else if (rw1valid and rw2valid and not rw3valid and rw4valid) {
|
||||
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without3, 12 * sizeof(double));
|
||||
return returnvalue::OK;
|
||||
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW3,
|
||||
12 * sizeof(double));
|
||||
return acsctrl::SINGLE_RW_UNAVAILABLE;
|
||||
} else if (rw1valid and rw2valid and rw3valid and not rw4valid) {
|
||||
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double));
|
||||
return returnvalue::OK;
|
||||
} else {
|
||||
return returnvalue::FAILED;
|
||||
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW4,
|
||||
12 * sizeof(double));
|
||||
return acsctrl::SINGLE_RW_UNAVAILABLE;
|
||||
}
|
||||
return acsctrl::MULTIPLE_RW_UNAVAILABLE;
|
||||
}
|
||||
|
||||
void Guidance::resetValues() { std::memcpy(quatIXprev, ZERO_VEC4, sizeof(quatIXprev)); }
|
||||
|
||||
void Guidance::getTargetParamsSafe(double sunTargetSafe[3]) {
|
||||
std::error_code e;
|
||||
if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or
|
||||
|
@ -1,11 +1,19 @@
|
||||
#ifndef 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 "../controllerdefinitions/AcsCtrlDefinitions.h"
|
||||
#include "AcsParameters.h"
|
||||
#include "SensorValues.h"
|
||||
#include <cmath>
|
||||
#include <filesystem>
|
||||
#include <string>
|
||||
|
||||
class Guidance {
|
||||
public:
|
||||
@ -14,6 +22,7 @@ class Guidance {
|
||||
|
||||
void getTargetParamsSafe(double sunTargetSafe[3]);
|
||||
ReturnValue_t solarArrayDeploymentComplete();
|
||||
void resetValues();
|
||||
|
||||
// Function to get the target quaternion and reference rotation rate from gps position and
|
||||
// position of the ground station
|
||||
@ -59,9 +68,11 @@ class Guidance {
|
||||
private:
|
||||
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;
|
||||
double savedQuaternion[4] = {0, 0, 0, 0};
|
||||
double omegaRefSaved[3] = {0, 0, 0};
|
||||
double quatIXprev[4] = {0, 0, 0, 0};
|
||||
|
||||
static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/acsDeploymentConfirm";
|
||||
static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/acsDeploymentConfirm";
|
||||
|
@ -39,7 +39,7 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
|
||||
double qError[3] = {errorQuat[0], errorQuat[1], errorQuat[2]};
|
||||
|
||||
double cInt = 2 * om * zeta;
|
||||
double kInt = 2 * pow(om, 2);
|
||||
double kInt = 2 * om * om;
|
||||
|
||||
double qErrorLaw[3] = {0, 0, 0};
|
||||
|
||||
@ -111,9 +111,13 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
|
||||
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 speedRw3, double *rwTrqNs) {
|
||||
if (not allRwAvabilable) {
|
||||
return;
|
||||
}
|
||||
// concentrate RW speeds as vector and convert to double
|
||||
double speedRws[4] = {static_cast<double>(speedRw0), static_cast<double>(speedRw1),
|
||||
static_cast<double>(speedRw2), static_cast<double>(speedRw3)};
|
||||
|
@ -33,7 +33,8 @@ class PtgCtrl {
|
||||
void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
|
||||
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 speedRw3, double *rwTrqNs);
|
||||
|
||||
|
@ -4,15 +4,12 @@
|
||||
#include <fsfw/src/fsfw/globalfunctions/constants.h>
|
||||
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
#include <fsfw/src/fsfw/globalfunctions/sign.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <sys/time.h>
|
||||
#include <fsfw/src/fsfw/serviceinterface.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
#include <iostream>
|
||||
|
||||
#include "fsfw/serviceinterface.h"
|
||||
|
||||
template <typename T1, typename T2 = T1>
|
||||
class MathOperations {
|
||||
public:
|
||||
@ -46,7 +43,7 @@ class MathOperations {
|
||||
static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize, uint8_t colSize) {
|
||||
int min_idx;
|
||||
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
|
||||
for (int k = 0; k < rowSize; k++) {
|
||||
for (int i = 0; i < colSize - 1; i++) {
|
||||
|
@ -1,6 +1,7 @@
|
||||
#ifndef 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/localPoolDefinitions.h>
|
||||
|
||||
@ -8,6 +9,22 @@
|
||||
|
||||
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 {
|
||||
MGM_SENSOR_DATA,
|
||||
MGM_PROCESSED_DATA,
|
||||
|
Loading…
Reference in New Issue
Block a user