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

Merge pull request 'New Safe Mode Controller' (#748) from acs-safe-flp-v2 into main
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source 
Reviewed-on: #748
Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>
This commit is contained in:
Robin Müller 2023-07-26 11:08:11 +02:00
commit dbb3434cd9
15 changed files with 400 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CHANGELOG.md
View File

@ -24,6 +24,10 @@ will consitute of a breaking change warranting a new major release:
- Re-ordered some functions of the core controller in the initialize function.
- Rad sensor is now only polled every 30 minutes instead of every device cycle to reduce wear of
the RADFET electronics.
- ACS Controller now includes the safe mode from FLP, which will calculate its rotational rate
from SUS and MGM measurements. To accommodate these changes, low-pass filters for SUS
measurements and rates as well as MGM measurements and rates are included. Usage of the new
controller as well as settings of the low-pass filters can be handled via parameter commands.
## Added

2
bsp_q7s/em/emObjectFactory.cpp
View File

@ -124,6 +124,8 @@ void ObjectFactory::produce(void* args) {
if (core::FW_VERSION_MAJOR >= 4) {
battAndImtqI2cDev = q7s::I2C_PS_EIVE;
}
static_cast<void>(battAndImtqI2cDev);
#if OBSW_ADD_MGT == 1
createImtqComponents(pwrSwitcher, enableHkSets, battAndImtqI2cDev);
#endif

10
mission/acs/defs.h
View File

@ -26,10 +26,12 @@ enum SafeModeStrategy : uint8_t {
SAFECTRL_OFF = 0,
SAFECTRL_NO_MAG_FIELD_FOR_CONTROL = 1,
SAFECTRL_NO_SENSORS_FOR_CONTROL = 2,
SAFECTRL_ACTIVE_MEKF = 10,
SAFECTRL_WITHOUT_MEKF = 11,
SAFECTRL_ECLIPSE_DAMPING = 12,
SAFECTRL_ECLIPSE_IDELING = 13,
SAFECTRL_MEKF = 10,
SAFECTRL_GYR = 11,
SAFECTRL_SUSMGM = 12,
SAFECTRL_ECLIPSE_DAMPING_GYR = 13,
SAFECTRL_ECLIPSE_DAMPING_SUSMGM = 14,
SAFECTRL_ECLIPSE_IDELING = 15,
SAFECTRL_DETUMBLE_FULL = 20,
SAFECTRL_DETUMBLE_DETERIORATED = 21,
};

87
mission/controller/AcsController.cpp
View File

@ -7,6 +7,7 @@
AcsController::AcsController(object_id_t objectId, bool enableHkSets)
: ExtendedControllerBase(objectId),
enableHkSets(enableHkSets),
fusedRotationEstimation(&acsParameters),
guidance(&acsParameters),
safeCtrl(&acsParameters),
ptgCtrl(&acsParameters),
@ -20,7 +21,8 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets)
gpsDataProcessed(this),
mekfData(this),
ctrlValData(this),
actuatorCmdData(this) {}
actuatorCmdData(this),
fusedRotRateData(this) {}
ReturnValue_t AcsController::initialize() {
ReturnValue_t result = parameterHelper.initialize();
@ -146,6 +148,8 @@ void AcsController::performSafe() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
&gyrDataProcessed, &fusedRotRateData);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
@ -172,25 +176,42 @@ void AcsController::performSafe() {
acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
fusedRotRateData.rotRateOrthogonal.isValid(), fusedRotRateData.rotRateTotal.isValid(),
acsParameters.safeModeControllerParameters.useMekf,
acsParameters.safeModeControllerParameters.useGyr,
acsParameters.safeModeControllerParameters.dampingDuringEclipse);
switch (safeCtrlStrat) {
case (acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF):
case (acs::SafeModeStrategy::SAFECTRL_MEKF):
safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value,
susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir,
magMomMtq, errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF):
safeCtrl.safeNonMekf(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
case (acs::SafeModeStrategy::SAFECTRL_GYR):
safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING):
safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
sunTargetDir, magMomMtq, errAng);
case (acs::SafeModeStrategy::SAFECTRL_SUSMGM):
safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateParallel.value,
fusedRotRateData.rotRateOrthogonal.value,
susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR):
safeCtrl.safeRateDampingGyr(mgmDataProcessed.mgmVecTot.value,
gyrDataProcessed.gyrVecTot.value, sunTargetDir, magMomMtq,
errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM):
safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value,
fusedRotRateData.rotRateTotal.value, sunTargetDir, magMomMtq,
errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
@ -214,12 +235,20 @@ void AcsController::performSafe() {
acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
// detumble check and switch
if (mekfData.satRotRateMekf.isValid() && acsParameters.safeModeControllerParameters.useMekf &&
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
} else if (gyrDataProcessed.gyrVecTot.isValid() &&
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) >
if (acsParameters.safeModeControllerParameters.useMekf) {
if (mekfData.satRotRateMekf.isValid() and
VectorOperations<double>::norm(mekfData.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) {
@ -289,17 +318,26 @@ void AcsController::performDetumble() {
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
if (mekfData.satRotRateMekf.isValid() &&
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++;
} else if (gyrDataProcessed.gyrVecTot.isValid() &&
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
if (acsParameters.safeModeControllerParameters.useMekf) {
if (mekfData.satRotRateMekf.isValid() and
VectorOperations<double>::norm(mekfData.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 safe mode transition in subsystem
@ -707,6 +745,11 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed);
localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole);
poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), enableHkSets, 10.0});
// Fused Rot Rate
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL, &rotRateOrthogonal);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL, &rotRateTotal);
poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0});
return returnvalue::OK;
}
@ -732,6 +775,8 @@ LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
return &ctrlValData;
case acsctrl::ACTUATOR_CMD_DATA:
return &actuatorCmdData;
case acsctrl::FUSED_ROTATION_RATE_DATA:
return &fusedRotRateData;
default:
return nullptr;
}

