eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 Wiki Activity

Merge branch 'main' into mekf-fix
Some checks failed
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert
2024-01-31 16:46:12 +01:00
parent 4e8776ff68 aa521e89f6
commit c85bef6de4
34 changed files with 412 additions and 290 deletions
Download Patch File Download Diff File Expand all files Collapse all files

193
mission/controller/AcsController.cpp
View File

@ -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;
}
@ -195,6 +195,8 @@ void AcsController::performAttitudeControl() {
mekfInvalidFlag = false;
}
handleDetumbling();
switch (mode) {
case acs::SAFE:
switch (submode) {
@ -284,33 +286,6 @@ void AcsController::performSafe() {
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
// detumble check and switch
if (acsParameters.safeModeControllerParameters.useMekf) {
if (attitudeEstimationData.satRotRateMekf.isValid() and
VectorOperations<double>::norm(attitudeEstimationData.satRotRateMekf.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
}
} else if (acsParameters.safeModeControllerParameters.useGyr) {
if (gyrDataProcessed.gyrVecTot.isValid() and
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
}
} else if (fusedRotRateData.rotRateTotal.isValid() and
VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
} else if (detumbleCounter > 0) {
detumbleCounter -= 1;
}
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
detumbleCounter = 0;
// Triggers detumble mode transition in subsystem
triggerEvent(acs::SAFE_RATE_VIOLATION);
startTransition(mode, acs::SafeSubmode::DETUMBLE);
}
updateCtrlValData(errAng, safeCtrlStrat);
updateActuatorCmdData(cmdDipoleMtqs);
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
@ -346,33 +321,6 @@ void AcsController::performDetumble() {
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
if (acsParameters.safeModeControllerParameters.useMekf) {
if (attitudeEstimationData.satRotRateMekf.isValid() and
VectorOperations<double>::norm(attitudeEstimationData.satRotRateMekf.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++;
}
} else if (acsParameters.safeModeControllerParameters.useGyr) {
if (gyrDataProcessed.gyrVecTot.isValid() and
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++;
}
} else if (fusedRotRateData.rotRateTotal.isValid() and
VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++;
} else if (detumbleCounter > 0) {
detumbleCounter -= 1;
}
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
detumbleCounter = 0;
// Triggers safe mode transition in subsystem
triggerEvent(acs::SAFE_RATE_RECOVERY);
startTransition(mode, acs::SafeSubmode::DEFAULT);
}
updateCtrlValData(safeCtrlStrat);
updateActuatorCmdData(cmdDipoleMtqs);
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
@ -403,14 +351,18 @@ void AcsController::performPointingCtrl() {
acs::ControlModeStrategy ptgCtrlStrat = ptgCtrl.pointingCtrlStrategy(
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, strValid,
attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotal.isValid(),
fusedRotRateData.rotRateSource.isValid(), useMekf);
fusedRotRateData.rotRateSource.value, useMekf);
if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) {
if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL or
ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) {
ptgCtrlLostCounter++;
if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) {
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],
cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
acsParameters.rwHandlingParameters.rampTime);
@ -437,15 +389,15 @@ void AcsController::performPointingCtrl() {
std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB));
break;
default:
sif::error << "AcsController: Invalid pointing mode strategy for performDetumble"
sif::error << "AcsController: Invalid pointing mode strategy for performPointingCtrl"
<< std::endl;
break;
}
uint8_t enableAntiStiction = true;
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);
@ -453,8 +405,10 @@ void AcsController::performPointingCtrl() {
}
multipleRwUnavailableCounter++;
return;
} else {
multipleRwUnavailableCounter = 0;
}
multipleRwUnavailableCounter = 0;
if (result == acsctrl::SINGLE_RW_UNAVAILABLE) {
allRwAvailable = false;
}
// Variables required for guidance
@ -472,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);
@ -483,7 +437,6 @@ void AcsController::performPointingCtrl() {
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
break;
case acs::PTG_TARGET:
@ -496,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);
@ -507,7 +460,6 @@ void AcsController::performPointingCtrl() {
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
break;
case acs::PTG_TARGET_GS:
@ -517,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);
@ -528,7 +480,6 @@ void AcsController::performPointingCtrl() {
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction;
break;
case acs::PTG_NADIR:
@ -541,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);
@ -552,7 +503,6 @@ void AcsController::performPointingCtrl() {
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
break;
case acs::PTG_INERTIAL:
@ -564,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);
@ -575,7 +525,6 @@ void AcsController::performPointingCtrl() {
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
break;
default:
sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl;
@ -587,13 +536,11 @@ void AcsController::performPointingCtrl() {
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel,
acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws);
if (enableAntiStiction) {
ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
}
ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs);
updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate, ptgCtrlStrat);
updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs);
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
@ -601,6 +548,62 @@ void AcsController::performPointingCtrl() {
acsParameters.rwHandlingParameters.rampTime);
}
void AcsController::handleDetumbling() {
switch (detumbleState) {
case DetumbleState::NO_DETUMBLE:
if (fusedRotRateData.rotRateTotal.isValid() and
VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
} else if (detumbleCounter > 0) {
detumbleCounter -= 1;
}
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
if (mode == acs::AcsMode::SAFE) {
detumbleState = DetumbleState::DETUMBLE_FROM_SAFE;
break;
}
detumbleState = DetumbleState::DETUMBLE_FROM_PTG;
}
break;
case DetumbleState::DETUMBLE_FROM_PTG:
triggerEvent(acs::PTG_RATE_VIOLATION);
detumbleState = DetumbleState::PTG_TO_SAFE_TRANSITION;
break;
case DetumbleState::PTG_TO_SAFE_TRANSITION:
if (mode == acs::AcsMode::SAFE) {
detumbleState = DetumbleState::DETUMBLE_FROM_SAFE;
}
break;
case DetumbleState::DETUMBLE_FROM_SAFE:
detumbleCounter = 0;
// Triggers detumble mode transition in subsystem
triggerEvent(acs::SAFE_RATE_VIOLATION);
startTransition(mode, acs::SafeSubmode::DETUMBLE);
detumbleState = DetumbleState::IN_DETUMBLE;
break;
case DetumbleState::IN_DETUMBLE:
if (fusedRotRateData.rotRateTotal.isValid() and
VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++;
} else if (detumbleCounter > 0) {
detumbleCounter -= 1;
}
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
detumbleCounter = 0;
// Triggers safe mode transition in subsystem
triggerEvent(acs::RATE_RECOVERY);
startTransition(mode, acs::SafeSubmode::DEFAULT);
detumbleState = DetumbleState::NO_DETUMBLE;
}
break;
default:
sif::error << "AcsController: Invalid DetumbleState" << std::endl;
}
}
void AcsController::safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure) {
if (not safeCtrlFailureFlag) {
triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, mgmFailure, sensorFailure);
@ -671,7 +674,7 @@ void AcsController::updateActuatorCmdData(const double *rwTargetTorque,
}
}
void AcsController::updateCtrlValData(uint8_t safeModeStrat) {
void AcsController::updateCtrlValData(acs::ControlModeStrategy ctrlStrat) {
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
@ -682,13 +685,13 @@ void AcsController::updateCtrlValData(uint8_t safeModeStrat) {
ctrlValData.errAng.setValid(false);
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
ctrlValData.tgtRotRate.setValid(false);
ctrlValData.safeStrat.value = safeModeStrat;
ctrlValData.safeStrat.value = ctrlStrat;
ctrlValData.safeStrat.setValid(true);
ctrlValData.setValidity(true, false);
}
}
void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) {
void AcsController::updateCtrlValData(double errAng, acs::ControlModeStrategy ctrlStrat) {
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
@ -699,21 +702,22 @@ void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) {
ctrlValData.errAng.setValid(true);
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
ctrlValData.tgtRotRate.setValid(false);
ctrlValData.safeStrat.value = safeModeStrat;
ctrlValData.safeStrat.value = ctrlStrat;
ctrlValData.safeStrat.setValid(true);
ctrlValData.setValidity(true, false);
}
}
void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng,
const double *tgtRotRate) {
const double *tgtRotRate,
acs::ControlModeStrategy ctrlStrat) {
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double));
std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
ctrlValData.errAng.value = errAng;
std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double));
ctrlValData.safeStrat.value = acs::ControlModeStrategy::CTRL_OFF;
ctrlValData.safeStrat.value = ctrlStrat;
ctrlValData.setValidity(true, true);
}
}
@ -881,6 +885,25 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
}
void AcsController::modeChanged(Mode_t mode, Submode_t submode) {
if (mode == acs::AcsMode::SAFE) {
{
PoolReadGuard pg(&rw1SpeedSet);
rw1SpeedSet.setRwSpeed(0, 10);
}
{
PoolReadGuard pg(&rw2SpeedSet);
rw2SpeedSet.setRwSpeed(0, 10);
}
{
PoolReadGuard pg(&rw3SpeedSet);
rw3SpeedSet.setRwSpeed(0, 10);
}
{
PoolReadGuard pg(&rw4SpeedSet);
rw4SpeedSet.setRwSpeed(0, 10);
}
}
guidance.resetValues();
return ExtendedControllerBase::modeChanged(mode, submode);
}
@ -1077,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;
@ -1101,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;
}

36
mission/controller/AcsController.h
View File

@ -46,12 +46,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
uint16_t startAtIndex) override;
protected:
void performAttitudeControl();
void performSafe();
void performDetumble();
void performPointingCtrl();
private:
static constexpr int16_t ZERO_VEC3_INT16[3] = {0, 0, 0};
static constexpr double ZERO_VEC3[3] = {0, 0, 0};
static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
static constexpr double RW_OFF_TORQUE[4] = {0, 0, 0, 0};
@ -100,6 +96,15 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
enum class InternalState { STARTUP, INITIAL_DELAY, READY };
InternalState internalState = InternalState::STARTUP;
enum class DetumbleState {
NO_DETUMBLE,
DETUMBLE_FROM_PTG,
PTG_TO_SAFE_TRANSITION,
DETUMBLE_FROM_SAFE,
IN_DETUMBLE
};
DetumbleState detumbleState = DetumbleState::NO_DETUMBLE;
/** Device command IDs */
static const DeviceCommandId_t SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0x0;
static const DeviceCommandId_t RESET_MEKF = 0x1;
@ -107,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;
@ -133,6 +130,13 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
void modeChanged(Mode_t mode, Submode_t submode);
void announceMode(bool recursive);
void performAttitudeControl();
void performSafe();
void performDetumble();
void performPointingCtrl();
void handleDetumbling();
void safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure);
ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
@ -143,10 +147,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
void updateActuatorCmdData(const int16_t* mtqTargetDipole);
void updateActuatorCmdData(const double* rwTargetTorque, const int32_t* rwTargetSpeed,
const int16_t* mtqTargetDipole);
void updateCtrlValData(uint8_t safeModeStrat);
void updateCtrlValData(double errAng, uint8_t safeModeStrat);
void updateCtrlValData(acs::ControlModeStrategy ctrlStrat);
void updateCtrlValData(double errAng, acs::ControlModeStrategy ctrlStrat);
void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng,
const double* tgtRotRate);
const double* tgtRotRate, acs::ControlModeStrategy cStrat);
ReturnValue_t updateTle(const uint8_t* line1, const uint8_t* line2, bool fromFile);
ReturnValue_t writeTleToFs(const uint8_t* tle);