8
mission/controller/AcsController.h
View File

@ -13,6 +13,7 @@
#include <mission/acs/rwHelpers.h>
#include <mission/acs/susMax1227Helpers.h>
#include <mission/controller/acs/ActuatorCmd.h>
#include <mission/controller/acs/FusedRotationEstimation.h>
#include <mission/controller/acs/Guidance.h>
#include <mission/controller/acs/MultiplicativeKalmanFilter.h>
#include <mission/controller/acs/Navigation.h>
@ -49,6 +50,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
AcsParameters acsParameters;
SensorProcessing sensorProcessing;
FusedRotationEstimation fusedRotationEstimation;
Navigation navigation;
ActuatorCmd actuatorCmd;
Guidance guidance;
@ -226,6 +228,12 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
PoolEntry<int32_t> rwTargetSpeed = PoolEntry<int32_t>(4);
PoolEntry<int16_t> mtqTargetDipole = PoolEntry<int16_t>(3);
// Fused Rot Rate
acsctrl::FusedRotRateData fusedRotRateData;
PoolEntry<double> rotRateOrthogonal = PoolEntry<double>(3);
PoolEntry<double> rotRateParallel = PoolEntry<double>(3);
PoolEntry<double> rotRateTotal = PoolEntry<double>(3);
// Initial delay to make sure all pool variables have been initialized their owners
Countdown initialCountdown = Countdown(INIT_DELAY);
};

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

@ -105,6 +105,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->setVector(mgmHandlingParameters.mgm4variance);
break;
case 0x12:
parameterWrapper->set(mgmHandlingParameters.mgmVectorFilterWeight);
break;
case 0x13:
parameterWrapper->set(mgmHandlingParameters.mgmDerivativeFilterWeight);
break;
default:
@ -224,6 +227,12 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x24:
parameterWrapper->set(susHandlingParameters.susBrightnessThreshold);
break;
case 0x25:
parameterWrapper->set(susHandlingParameters.susVectorFilterWeight);
break;
case 0x26:
parameterWrapper->set(susHandlingParameters.susRateFilterWeight);
break;
default:
return INVALID_IDENTIFIER_ID;
}
@ -339,26 +348,41 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(safeModeControllerParameters.k_parallelMekf);
break;
case 0x3:
parameterWrapper->set(safeModeControllerParameters.k_orthoNonMekf);
parameterWrapper->set(safeModeControllerParameters.k_orthoGyr);
break;
case 0x4:
parameterWrapper->set(safeModeControllerParameters.k_alignNonMekf);
parameterWrapper->set(safeModeControllerParameters.k_alignGyr);
break;
case 0x5:
parameterWrapper->set(safeModeControllerParameters.k_parallelNonMekf);
parameterWrapper->set(safeModeControllerParameters.k_parallelGyr);
break;
case 0x6:
parameterWrapper->setVector(safeModeControllerParameters.sunTargetDirLeop);
parameterWrapper->set(safeModeControllerParameters.k_orthoSusMgm);
break;
case 0x7:
parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir);
parameterWrapper->set(safeModeControllerParameters.k_alignSusMgm);
break;
case 0x8:
parameterWrapper->set(safeModeControllerParameters.useMekf);
parameterWrapper->set(safeModeControllerParameters.k_parallelSusMgm);
break;
case 0x9:
parameterWrapper->setVector(safeModeControllerParameters.sunTargetDirLeop);
break;
case 0xA:
parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir);
break;
case 0xB:
parameterWrapper->set(safeModeControllerParameters.useMekf);
break;
case 0xC:
parameterWrapper->set(safeModeControllerParameters.useGyr);
break;
case 0xD:
parameterWrapper->set(safeModeControllerParameters.dampingDuringEclipse);
break;
case 0xE:
parameterWrapper->set(safeModeControllerParameters.sineLimitSunRotRate);
break;
default:
return INVALID_IDENTIFIER_ID;
}

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