69
mission/controller/acs/AcsParameters.cpp
View File

@ -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);
@ -432,9 +432,6 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(idleModeControllerParameters.desatOn);
break;
case 0x9:
parameterWrapper->set(idleModeControllerParameters.enableAntiStiction);
break;
case 0xA:
parameterWrapper->set(idleModeControllerParameters.useMekf);
break;
default:
@ -471,42 +468,39 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(targetModeControllerParameters.desatOn);
break;
case 0x9:
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
break;
case 0xA:
parameterWrapper->set(targetModeControllerParameters.useMekf);
break;
case 0xB:
case 0xA:
parameterWrapper->setVector(targetModeControllerParameters.refDirection);
break;
case 0xC:
case 0xB:
parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
break;
case 0xD:
case 0xC:
parameterWrapper->setVector(targetModeControllerParameters.quatRef);
break;
case 0xE:
case 0xD:
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
break;
case 0xF:
case 0xE:
parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
break;
case 0x10:
case 0xF:
parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
break;
case 0x11:
case 0x10:
parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
break;
case 0x12:
case 0x11:
parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
break;
case 0x13:
case 0x12:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
break;
case 0x14:
case 0x13:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
break;
case 0x15:
case 0x14:
parameterWrapper->set(targetModeControllerParameters.blindRotRate);
break;
default:
@ -543,24 +537,21 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
break;
case 0x9:
parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
break;
case 0xA:
parameterWrapper->set(gsTargetModeControllerParameters.useMekf);
break;
case 0xB:
case 0xA:
parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
break;
case 0xC:
case 0xB:
parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
break;
case 0xD:
case 0xC:
parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
break;
case 0xE:
case 0xD:
parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
break;
case 0xF:
case 0xE:
parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt);
break;
default:
@ -597,21 +588,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(nadirModeControllerParameters.desatOn);
break;
case 0x9:
parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
break;
case 0xA:
parameterWrapper->set(nadirModeControllerParameters.useMekf);
break;
case 0xB:
case 0xA:
parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
break;
case 0xC:
case 0xB:
parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
break;
case 0xD:
case 0xC:
parameterWrapper->setVector(nadirModeControllerParameters.refRotRate);
break;
case 0xE:
case 0xD:
parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax);
break;
default:
@ -648,18 +636,15 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(inertialModeControllerParameters.desatOn);
break;
case 0x9:
parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
break;
case 0xA:
parameterWrapper->set(inertialModeControllerParameters.useMekf);
break;
case 0xB:
case 0xA:
parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
break;
case 0xC:
case 0xB:
parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
break;
case 0xD:
case 0xC:
parameterWrapper->setVector(inertialModeControllerParameters.quatRef);
break;
default:

25
mission/controller/acs/AcsParameters.h
View File

@ -815,19 +815,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;
@ -866,7 +866,6 @@ class AcsParameters : public HasParametersIF {
double desatMomentumRef[3] = {0, 0, 0};
double deSatGainFactor = 1000;
uint8_t desatOn = true;
uint8_t enableAntiStiction = true;
uint8_t useMekf = false;
} pointingLawParameters;

87
mission/controller/acs/Guidance.cpp
View File

@ -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() {}
@ -418,7 +408,9 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
double errorQuat[4], double errorSatRotRate[3], double &errorAngle) {
// First calculate error quaternion between current and target orientation
QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
double invTargetQuat[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(targetQuat, invTargetQuat);
QuaternionOperations::multiply(currentQuat, invTargetQuat, errorQuat);
// Last calculate add rotation from reference quaternion
QuaternionOperations::multiply(refQuat, errorQuat, errorQuat);
// Keep scalar part of quaternion positive
@ -429,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
@ -453,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,
@ -504,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

21
mission/controller/acs/Guidance.h
View File

@ -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";

13
mission/controller/acs/control/PtgCtrl.cpp
View File

@ -21,9 +21,8 @@ acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(
return acs::ControlModeStrategy::PTGCTRL_STR;
} else if (questValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) {
return acs::ControlModeStrategy::PTGCTRL_QUEST;
} else {
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
}
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
}
void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
@ -40,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};
@ -112,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)};
@ -221,6 +224,8 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpee
}
}
}
} else {
rwCmdSpeeds[i] = 0;
}
}
}

3
mission/controller/acs/control/PtgCtrl.h
View File

@ -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);

13
mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
View File

@ -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,18 @@
namespace acsctrl {
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(0xA0);
//! [EXPORT] : [COMMENT] Writing the TLE to the file has failed.
static constexpr ReturnValue_t WRITE_FILE_FAILED = MAKE_RETURN_CODE(0xA1);
//! [EXPORT] : [COMMENT] Reading the TLE to the file has failed.
static constexpr ReturnValue_t READ_FILE_FAILED = MAKE_RETURN_CODE(0xA2);
//! [EXPORT] : [COMMENT] A single RW has failed.
static constexpr ReturnValue_t SINGLE_RW_UNAVAILABLE = MAKE_RETURN_CODE(0xA3);
//! [EXPORT] : [COMMENT] Multiple RWs have failed.
static constexpr ReturnValue_t MULTIPLE_RW_UNAVAILABLE = MAKE_RETURN_CODE(0xA4);
enum SetIds : uint32_t {
MGM_SENSOR_DATA,
MGM_PROCESSED_DATA,