@ -77,7 +77,8 @@ class AcsParameters : public HasParametersIF {
float mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)};
float mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)};
float mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)};
float mgmDerivativeFilterWeight = 0.5;
float mgmVectorFilterWeight = 0.85;
float mgmDerivativeFilterWeight = 0.85;
} mgmHandlingParameters;
struct SusHandlingParameters {
@ -767,6 +768,8 @@ class AcsParameters : public HasParametersIF {
0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513,
-0.000889232196185857, -0.00168429567131815}};
float susBrightnessThreshold = 0.7;
float susVectorFilterWeight = .85;
float susRateFilterWeight = .85;
} susHandlingParameters;
struct GyrHandlingParameters {
@ -825,15 +828,22 @@ class AcsParameters : public HasParametersIF {
double k_alignMekf = 4.0e-5;
double k_parallelMekf = 3.75e-4;
double k_orthoNonMekf = 4.4e-3;
double k_alignNonMekf = 4.0e-5;
double k_parallelNonMekf = 3.75e-4;
double k_orthoGyr = 4.4e-3;
double k_alignGyr = 4.0e-5;
double k_parallelGyr = 3.75e-4;
double k_orthoSusMgm = 1.1e-2;
double k_alignSusMgm = 2.0e-5;
double k_parallelSusMgm = 4.4e-4;
double sunTargetDirLeop[3] = {0, sqrt(.5), sqrt(.5)};
double sunTargetDir[3] = {0, 0, 1};
uint8_t useMekf = false;
uint8_t useGyr = true;
uint8_t dampingDuringEclipse = true;
float sineLimitSunRotRate = 0.24;
} safeModeControllerParameters;
struct PointingLawParameters {

1
mission/controller/acs/CMakeLists.txt
View File

@ -2,6 +2,7 @@ target_sources(
${LIB_EIVE_MISSION}
PRIVATE AcsParameters.cpp
ActuatorCmd.cpp
FusedRotationEstimation.cpp
Guidance.cpp
Igrf13Model.cpp
MultiplicativeKalmanFilter.cpp

103
mission/controller/acs/FusedRotationEstimation.cpp Normal file
View File

@ -0,0 +1,103 @@
#include "FusedRotationEstimation.h"
FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_) {
acsParameters = acsParameters_;
}
void FusedRotationEstimation::estimateFusedRotationRateSafe(
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) {
if ((not mgmDataProcessed->mgmVecTot.isValid() and not susDataProcessed->susVecTot.isValid() and
not fusedRotRateData->rotRateTotal.isValid()) or
(not susDataProcessed->susVecTotDerivative.isValid() and
not mgmDataProcessed->mgmVecTotDerivative.isValid())) {
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double));
fusedRotRateData->setValidity(false, true);
}
return;
}
if (not susDataProcessed->susVecTot.isValid()) {
estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData);
return;
}
// calculate rotation around the sun
double magSunCross[3] = {0, 0, 0};
VectorOperations<double>::cross(mgmDataProcessed->mgmVecTot.value,
susDataProcessed->susVecTot.value, magSunCross);
double magSunCrossNorm = VectorOperations<double>::norm(magSunCross, 3);
double magNorm = VectorOperations<double>::norm(mgmDataProcessed->mgmVecTot.value, 3);
double fusedRotRateParallel[3] = {0, 0, 0};
if (magSunCrossNorm >
(acsParameters->safeModeControllerParameters.sineLimitSunRotRate * magNorm)) {
double omegaParallel =
VectorOperations<double>::dot(mgmDataProcessed->mgmVecTotDerivative.value, magSunCross) *
pow(magSunCrossNorm, -2);
VectorOperations<double>::mulScalar(susDataProcessed->susVecTot.value, omegaParallel,
fusedRotRateParallel, 3);
} else {
estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData);
return;
}
// calculate rotation orthogonal to the sun
double fusedRotRateOrthogonal[3] = {0, 0, 0};
VectorOperations<double>::cross(susDataProcessed->susVecTotDerivative.value,
susDataProcessed->susVecTot.value, fusedRotRateOrthogonal);
VectorOperations<double>::mulScalar(
fusedRotRateOrthogonal,
pow(VectorOperations<double>::norm(susDataProcessed->susVecTot.value, 3), -2),
fusedRotRateOrthogonal, 3);
// calculate total rotation rate
double fusedRotRateTotal[3] = {0, 0, 0};
VectorOperations<double>::add(fusedRotRateParallel, fusedRotRateOrthogonal, fusedRotRateTotal);
// store for calculation of angular acceleration
if (gyrDataProcessed->gyrVecTot.isValid()) {
std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double));
}
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateOrthogonal,
3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, fusedRotRateParallel, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double));
fusedRotRateData->setValidity(true, true);
}
}
void FusedRotationEstimation::estimateFusedRotationRateEclipse(
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) {
if (not gyrDataProcessed->gyrVecTot.isValid() or
VectorOperations<double>::norm(fusedRotRateData->rotRateTotal.value, 3) == 0) {
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double));
fusedRotRateData->setValidity(false, true);
}
return;
}
double angAccelB[3] = {0, 0, 0};
VectorOperations<double>::subtract(gyrDataProcessed->gyrVecTot.value, rotRateOldB, angAccelB, 3);
double fusedRotRateTotal[3] = {0, 0, 0};
VectorOperations<double>::add(fusedRotRateData->rotRateTotal.value, angAccelB, fusedRotRateTotal,
3);
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(false);
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
fusedRotRateData->rotRateParallel.setValid(false);
std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double));
fusedRotRateData->rotRateTotal.setValid(true);
}
}

29
mission/controller/acs/FusedRotationEstimation.h Normal file
View File

@ -0,0 +1,29 @@
#ifndef MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_
#define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
class FusedRotationEstimation {
public:
FusedRotationEstimation(AcsParameters *acsParameters_);
void estimateFusedRotationRateSafe(acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::FusedRotRateData *fusedRotRateData);
protected:
private:
static constexpr double ZERO_VEC[3] = {0, 0, 0};
AcsParameters *acsParameters;
double rotRateOldB[3] = {0, 0, 0};
void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::FusedRotRateData *fusedRotRateData);
};
#endif /* MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ */

14
mission/controller/acs/SensorProcessing.cpp
View File

@ -132,6 +132,10 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
for (uint8_t i = 0; i < 3; i++) {
mgmVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i];
}
if (VectorOperations<double>::norm(mgmVecTot, 3) != 0 and mgmDataProcessed->mgmVecTot.isValid()) {
lowPassFilter(mgmVecTot, mgmDataProcessed->mgmVecTot.value,
mgmParameters->mgmVectorFilterWeight);
}
//-----------------------Mgm Rate Computation ---------------------------------------------------
double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0};
@ -351,6 +355,11 @@ void SensorProcessing::processSus(
double susVecTot[3] = {0.0, 0.0, 0.0};
VectorOperations<double>::normalize(susMeanValue, susVecTot, 3);
if (VectorOperations<double>::norm(susVecTot, 3) != 0 and susDataProcessed->susVecTot.isValid()) {
lowPassFilter(susVecTot, susDataProcessed->susVecTot.value,
susParameters->susVectorFilterWeight);
}
/* -------- Sun Derivatiative --------------------- */
double susVecTotDerivative[3] = {0.0, 0.0, 0.0};
@ -363,6 +372,11 @@ void SensorProcessing::processSus(
susVecTotDerivativeValid = true;
}
}
if (VectorOperations<double>::norm(susVecTotDerivative, 3) != 0 and
susDataProcessed->susVecTotDerivative.isValid()) {
lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value,
susParameters->susRateFilterWeight);
}
timeOfSavedSusDirEst = timeOfSusMeasurement;
{
PoolReadGuard pg(susDataProcessed);

111
mission/controller/acs/control/SafeCtrl.cpp
View File

@ -9,20 +9,36 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
SafeCtrl::~SafeCtrl() {}
acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool satRotRateValid, const bool sunDirValid,
const uint8_t mekfEnabled,
const uint8_t dampingEnabled) {
acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(
const bool magFieldValid, const bool mekfValid, const bool satRotRateValid,
const bool sunDirValid, const bool fusedRateSplitValid, const bool fusedRateTotalValid,
const uint8_t mekfEnabled, const uint8_t gyrEnabled, const uint8_t dampingEnabled) {
if (not magFieldValid) {
return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (mekfEnabled and mekfValid) {
return acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF;
} else if (satRotRateValid and sunDirValid) {
return acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF;
} else if (dampingEnabled and satRotRateValid and not sunDirValid) {
return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING;
} else if (not dampingEnabled and satRotRateValid and not sunDirValid) {
return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING;
return acs::SafeModeStrategy::SAFECTRL_MEKF;
} else if (sunDirValid) {
if (gyrEnabled and satRotRateValid) {
return acs::SafeModeStrategy::SAFECTRL_GYR;
} else if (not gyrEnabled and fusedRateSplitValid) {
return acs::SafeModeStrategy::SAFECTRL_SUSMGM;
} else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
}
} else if (not sunDirValid) {
if (dampingEnabled) {
if (gyrEnabled and satRotRateValid) {
return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR;
} else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) {
return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM;
} else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
}
} else if (not dampingEnabled and satRotRateValid) {
return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING;
} else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
}
} else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
}
@ -43,8 +59,7 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
errorAngle = acos(dotSun);
splitRotationalRate(satRotRateB, sunDirB);
calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelMekf,
calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelMekf,
acsParameters->safeModeControllerParameters.k_orthoMekf);
calculateAngleErrorTorque(sunDirB, sunDirRefB,
acsParameters->safeModeControllerParameters.k_alignMekf);
@ -57,9 +72,8 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
calculateMagneticMoment(magMomB);
}
void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
const double *sunDirB, const double *sunDirRefB, double *magMomB,
double &errorAngle) {
void SafeCtrl::safeGyr(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
const double *sunDirRefB, double *magMomB, double &errorAngle) {
// convert magFieldB from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
@ -68,11 +82,10 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
errorAngle = acos(dotSun);
splitRotationalRate(satRotRateB, sunDirB);
calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelNonMekf,
acsParameters->safeModeControllerParameters.k_orthoNonMekf);
calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelGyr,
acsParameters->safeModeControllerParameters.k_orthoGyr);
calculateAngleErrorTorque(sunDirB, sunDirRefB,
acsParameters->safeModeControllerParameters.k_alignNonMekf);
acsParameters->safeModeControllerParameters.k_alignGyr);
// sum of all torques
for (uint8_t i = 0; i < 3; i++) {
@ -82,8 +95,33 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
calculateMagneticMoment(magMomB);
}
void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB,
const double *sunDirRefB, double *magMomB, double &errorAngle) {
void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateParallelB,
const double *rotRateOrthogonalB, const double *sunDirB,
const double *sunDirRefB, double *magMomB, double &errorAngle) {
// convert magFieldB from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
// calculate error angle between sunDirRef and sunDir
double dotSun = VectorOperations<double>::dot(sunDirRefB, sunDirB);
errorAngle = acos(dotSun);
std::memcpy(satRotRateParallelB, rotRateParallelB, sizeof(satRotRateParallelB));
std::memcpy(satRotRateOrthogonalB, rotRateOrthogonalB, sizeof(satRotRateOrthogonalB));
calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelSusMgm,
acsParameters->safeModeControllerParameters.k_orthoSusMgm);
calculateAngleErrorTorque(sunDirB, sunDirRefB,
acsParameters->safeModeControllerParameters.k_alignSusMgm);
// sum of all torques
for (uint8_t i = 0; i < 3; i++) {
cmdTorque[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i];
}
calculateMagneticMoment(magMomB);
}
void SafeCtrl::safeRateDampingGyr(const double *magFieldB, const double *satRotRateB,
const double *sunDirRefB, double *magMomB, double &errorAngle) {
// convert magFieldB from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
@ -91,9 +129,28 @@ void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRate
errorAngle = NAN;
splitRotationalRate(satRotRateB, sunDirRefB);
calculateRotationalRateTorque(sunDirRefB, sunDirRefB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelNonMekf,
acsParameters->safeModeControllerParameters.k_orthoNonMekf);
calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelGyr,
acsParameters->safeModeControllerParameters.k_orthoGyr);
// sum of all torques
VectorOperations<double>::add(cmdParallel, cmdOrtho, cmdTorque, 3);
// calculate magnetic moment to command
calculateMagneticMoment(magMomB);
}
void SafeCtrl::safeRateDampingSusMgm(const double *magFieldB, const double *satRotRateB,
const double *sunDirRefB, double *magMomB,
double &errorAngle) {
// convert magFieldB from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
// no error angle available for eclipse
errorAngle = NAN;
splitRotationalRate(satRotRateB, sunDirRefB);
calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelSusMgm,
acsParameters->safeModeControllerParameters.k_orthoSusMgm);
// sum of all torques
VectorOperations<double>::add(cmdParallel, cmdOrtho, cmdTorque, 3);
@ -110,9 +167,7 @@ void SafeCtrl::splitRotationalRate(const double *satRotRateB, const double *sunD
VectorOperations<double>::subtract(satRotRateB, satRotRateParallelB, satRotRateOrthogonalB, 3);
}
void SafeCtrl::calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB,
double &errorAngle, const double gainParallel,
const double gainOrtho) {
void SafeCtrl::calculateRotationalRateTorque(const double gainParallel, const double gainOrtho) {
// calculate torque for parallel rotational rate
VectorOperations<double>::mulScalar(satRotRateParallelB, -gainParallel, cmdParallel, 3);

27
mission/controller/acs/control/SafeCtrl.h
View File

@ -14,23 +14,34 @@ class SafeCtrl {
acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool satRotRateValid, const bool sunDirValid,
const uint8_t mekfEnabled, const uint8_t dampingEnabled);
const bool fusedRateSplitValid,
const bool fusedRateTotalValid, const uint8_t mekfEnabled,
const uint8_t gyrEnabled, const uint8_t dampingEnabled);
void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI,
const double *quatBI, const double *sunDirRefB, double *magMomB,
double &errorAngle);
void safeNonMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
const double *sunDirRefB, double *magMomB, double &errorAngle);
void safeGyr(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
const double *sunDirRefB, double *magMomB, double &errorAngle);
void safeRateDamping(const double *magFieldB, const double *satRotRateB, const double *sunDirRefB,
double *magMomB, double &errorAngle);
void safeSusMgm(const double *magFieldB, const double *rotRateParallelB,
const double *rotRateOrthogonalB, const double *sunDirB, const double *sunDirRefB,
double *magMomB, double &errorAngle);
void safeRateDampingGyr(const double *magFieldB, const double *satRotRateB,
const double *sunDirRefB, double *magMomB, double &errorAngle);
void safeRateDampingSusMgm(const double *magFieldB, const double *satRotRateB,
const double *sunDirRefB, double *magMomB, double &errorAngle);
void splitRotationalRate(const double *satRotRateB, const double *sunDirB);
void calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB,
double &errorAngle, const double gainParallel,
const double gainOrtho);
void calculateRotationalRates(const double *magFieldB, const double *magRateB,
const double *sunDirB, const double *sunRateB,
double *fusedRotRate);
void calculateRotationalRateTorque(const double gainParallel, const double gainOrtho);
void calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB,
const double gainAlign);

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

@ -18,7 +18,8 @@ enum SetIds : uint32_t {
GPS_PROCESSED_DATA,
MEKF_DATA,
CTRL_VAL_DATA,
ACTUATOR_CMD_DATA
ACTUATOR_CMD_DATA,
FUSED_ROTATION_RATE_DATA,
};
enum PoolIds : lp_id_t {
@ -103,6 +104,10 @@ enum PoolIds : lp_id_t {
RW_TARGET_TORQUE,
RW_TARGET_SPEED,
MTQ_TARGET_DIPOLE,
// Fused Rotation Rate
ROT_RATE_ORTHOGONAL,
ROT_RATE_PARALLEL,
ROT_RATE_TOTAL,
};
static constexpr uint8_t MGM_SET_RAW_ENTRIES = 6;
@ -115,6 +120,7 @@ static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 5;
static constexpr uint8_t MEKF_SET_ENTRIES = 3;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 3;
/**
* @brief Raw MGM sensor data. Includes the IMTQ sensor data and actuator status.
@ -273,6 +279,19 @@ class ActuatorCmdData : public StaticLocalDataSet<ACT_CMD_SET_ENTRIES> {
private:
};
class FusedRotRateData : public StaticLocalDataSet<FUSED_ROT_RATE_SET_ENTRIES> {
public:
FusedRotRateData(HasLocalDataPoolIF* hkOwner)
: StaticLocalDataSet(hkOwner, FUSED_ROTATION_RATE_DATA) {}
lp_vec_t<double, 3> rotRateOrthogonal =
lp_vec_t<double, 3>(sid.objectId, ROT_RATE_ORTHOGONAL, this);
lp_vec_t<double, 3> rotRateParallel = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_PARALLEL, this);
lp_vec_t<double, 3> rotRateTotal = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL, this);
private:
};
} // namespace acsctrl
#endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ */

2
tmtc

@ -1 +1 @@
Subproject commit a82cbff5a83eb37c68234bdeecd3b7d308d65eb1
Subproject commit 15716c988b6d26ae7f00e44b919d5ae7505d81ad