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

First Version of ACS Controller #329

Merged
muellerr merged 106 commits from acs-ctrl-v1 into develop 2022-12-02 16:21:58 +01:00
Conversation 0 Commits 106 Files Changed 50 +590 -522
24 changed files with 590 additions and 522 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit a13ccb43d2 - Show all commits

162
mission/controller/AcsController.cpp
View File

@ -20,6 +20,7 @@ AcsController::AcsController(object_id_t objectId)
gyrDataProcessed(this), gyrDataProcessed(this),
gpsDataProcessed(this), gpsDataProcessed(this),
mekfData(this), mekfData(this),
ctrlValData(this),
actuatorCmdData(this) {} actuatorCmdData(this) {}
ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
@ -48,7 +49,9 @@ void AcsController::performControlOperation() {
case SUBMODE_DETUMBLE: case SUBMODE_DETUMBLE:
performDetumble(); performDetumble();
break; break;
case SUBMODE_PTG_GS: case SUBMODE_PTG_TARGET:
case SUBMODE_PTG_NADIR:
case SUBMODE_PTG_INERTIAL:
performPointingCtrl(); performPointingCtrl();
break; break;
} }
@ -80,7 +83,7 @@ void AcsController::performControlOperation() {
// DEBUG : REMOVE AFTER COMPLETION // DEBUG : REMOVE AFTER COMPLETION
mode = MODE_ON; mode = MODE_ON;
submode = SUBMODE_DETUMBLE; submode = SUBMODE_SAFE;
// DEBUG END // DEBUG END
} }
@ -90,7 +93,6 @@ void AcsController::performSafe() {
// another place since we have to do it for every mode regardless of safe or not // another place since we have to do it for every mode regardless of safe or not
ACS::SensorValues sensorValues; ACS::SensorValues sensorValues;
ACS::OutputValues outputValues;
timeval now; timeval now;
Clock::getClock_timeval(&now); Clock::getClock_timeval(&now);
@ -98,38 +100,54 @@ void AcsController::performSafe() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters); &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t validMekf; ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &outputValues, &validMekf); navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
&mekfData, &validMekf);
// Give desired satellite rate and sun direction to align // Give desired satellite rate and sun direction to align
double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0}; double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
// IF MEKF is working // IF MEKF is working
double magMomMtq[3] = {0, 0, 0}; double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
bool magMomMtqValid = false; bool magMomMtqValid = false;
if (validMekf == returnvalue::OK) { if (validMekf == returnvalue::OK) {
safeCtrl.safeMekf(now, (outputValues.quatMekfBJ), &(outputValues.quatMekfBJValid), safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(),
(outputValues.magFieldModel), &(outputValues.magFieldModelValid), mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(),
(outputValues.sunDirModel), &(outputValues.sunDirModelValid), susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(),
(outputValues.satRateMekf), &(outputValues.satRateMekfValid), sunTargetDir, mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(),
satRateSafe, magMomMtq, &magMomMtqValid); sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
} else { } else {
safeCtrl.safeNoMekf(now, outputValues.sunDirEst, &outputValues.sunDirEstValid, safeCtrl.safeNoMekf(
outputValues.sunVectorDerivative, &(outputValues.sunVectorDerivativeValid), now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(),
outputValues.magFieldEst, &(outputValues.magFieldEstValid), susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.isValid(),
outputValues.magneticFieldVectorDerivative, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
&(outputValues.magneticFieldVectorDerivativeValid), sunTargetDir, mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(),
satRateSafe, magMomMtq, &magMomMtqValid); sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
} }
double dipolCmdUnits[3] = {0, 0, 0}; double dipolCmdUnits[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
{
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
double zeroQuat[4] = {0, 0, 0, 0};
std::memcpy(ctrlValData.tgtQuat.value, zeroQuat, 4 * sizeof(double));
ctrlValData.tgtQuat.setValid(false);
std::memcpy(ctrlValData.errQuat.value, zeroQuat, 4 * sizeof(double));
ctrlValData.errQuat.setValid(false);
ctrlValData.errAng.value = errAng;
ctrlValData.errAng.setValid(true);
ctrlValData.setValidity(true, false);
}
}
// Detumble check and switch // Detumble check and switch
if (outputValues.satRateMekfValid && VectorOperations<double>::norm(outputValues.satRateMekf, 3) > if (mekfData.satRotRateMekf.isValid() &&
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) { acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++; detumbleCounter++;
} else if (outputValues.satRateEstValid && } else if (gyrDataProcessed.gyrVecTot.isValid() &&
VectorOperations<double>::norm(outputValues.satRateEst, 3) > VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) { acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++; detumbleCounter++;
} else { } else {
@ -139,12 +157,31 @@ void AcsController::performSafe() {
submode = SUBMODE_DETUMBLE; submode = SUBMODE_DETUMBLE;
detumbleCounter = 0; detumbleCounter = 0;
} }
// commanding.magnetorquesDipol();
{
PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) {
double zeroVec[3] = {0, 0, 0, 0};
std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(double));
actuatorCmdData.rwTargetTorque.setValid(false);
std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(double));
actuatorCmdData.rwTargetSpeed.setValid(false);
std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(double));
actuatorCmdData.mtqTargetDipole.setValid(true);
actuatorCmdData.setValidity(true, false);
}
}
// {
// PoolReadGuard pg(&dipoleSet);
// MutexGuard mg(torquer::lazyLock());
// torquer::NEW_ACTUATION_FLAG = true;
// dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2],
//torqueDuration);
// }
} }
void AcsController::performDetumble() { void AcsController::performDetumble() {
ACS::SensorValues sensorValues; ACS::SensorValues sensorValues;
ACS::OutputValues outputValues;
timeval now; timeval now;
Clock::getClock_timeval(&now); Clock::getClock_timeval(&now);
@ -152,20 +189,22 @@ void AcsController::performDetumble() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters); &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t validMekf; ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &outputValues, &validMekf); navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
&mekfData, &validMekf);
double magMomMtq[3] = {0, 0, 0}; double magMomMtq[3] = {0, 0, 0};
detumble.bDotLaw(outputValues.magneticFieldVectorDerivative, detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
&outputValues.magneticFieldVectorDerivativeValid, outputValues.magFieldEst, mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
&outputValues.magFieldEstValid, magMomMtq); mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
double dipolCmdUnits[3] = {0, 0, 0}; double dipolCmdUnits[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
if (outputValues.satRateMekfValid && VectorOperations<double>::norm(outputValues.satRateMekf, 3) < if (mekfData.satRotRateMekf.isValid() &&
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) { acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++; detumbleCounter++;
} else if (outputValues.satRateEstValid && } else if (gyrDataProcessed.gyrVecTot.isValid() &&
VectorOperations<double>::norm(outputValues.satRateEst, 3) < VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) { acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++; detumbleCounter++;
} else { } else {
@ -175,11 +214,31 @@ void AcsController::performDetumble() {
submode = SUBMODE_SAFE; submode = SUBMODE_SAFE;
detumbleCounter = 0; detumbleCounter = 0;
} }
{
PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) {
double zeroVec[3] = {0, 0, 0, 0};
std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(double));
actuatorCmdData.rwTargetTorque.setValid(false);
std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(double));
actuatorCmdData.rwTargetSpeed.setValid(false);
std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(double));
actuatorCmdData.mtqTargetDipole.setValid(true);
actuatorCmdData.setValidity(true, false);
}
}
// {
// PoolReadGuard pg(&dipoleSet);
// MutexGuard mg(torquer::lazyLock());
// torquer::NEW_ACTUATION_FLAG = true;
// dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2],
// torqueDuration);
// }
} }
void AcsController::performPointingCtrl() { void AcsController::performPointingCtrl() {
ACS::SensorValues sensorValues; ACS::SensorValues sensorValues;
ACS::OutputValues outputValues;
timeval now; timeval now;
Clock::getClock_timeval(&now); Clock::getClock_timeval(&now);
@ -187,12 +246,14 @@ void AcsController::performPointingCtrl() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters); &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t validMekf; ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &outputValues, &validMekf); navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
&mekfData, &validMekf);
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); guidance.targetQuatPtg(&sensorValues, &mekfData, &susDataProcessed, now, targetQuat, refSatRate);
double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0}; double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
guidance.comparePtg(targetQuat, &outputValues, refSatRate, quatError, deltaRate); deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed
guidance.comparePtg(targetQuat, &mekfData, refSatRate, quatErrorComplete, quatError, deltaRate);
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0; double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0;
@ -207,11 +268,29 @@ void AcsController::performPointingCtrl() {
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torquePtgRws, &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torquePtgRws,
rwTrqNs, cmdSpeedRws); rwTrqNs, cmdSpeedRws);
double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
outputValues.magFieldEst, &outputValues.magFieldEstValid, outputValues.satRateMekf, mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
{
PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double));
std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdSpeedRws, 4 * sizeof(double));
std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolUnits, 3 * sizeof(double));
actuatorCmdData.setValidity(true, true);
}
}
// {
// PoolReadGuard pg(&dipoleSet);
// MutexGuard mg(torquer::lazyLock());
// torquer::NEW_ACTUATION_FLAG = true;
// dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2],
// torqueDuration);
// }
} }
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
@ -286,6 +365,11 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf); localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf); localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf);
poolManager.subscribeForRegularPeriodicPacket({mekfData.getSid(), false, 5.0}); poolManager.subscribeForRegularPeriodicPacket({mekfData.getSid(), false, 5.0});
// Ctrl Values
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng);
poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), false, 5.0});
// Actuator CMD // Actuator CMD
localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque); localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque);
localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed); localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed);
@ -312,6 +396,8 @@ LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
return &gpsDataProcessed; return &gpsDataProcessed;
case acsctrl::MEKF_DATA: case acsctrl::MEKF_DATA:
return &mekfData; return &mekfData;
case acsctrl::CTRL_VAL_DATA:
return &ctrlValData;
case acsctrl::ACTUATOR_CMD_DATA: case acsctrl::ACTUATOR_CMD_DATA:
return &actuatorCmdData; return &actuatorCmdData;
default: default:
@ -328,7 +414,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
return INVALID_SUBMODE; return INVALID_SUBMODE;
} }
} else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) { } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) {
if ((submode > 5) || (submode < 2)) { if ((submode > 6) || (submode < 2)) {
return INVALID_SUBMODE; return INVALID_SUBMODE;
} else { } else {
return returnvalue::OK; return returnvalue::OK;

18
mission/controller/AcsController.h
View File

@ -8,7 +8,6 @@
#include "acs/ActuatorCmd.h" #include "acs/ActuatorCmd.h"
#include "acs/Guidance.h" #include "acs/Guidance.h"
#include "acs/Navigation.h" #include "acs/Navigation.h"
#include "acs/OutputValues.h"
#include "acs/SensorProcessing.h" #include "acs/SensorProcessing.h"
#include "acs/control/Detumble.h" #include "acs/control/Detumble.h"
#include "acs/control/PtgCtrl.h" #include "acs/control/PtgCtrl.h"
@ -27,8 +26,9 @@ class AcsController : public ExtendedControllerBase {
static const Submode_t SUBMODE_SAFE = 2; static const Submode_t SUBMODE_SAFE = 2;
static const Submode_t SUBMODE_DETUMBLE = 3; static const Submode_t SUBMODE_DETUMBLE = 3;
static const Submode_t SUBMODE_PTG_GS = 4; static const Submode_t SUBMODE_PTG_TARGET = 4;
static const Submode_t SUBMODE_PTG_NADIR = 5; static const Submode_t SUBMODE_PTG_NADIR = 5;
static const Submode_t SUBMODE_PTG_INERTIAL = 6;
protected: protected:
void performSafe(); void performSafe();
@ -83,7 +83,7 @@ class AcsController : public ExtendedControllerBase {
PoolEntry<float> mgm3VecProc = PoolEntry<float>(3); PoolEntry<float> mgm3VecProc = PoolEntry<float>(3);
PoolEntry<float> mgm4VecProc = PoolEntry<float>(3); PoolEntry<float> mgm4VecProc = PoolEntry<float>(3);
PoolEntry<double> mgmVecTot = PoolEntry<double>(3); PoolEntry<double> mgmVecTot = PoolEntry<double>(3);
PoolEntry<float> mgmVecTotDer = PoolEntry<float>(3); PoolEntry<double> mgmVecTotDer = PoolEntry<double>(3);
PoolEntry<double> magIgrf = PoolEntry<double>(3); PoolEntry<double> magIgrf = PoolEntry<double>(3);
// SUSs // SUSs
@ -115,9 +115,9 @@ class AcsController : public ExtendedControllerBase {
PoolEntry<float> sus9VecProc = PoolEntry<float>(3); PoolEntry<float> sus9VecProc = PoolEntry<float>(3);
PoolEntry<float> sus10VecProc = PoolEntry<float>(3); PoolEntry<float> sus10VecProc = PoolEntry<float>(3);
PoolEntry<float> sus11VecProc = PoolEntry<float>(3); PoolEntry<float> sus11VecProc = PoolEntry<float>(3);
PoolEntry<float> susVecTot = PoolEntry<float>(3); PoolEntry<double> susVecTot = PoolEntry<double>(3);
PoolEntry<float> susVecTotDer = PoolEntry<float>(3); PoolEntry<double> susVecTotDer = PoolEntry<double>(3);
PoolEntry<float> sunIjk = PoolEntry<float>(3); PoolEntry<double> sunIjk = PoolEntry<double>(3);
// GYRs // GYRs
acsctrl::GyrDataRaw gyrDataRaw; acsctrl::GyrDataRaw gyrDataRaw;
@ -144,6 +144,12 @@ class AcsController : public ExtendedControllerBase {
PoolEntry<double> quatMekf = PoolEntry<double>(4); PoolEntry<double> quatMekf = PoolEntry<double>(4);
PoolEntry<double> satRotRateMekf = PoolEntry<double>(3); PoolEntry<double> satRotRateMekf = PoolEntry<double>(3);
// Ctrl Values
acsctrl::CtrlValData ctrlValData;
PoolEntry<double> tgtQuat = PoolEntry<double>(4);
PoolEntry<double> errQuat = PoolEntry<double>(4);
PoolEntry<double> errAng = PoolEntry<double>();
// Actuator CMD // Actuator CMD
acsctrl::ActuatorCmdData actuatorCmdData; acsctrl::ActuatorCmdData actuatorCmdData;
PoolEntry<double> rwTargetTorque = PoolEntry<double>(4); PoolEntry<double> rwTargetTorque = PoolEntry<double>(4);

28
mission/controller/acs/ActuatorCmd.cpp
View File

@ -5,23 +5,21 @@
* Author: Robin Marquardt * Author: Robin Marquardt
*/ */
#include "ActuatorCmd.h" #include "ActuatorCmd.h"
#include "util/MathOperations.h"
#include "util/CholeskyDecomposition.h"
#include <cmath>
#include <fsfw/globalfunctions/constants.h> #include <fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h> #include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h> #include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) { #include <cmath>
acsParameters = *acsParameters_;
}
ActuatorCmd::~ActuatorCmd(){ #include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
} ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; }
ActuatorCmd::~ActuatorCmd() {}
void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1,
const int32_t *speedRw2, const int32_t *speedRw3, const int32_t *speedRw2, const int32_t *speedRw3,
@ -57,10 +55,10 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1
} }
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits) { void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits) {
// Convert to Unit frame
// Convert to Unit frame MatrixOperations<double>::multiply(*acsParameters.magnetorquesParameter.inverseAlignment,
MatrixOperations<double>::multiply(*acsParameters.magnetorquesParameter.inverseAlignment, dipolMoment, dipolMomentUnits, 3, 3, 1); dipolMoment, dipolMomentUnits, 3, 3, 1);
// Scaling along largest element if dipol exceeds maximum // Scaling along largest element if dipol exceeds maximum
double maxDipol = acsParameters.magnetorquesParameter.DipolMax; double maxDipol = acsParameters.magnetorquesParameter.DipolMax;
double maxValue = 0; double maxValue = 0;
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
@ -70,9 +68,7 @@ void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnit
} }
if (maxValue > maxDipol) { if (maxValue > maxDipol) {
double scalingFactor = maxDipol / maxValue; double scalingFactor = maxDipol / maxValue;
VectorOperations<double>::mulScalar(dipolMomentUnits, scalingFactor, dipolMomentUnits, 3); VectorOperations<double>::mulScalar(dipolMomentUnits, scalingFactor, dipolMomentUnits, 3);
} }
} }

28
mission/controller/acs/ActuatorCmd.h
View File

@ -8,29 +8,28 @@
#ifndef ACTUATORCMD_H_ #ifndef ACTUATORCMD_H_
#define ACTUATORCMD_H_ #define ACTUATORCMD_H_
#include "AcsParameters.h" #include "AcsParameters.h"
#include "SensorProcessing.h"
#include "MultiplicativeKalmanFilter.h" #include "MultiplicativeKalmanFilter.h"
#include "SensorProcessing.h"
#include "SensorValues.h" #include "SensorValues.h"
#include "OutputValues.h"
class ActuatorCmd{ class ActuatorCmd {
public: public:
ActuatorCmd(AcsParameters *acsParameters_); //Input mode ? ActuatorCmd(AcsParameters *acsParameters_); // Input mode ?
virtual ~ActuatorCmd(); virtual ~ActuatorCmd();
/* /*
* @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction wheels, also * @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction
* will calculate the needed revolutions per minute for the RWs, which will be given * wheels, also will calculate the needed revolutions per minute for the RWs, which will be given
* as Input to the RWs * as Input to the RWs
* @param: rwTrqIn given torque from pointing controller * @param: rwTrqIn given torque from pointing controller
* rwTrqNS Nullspace torque * rwTrqNS Nullspace torque
* rwCmdSpeed output revolutions per minute for every reaction wheel * rwCmdSpeed output revolutions per minute for every
* reaction wheel
*/ */
void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
const int32_t *speedRw2, const int32_t *speedRw3, const double *rwTrqIn, const int32_t *speedRw3, const double *rwTrqIn, const double *rwTrqNS,
const double *rwTrqNS, double *rwCmdSpeed); double *rwCmdSpeed);
/* /*
* @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques
@ -40,9 +39,8 @@ public:
*/ */
void cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits); void cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits);
protected: protected:
private:
private:
AcsParameters acsParameters; AcsParameters acsParameters;
}; };

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

@ -6,7 +6,6 @@ target_sources(
Igrf13Model.cpp Igrf13Model.cpp
MultiplicativeKalmanFilter.cpp MultiplicativeKalmanFilter.cpp
Navigation.cpp Navigation.cpp
OutputValues.cpp
SensorProcessing.cpp SensorProcessing.cpp
SensorValues.cpp SensorValues.cpp
SusConverter.cpp) SusConverter.cpp)

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

@ -7,6 +7,7 @@
#include "Guidance.h" #include "Guidance.h"
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h> #include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h> #include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h> #include <fsfw/globalfunctions/math/VectorOperations.h>
@ -29,8 +30,9 @@ void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3
// memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24); // memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24);
} }
void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
timeval now, double targetQuat[4], double refSatRate[3]) { acsctrl::SusDataProcessed *susDataProcessed, timeval now,
double targetQuat[4], double refSatRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion to groundstation // Calculation of target quaternion to groundstation
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
@ -77,10 +79,8 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double quatBJ[4] = {0, 0, 0, 0}; double quatBJ[4] = {0, 0, 0, 0};
quatBJ[0] = outputValues->quatMekfBJ[0]; std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
quatBJ[1] = outputValues->quatMekfBJ[1];
quatBJ[2] = outputValues->quatMekfBJ[2];
quatBJ[3] = outputValues->quatMekfBJ[3];
QuaternionOperations::toDcm(quatBJ, dcmBJ); QuaternionOperations::toDcm(quatBJ, dcmBJ);
MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3); MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
@ -137,23 +137,16 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues
double sunDirJ[3] = {0, 0, 0}; double sunDirJ[3] = {0, 0, 0};
double sunDirB[3] = {0, 0, 0}; double sunDirB[3] = {0, 0, 0};
if (outputValues->sunDirModelValid) { if (susDataProcessed->sunIjkModel.isValid()) {
sunDirJ[0] = outputValues->sunDirModel[0]; std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
sunDirJ[1] = outputValues->sunDirModel[1];
sunDirJ[2] = outputValues->sunDirModel[2];
MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1); MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
} } else {
std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double));
else {
sunDirB[0] = outputValues->sunDirEst[0];
sunDirB[1] = outputValues->sunDirEst[1];
sunDirB[2] = outputValues->sunDirEst[2];
} }
double exclAngle = acsParameters.strParameters.exclusionAngle, double exclAngle = acsParameters.strParameters.exclusionAngle,
blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart, blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart,
blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop; blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop;
double sightAngleSun = double sightAngleSun =
VectorOperations<double>::dot(acsParameters.strParameters.boresightAxis, sunDirB); VectorOperations<double>::dot(acsParameters.strParameters.boresightAxis, sunDirB);
@ -190,8 +183,8 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues
} }
} }
void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues, void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3],
double refSatRate[3], double quatError[3], double deltaRate[3]) { double quatErrorComplete[4], double quatError[3], double deltaRate[3]) {
double quatRef[4] = {0, 0, 0, 0}; double quatRef[4] = {0, 0, 0, 0};
quatRef[0] = acsParameters.targetModeControllerParameters.quatRef[0]; quatRef[0] = acsParameters.targetModeControllerParameters.quatRef[0];
quatRef[1] = acsParameters.targetModeControllerParameters.quatRef[1]; quatRef[1] = acsParameters.targetModeControllerParameters.quatRef[1];
@ -199,9 +192,7 @@ void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues,
quatRef[3] = acsParameters.targetModeControllerParameters.quatRef[3]; quatRef[3] = acsParameters.targetModeControllerParameters.quatRef[3];
double satRate[3] = {0, 0, 0}; double satRate[3] = {0, 0, 0};
satRate[0] = outputValues->satRateMekf[0]; std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double));
satRate[1] = outputValues->satRateMekf[1];
satRate[2] = outputValues->satRateMekf[2];
VectorOperations<double>::subtract(satRate, refSatRate, deltaRate, 3); VectorOperations<double>::subtract(satRate, refSatRate, deltaRate, 3);
// valid checks ? // valid checks ?
double quatErrorMtx[4][4] = {{quatRef[3], quatRef[2], -quatRef[1], -quatRef[0]}, double quatErrorMtx[4][4] = {{quatRef[3], quatRef[2], -quatRef[1], -quatRef[0]},
@ -209,7 +200,6 @@ void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues,
{quatRef[1], -quatRef[0], quatRef[3], -quatRef[2]}, {quatRef[1], -quatRef[0], quatRef[3], -quatRef[2]},
{quatRef[0], -quatRef[1], quatRef[2], quatRef[3]}}; {quatRef[0], -quatRef[1], quatRef[2], quatRef[3]}};
double quatErrorComplete[4] = {0, 0, 0, 0};
MatrixOperations<double>::multiply(*quatErrorMtx, targetQuat, quatErrorComplete, 4, 4, 1); MatrixOperations<double>::multiply(*quatErrorMtx, targetQuat, quatErrorComplete, 4, 4, 1);
if (quatErrorComplete[3] < 0) { if (quatErrorComplete[3] < 0) {
@ -225,8 +215,8 @@ void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues,
} }
void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) { void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) {
if (sensorValues->rw1Set.isValid() && sensorValues->rw2Set.isValid() && sensorValues->rw3Set.isValid() && if (sensorValues->rw1Set.isValid() && sensorValues->rw2Set.isValid() &&
sensorValues->rw4Set.isValid()) { sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) {
rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0]; rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1]; rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2]; rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];

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

@ -8,33 +8,35 @@
#ifndef GUIDANCE_H_ #ifndef GUIDANCE_H_
#define GUIDANCE_H_ #define GUIDANCE_H_
#include "AcsParameters.h"
#include "SensorValues.h"
#include "OutputValues.h"
#include <time.h> #include <time.h>
#include "../controllerdefinitions/AcsCtrlDefinitions.h"
#include "AcsParameters.h"
#include "SensorValues.h"
class Guidance { class Guidance {
public: public:
Guidance(AcsParameters *acsParameters_); Guidance(AcsParameters *acsParameters_);
virtual ~Guidance(); virtual ~Guidance();
void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]); void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]);
// Function to get the target quaternion and refence rotation rate from gps position and position of the ground station // Function to get the target quaternion and refence rotation rate from gps position and position
void targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, // of the ground station
double targetQuat[4], double refSatRate[3]); void targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed, timeval now, double targetQuat[4],
double refSatRate[3]);
// @note: compares target Quaternion and reference quaternion, also actual satellite rate and desired // @note: compares target Quaternion and reference quaternion, also actual satellite rate and
void comparePtg( double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3] ); // desired
void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3],
double quatErrorComplete[4], double quatError[3], double deltaRate[3]);
// @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid reation wheel // @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
// maybe can be done in "commanding.h" // reation wheel maybe can be done in "commanding.h"
void getDistributionMatrixRw(ACS::SensorValues* sensorValues, double *rwPseudoInv); void getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);
private:
private:
AcsParameters acsParameters; AcsParameters acsParameters;
bool strBlindAvoidFlag = false; bool strBlindAvoidFlag = false;
}; };

88
mission/controller/acs/Igrf13Model.cpp
View File

@ -6,36 +6,34 @@
*/ */
#include "Igrf13Model.h" #include "Igrf13Model.h"
#include "util/MathOperations.h"
#include <math.h>
#include <stdint.h>
#include <string.h>
//#include <time.h>
#include <fsfw/globalfunctions/constants.h> #include <fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h> #include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h> #include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h> #include <fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h>
#include <stdint.h>
#include <string.h>
#include "util/MathOperations.h"
Igrf13Model::Igrf13Model(){ Igrf13Model::Igrf13Model() {}
} Igrf13Model::~Igrf13Model() {}
Igrf13Model::~Igrf13Model(){
}
void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
const double altitude, timeval timeOfMagMeasurement, double* magFieldModelInertial) { const double altitude, timeval timeOfMagMeasurement,
double* magFieldModelInertial) {
double phi = longitude, theta = gcLatitude; //geocentric double phi = longitude, theta = gcLatitude; // geocentric
/* Here is the co-latitude needed*/ /* Here is the co-latitude needed*/
theta -= 90*Math::PI/180; theta -= 90 * Math::PI / 180;
theta *= (-1); theta *= (-1);
double rE = 6371200.0; // radius earth [m] double rE = 6371200.0; // radius earth [m]
/* Predefine recursive associated Legendre polynomials */ /* Predefine recursive associated Legendre polynomials */
double P11 = 1; double P11 = 1;
double P10 = P11; //P10 = P(n-1,m-0) double P10 = P11; // P10 = P(n-1,m-0)
double dP11 = 0; //derivative double dP11 = 0; // derivative
double dP10 = dP11; //derivative double dP10 = dP11; // derivative
double P2 = 0, dP2 = 0, P20 = 0, dP20 = 0, K = 0; double P2 = 0, dP2 = 0, P20 = 0, dP20 = 0, K = 0;
@ -60,8 +58,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
dP20 = dP10; dP20 = dP10;
dP10 = dP2; dP10 = dP2;
} else { } else {
K = (pow((n - 1), 2) - pow(m, 2)) K = (pow((n - 1), 2) - pow(m, 2)) / ((2 * n - 1) * (2 * n - 3));
/ ((2 * n - 1) * (2 * n - 3));
P2 = cos(theta) * P10 - K * P20; P2 = cos(theta) * P10 - K * P20;
dP2 = cos(theta) * dP10 - sin(theta) * P10 - K * dP20; dP2 = cos(theta) * dP10 - sin(theta) * P10 - K * dP20;
P20 = P10; P20 = P10;
@ -70,14 +67,17 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
dP10 = dP2; dP10 = dP2;
} }
/* gradient of scalar potential towards radius */ /* gradient of scalar potential towards radius */
magFieldModel[0]+=pow(rE/(altitude+rE),(n+2))*(n+1)* magFieldModel[0] +=
((updatedG[m][n-1]*cos(m*phi) + updatedH[m][n-1]*sin(m*phi))*P2); pow(rE / (altitude + rE), (n + 2)) * (n + 1) *
((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * P2);
/* gradient of scalar potential towards phi */ /* gradient of scalar potential towards phi */
magFieldModel[1]+=pow(rE/(altitude+rE),(n+2))* magFieldModel[1] +=
((updatedG[m][n-1]*cos(m*phi) + updatedH[m][n-1]*sin(m*phi))*dP2); pow(rE / (altitude + rE), (n + 2)) *
((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * dP2);
/* gradient of scalar potential towards theta */ /* gradient of scalar potential towards theta */
magFieldModel[2]+=pow(rE/(altitude+rE),(n+2))* magFieldModel[2] +=
((-updatedG[m][n-1]*sin(m*phi) + updatedH[m][n-1]*cos(m*phi))*P2*m); pow(rE / (altitude + rE), (n + 2)) *
((-updatedG[m][n - 1] * sin(m * phi) + updatedH[m][n - 1] * cos(m * phi)) * P2 * m);
} }
} }
} }
@ -86,40 +86,40 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
magFieldModel[2] *= (-1 / sin(theta)); magFieldModel[2] *= (-1 / sin(theta));
/* Next step: transform into inertial KOS (IJK)*/ /* Next step: transform into inertial KOS (IJK)*/
//Julean Centuries // Julean Centuries
double JD2000Floor = 0; double JD2000Floor = 0;
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement); double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
JD2000Floor = floor(JD2000); JD2000Floor = floor(JD2000);
double JC2000 = JD2000Floor / 36525; double JC2000 = JD2000Floor / 36525;
double gst = 100.4606184 + 36000.77005361 * JC2000 + 0.00038793 * pow(JC2000,2) double gst = 100.4606184 + 36000.77005361 * JC2000 + 0.00038793 * pow(JC2000, 2) -
- 0.000000026 * pow(JC2000,3); //greenwich sidereal time 0.000000026 * pow(JC2000, 3); // greenwich sidereal time
gst *= PI/180; //convert to radians gst *= PI / 180; // convert to radians
double sec = (JD2000 - JD2000Floor) * 86400; // Seconds on this day (Universal time) // FROM GPS ? double sec =
(JD2000 - JD2000Floor) * 86400; // Seconds on this day (Universal time) // FROM GPS ?
double omega0 = 0.00007292115; // mean angular velocity earth [rad/s] double omega0 = 0.00007292115; // mean angular velocity earth [rad/s]
gst +=omega0 * sec; gst += omega0 * sec;
double lst = gst + longitude; //local sidereal time [rad] double lst = gst + longitude; // local sidereal time [rad]
magFieldModelInertial[0] = magFieldModel[0] * cos(theta) +
magFieldModel[1] * sin(theta) * cos(lst) - magFieldModel[1] * sin(lst);
magFieldModelInertial[0] = magFieldModel[0] * cos(theta) + magFieldModel[1] * sin(theta)*cos(lst) - magFieldModel[1] * sin(lst); magFieldModelInertial[1] = magFieldModel[0] * cos(theta) +
magFieldModelInertial[1] = magFieldModel[0] * cos(theta) + magFieldModel[1] * sin(theta)*sin(lst) + magFieldModel[1] * cos(lst); magFieldModel[1] * sin(theta) * sin(lst) + magFieldModel[1] * cos(lst);
magFieldModelInertial[2] = magFieldModel[0] * sin(theta) + magFieldModel[1] * cos(lst); magFieldModelInertial[2] = magFieldModel[0] * sin(theta) + magFieldModel[1] * cos(lst);
double normVecMagFieldInert[3] = {0,0,0}; double normVecMagFieldInert[3] = {0, 0, 0};
VectorOperations<double>::normalize(magFieldModelInertial, normVecMagFieldInert, 3); VectorOperations<double>::normalize(magFieldModelInertial, normVecMagFieldInert, 3);
} }
void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement){ void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) {
double JD2000Igrf = (2458850.0 - 2451545); // Begin of IGRF-13 (2020-01-01,00:00:00) in JD2000
double JD2000Igrf = (2458850.0-2451545); //Begin of IGRF-13 (2020-01-01,00:00:00) in JD2000
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement); double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
double days = ceil(JD2000-JD2000Igrf); double days = ceil(JD2000 - JD2000Igrf);
for(int i = 0;i <= igrfOrder; i++){ for (int i = 0; i <= igrfOrder; i++) {
for(int j = 0;j <= (igrfOrder-1); j++){ for (int j = 0; j <= (igrfOrder - 1); j++) {
updatedG[i][j] = coeffG[i][j] + svG[i][j] * (days/365); updatedG[i][j] = coeffG[i][j] + svG[i][j] * (days / 365);
updatedH[i][j] = coeffH[i][j] + svH[i][j] * (days/365); updatedH[i][j] = coeffH[i][j] + svH[i][j] * (days / 365);
} }
} }
} }

126
mission/controller/acs/Igrf13Model.h
View File

@ -16,25 +16,26 @@
#ifndef IGRF13MODEL_H_ #ifndef IGRF13MODEL_H_
#define IGRF13MODEL_H_ #define IGRF13MODEL_H_
#include <cmath> #include <fsfw/parameters/HasParametersIF.h>
#include <stdint.h> #include <stdint.h>
#include <string.h> #include <string.h>
#include <time.h> #include <time.h>
#include <fsfw/parameters/HasParametersIF.h>
#include <cmath>
// Output should be transformed to [T] instead of [nT] // Output should be transformed to [T] instead of [nT]
// Updating Coefficients has to be implemented yet. Question, updating everyday ? // Updating Coefficients has to be implemented yet. Question, updating everyday ?
class Igrf13Model/*:public HasParametersIF*/{ class Igrf13Model /*:public HasParametersIF*/ {
public:
public:
Igrf13Model(); Igrf13Model();
virtual ~Igrf13Model(); virtual ~Igrf13Model();
// Main Function // Main Function
void magFieldComp(const double longitude, const double gcLatitude, const double altitude, timeval timeOfMagMeasurement,double* magFieldModelInertial); void magFieldComp(const double longitude, const double gcLatitude, const double altitude,
// Right now the radius for igrf is simply with r0 + altitude calculated. In reality the radius is oriented from the satellite to earth COM timeval timeOfMagMeasurement, double* magFieldModelInertial);
// Difference up to 25 km, which is 5 % of the total flight altitude // Right now the radius for igrf is simply with r0 + altitude calculated. In reality the radius is
// oriented from the satellite to earth COM Difference up to 25 km, which is 5 % of the total
// flight altitude
/* Inputs: /* Inputs:
* - longitude: geocentric longitude [rad] * - longitude: geocentric longitude [rad]
* - latitude: geocentric latitude [rad] * - latitude: geocentric latitude [rad]
@ -44,50 +45,52 @@ public:
* Outputs: * Outputs:
* - magFieldModelInertial: Magnetic Field Vector in IJK KOS [nT]*/ * - magFieldModelInertial: Magnetic Field Vector in IJK KOS [nT]*/
// Coefficient wary over year, could be updated sometimes. // Coefficient wary over year, could be updated sometimes.
void updateCoeffGH(timeval timeOfMagMeasurement); //Secular variation (SV) void updateCoeffGH(timeval timeOfMagMeasurement); // Secular variation (SV)
double magFieldModel[3]; double magFieldModel[3];
private: private:
const double coeffG[14][13] = {{-29404.8,-2499.6, 1363.2, 903.0,-234.3, 66.0, 80.6, 23.7, 5.0,-1.9, 3.0,-2.0, 0.1}, const double coeffG[14][13] = {
{ -1450.9, 2982.0,-2381.2, 809.5, 363.2, 65.5,-76.7, 9.7, 8.4,-6.2,-1.4,-0.1,-0.9}, {-29404.8, -2499.6, 1363.2, 903.0, -234.3, 66.0, 80.6, 23.7, 5.0, -1.9, 3.0, -2.0, 0.1},
{ 0.0, 1677.0, 1236.2, 86.3, 187.8, 72.9, -8.2,-17.6, 2.9,-0.1,-2.5, 0.5, 0.5}, {-1450.9, 2982.0, -2381.2, 809.5, 363.2, 65.5, -76.7, 9.7, 8.4, -6.2, -1.4, -0.1, -0.9},
{ 0.0, 0.0, 525.7,-309.4,-140.7,-121.5, 56.5, -0.5, -1.5, 1.7, 2.3, 1.3, 0.7}, {0.0, 1677.0, 1236.2, 86.3, 187.8, 72.9, -8.2, -17.6, 2.9, -0.1, -2.5, 0.5, 0.5},
{ 0.0, 0.0, 0.0, 48.0,-151.2, -36.2, 15.8,-21.1, -1.1,-0.9,-0.9,-1.2,-0.3}, {0.0, 0.0, 525.7, -309.4, -140.7, -121.5, 56.5, -0.5, -1.5, 1.7, 2.3, 1.3, 0.7},
{ 0.0, 0.0, 0.0, 0.0, 13.5, 13.5, 6.4, 15.3,-13.2, 0.7, 0.3, 0.7, 0.8}, {0.0, 0.0, 0.0, 48.0, -151.2, -36.2, 15.8, -21.1, -1.1, -0.9, -0.9, -1.2, -0.3},
{ 0.0, 0.0, 0.0, 0.0, 0.0, -64.7, -7.2, 13.7, 1.1,-0.9,-0.7, 0.3, 0.0}, {0.0, 0.0, 0.0, 0.0, 13.5, 13.5, 6.4, 15.3, -13.2, 0.7, 0.3, 0.7, 0.8},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.8,-16.5, 8.8, 1.9,-0.1, 0.5, 0.8}, {0.0, 0.0, 0.0, 0.0, 0.0, -64.7, -7.2, 13.7, 1.1, -0.9, -0.7, 0.3, 0.0},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.3, -9.3, 1.4, 1.4,-0.3, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.8, -16.5, 8.8, 1.9, -0.1, 0.5, 0.8},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-11.9,-2.4,-0.6,-0.5, 0.4}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.3, -9.3, 1.4, 1.4, -0.3, 0.0},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-3.8, 0.2, 0.1, 0.1}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -11.9, -2.4, -0.6, -0.5, 0.4},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.1,-1.1, 0.5}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -3.8, 0.2, 0.1, 0.1},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-0.3,-0.5}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.1, -1.1, 0.5},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-0.4}}; // [m][n] in nT {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.3, -0.5},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.4}}; // [m][n] in nT
const double coeffH[14][13] = {{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0 }, const double coeffH[14][13] = {
{4652.5,-2991.6, -82.1, 281.9, 47.7,-19.1,-51.5, 8.4,-23.4, 3.4, 0.0,-1.2,-0.9}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0},
{ 0.0, -734.6, 241.9,-158.4, 208.3, 25.1,-16.9,-15.3, 11.0,-0.2, 2.5, 0.5, 0.6}, {4652.5, -2991.6, -82.1, 281.9, 47.7, -19.1, -51.5, 8.4, -23.4, 3.4, 0.0, -1.2, -0.9},
{ 0.0, 0.0,-543.4, 199.7,-121.2, 52.8, 2.2, 12.8, 9.8, 3.6,-0.6, 1.4, 1.4}, {0.0, -734.6, 241.9, -158.4, 208.3, 25.1, -16.9, -15.3, 11.0, -0.2, 2.5, 0.5, 0.6},
{ 0.0, 0.0, 0.0,-349.7, 32.3,-64.5, 23.5,-11.7, -5.1, 4.8,-0.4,-1.8,-0.4}, {0.0, 0.0, -543.4, 199.7, -121.2, 52.8, 2.2, 12.8, 9.8, 3.6, -0.6, 1.4, 1.4},
{ 0.0, 0.0, 0.0, 0.0, 98.9, 8.9, -2.2, 14.9, -6.3,-8.6, 0.6, 0.1,-1.3}, {0.0, 0.0, 0.0, -349.7, 32.3, -64.5, 23.5, -11.7, -5.1, 4.8, -0.4, -1.8, -0.4},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 68.1,-27.2, 3.6, 7.8,-0.1,-0.2, 0.8,-0.1}, {0.0, 0.0, 0.0, 0.0, 98.9, 8.9, -2.2, 14.9, -6.3, -8.6, 0.6, 0.1, -1.3},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.8, -6.9, 0.4,-4.3,-1.7,-0.2, 0.3}, {0.0, 0.0, 0.0, 0.0, 0.0, 68.1, -27.2, 3.6, 7.8, -0.1, -0.2, 0.8, -0.1},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.8, -1.4,-3.4,-1.6, 0.6,-0.1}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.8, -6.9, 0.4, -4.3, -1.7, -0.2, 0.3},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.6,-0.1,-3.0, 0.2, 0.5}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.8, -1.4, -3.4, -1.6, 0.6, -0.1},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-8.8,-2.0,-0.9, 0.5}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.6, -0.1, -3.0, 0.2, 0.5},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-2.6, 0.0,-0.4}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -8.8, -2.0, -0.9, 0.5},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5,-0.4}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -2.6, 0.0, -0.4},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-0.6}}; // [m][n] in nT {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, -0.4},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.6}}; // [m][n] in nT
const double svG[14][13] = {{5.7,-11.0, 2.2,-1.2,-0.3,-0.5,-0.1, 0.0, 0.0, 0.0, 0.0, 0.0 ,0.0}, const double svG[14][13] = {
{7.4, -7.0, -5.9,-1.6, 0.5,-0.3,-0.2, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0}, {5.7, -11.0, 2.2, -1.2, -0.3, -0.5, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, -2.1, 3.1,-5.9,-0.6, 0.4, 0.0,-0.1, 0.0, 0.0, 0.0, 0.0, 0.0}, {7.4, -7.0, -5.9, -1.6, 0.5, -0.3, -0.2, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0,-12.0, 5.2, 0.2, 1.3, 0.7, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, -2.1, 3.1, -5.9, -0.6, 0.4, 0.0, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0,-5.1, 1.3,-1.4, 0.1,-0.1, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, -12.0, 5.2, 0.2, 1.3, 0.7, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.9, 0.0,-0.5, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, -5.1, 1.3, -1.4, 0.1, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.9,-0.8, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.9, 0.0, -0.5, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.8,-0.1, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.9, -0.8, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.8, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
@ -95,20 +98,21 @@ private:
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT
const double svH[14][13] = {{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, const double svH[14][13] = {
{-25.9,-30.2, 6.0,-0.1, 0.0, 0.0, 0.6,-0.2, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{ 0.0,-22.4,-1.1, 6.5, 2.5,-1.6, 0.6, 0.6, 0.0, 0.0, 0.0, 0.0, 0.0}, {-25.9, -30.2, 6.0, -0.1, 0.0, 0.0, 0.6, -0.2, 0.0, 0.0, 0.0, 0.0, 0.0},
{ 0.0, 0.0, 0.5, 3.6,-0.6,-1.3,-0.8,-0.2, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, -22.4, -1.1, 6.5, 2.5, -1.6, 0.6, 0.6, 0.0, 0.0, 0.0, 0.0, 0.0},
{ 0.0, 0.0, 0.0,-5.0, 3.0, 0.8,-0.2, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.5, 3.6, -0.6, -1.3, -0.8, -0.2, 0.0, 0.0, 0.0, 0.0, 0.0},
{ 0.0, 0.0, 0.0, 0.0, 0.3, 0.0,-1.1,-0.3, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, -5.0, 3.0, 0.8, -0.2, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.1,-0.4, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.3, 0.0, -1.1, -0.3, 0.0, 0.0, 0.0, 0.0, 0.0},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.1, -0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT
double updatedG[14][13]; double updatedG[14][13];
double updatedH[14][13]; double updatedH[14][13];

86
mission/controller/acs/MultiplicativeKalmanFilter.cpp
View File

@ -7,6 +7,7 @@
#include "MultiplicativeKalmanFilter.h" #include "MultiplicativeKalmanFilter.h"
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h> #include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h> #include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h> #include <fsfw/globalfunctions/math/VectorOperations.h>
@ -34,11 +35,11 @@ void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_
void MultiplicativeKalmanFilter::reset() {} void MultiplicativeKalmanFilter::reset() {}
void MultiplicativeKalmanFilter::init( void MultiplicativeKalmanFilter::init(
const double *magneticField_, const bool *validMagField_, const double *sunDir_, const double *magneticField_, const bool validMagField_, const double *sunDir_,
const bool *validSS, const double *sunDirJ, const bool *validSSModel, const double *magFieldJ, const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
const bool *validMagModel) { // valids for "model measurements"? const bool validMagModel) { // valids for "model measurements"?
// check for valid mag/sun // check for valid mag/sun
if (*validMagField_ && *validSS && *validSSModel && *validMagModel) { if (validMagField_ && validSS && validSSModel && validMagModel) {
validInit = true; validInit = true;
// AcsParameters mekfEstParams; // AcsParameters mekfEstParams;
// loadAcsParameters(&mekfEstParams); // loadAcsParameters(&mekfEstParams);
@ -208,48 +209,78 @@ void MultiplicativeKalmanFilter::init(
// --------------- MEKF DISCRETE TIME STEP ------------------------------- // --------------- MEKF DISCRETE TIME STEP -------------------------------
ReturnValue_t MultiplicativeKalmanFilter::mekfEst( ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
const double *quaternionSTR, const bool *validSTR_, const double *rateGYRs_, const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_,
const bool *validGYRs_, const double *magneticField_, const bool *validMagField_, const bool validGYRs_, const double *magneticField_, const bool validMagField_,
const double *sunDir_, const bool *validSS, const double *sunDirJ, const bool *validSSModel, const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel,
const double *magFieldJ, const bool *validMagModel, double *outputQuat, double *outputSatRate) { const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData) {
// Check for GYR Measurements // Check for GYR Measurements
// AcsParameters mekfEstParams; // AcsParameters mekfEstParams;
// loadAcsParameters(&mekfEstParams); // loadAcsParameters(&mekfEstParams);
int MDF = 0; // Matrix Dimension Factor int MDF = 0; // Matrix Dimension Factor
if (!(*validGYRs_)) { if (!validGYRs_) {
{
PoolReadGuard pg(mekfData);
if (pg.getReadResult() == returnvalue::OK) {
double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
double zeroVec[3] = {0.0, 0.0, 0.0};
std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
mekfData->setValidity(false, true);
}
}
validMekf = false; validMekf = false;
return KALMAN_NO_GYR_MEAS; return KALMAN_NO_GYR_MEAS;
} }
// Check for Model Calculations // Check for Model Calculations
else if (!(*validSSModel) || !(*validMagModel)) { else if (!validSSModel || !validMagModel) {
{
PoolReadGuard pg(mekfData);
if (pg.getReadResult() == returnvalue::OK) {
double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
double zeroVec[3] = {0.0, 0.0, 0.0};
std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
mekfData->setValidity(false, true);
}
}
validMekf = false; validMekf = false;
return KALMAN_NO_MODEL; return KALMAN_NO_MODEL;
} }
// Check Measurements available from SS, MAG, STR // Check Measurements available from SS, MAG, STR
if (*validSS && *validMagField_ && *validSTR_) { if (validSS && validMagField_ && validSTR_) {
sensorsAvail = 1; sensorsAvail = 1;
MDF = 9; MDF = 9;
} else if (*validSS && *validMagField_ && !(*validSTR_)) { } else if (validSS && validMagField_ && !validSTR_) {
sensorsAvail = 2; sensorsAvail = 2;
MDF = 6; MDF = 6;
} else if (*validSS && !(*validMagField_) && *validSTR_) { } else if (validSS && !validMagField_ && validSTR_) {
sensorsAvail = 3; sensorsAvail = 3;
MDF = 6; MDF = 6;
} else if (!(*validSS) && *validMagField_ && *validSTR_) { } else if (!validSS && validMagField_ && validSTR_) {
sensorsAvail = 4; sensorsAvail = 4;
MDF = 6; MDF = 6;
} else if (*validSS && !(*validMagField_) && !(*validSTR_)) { } else if (validSS && !validMagField_ && !(validSTR_)) {
sensorsAvail = 5; sensorsAvail = 5;
MDF = 3; MDF = 3;
} else if (!(*validSS) && *validMagField_ && !(*validSTR_)) { } else if (!validSS && validMagField_ && !validSTR_) {
sensorsAvail = 6; sensorsAvail = 6;
MDF = 3; MDF = 3;
} else if (!(*validSS) && !(*validMagField_) && *validSTR_) { } else if (!validSS && !validMagField_ && validSTR_) {
sensorsAvail = 7; sensorsAvail = 7;
MDF = 3; MDF = 3;
} else { } else {
sensorsAvail = 8; // no measurements sensorsAvail = 8; // no measurements
validMekf = false; validMekf = false;
{
PoolReadGuard pg(mekfData);
if (pg.getReadResult() == returnvalue::OK) {
double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
double zeroVec[3] = {0.0, 0.0, 0.0};
std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
mekfData->setValidity(false, true);
}
}
return returnvalue::FAILED; return returnvalue::FAILED;
} }
@ -289,7 +320,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
MathOperations<double>::skewMatrix(magEstB, *measSensMatrix22); MathOperations<double>::skewMatrix(magEstB, *measSensMatrix22);
double measVecQuat[3] = {0, 0, 0}; double measVecQuat[3] = {0, 0, 0};
if (*validSTR_) { if (validSTR_) {
double quatError[4] = {0, 0, 0, 0}; double quatError[4] = {0, 0, 0, 0};
double invPropagatedQuat[4] = {0, 0, 0, 0}; double invPropagatedQuat[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(propagatedQuaternion, invPropagatedQuat); QuaternionOperations::inverse(propagatedQuaternion, invPropagatedQuat);
@ -913,11 +944,6 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
VectorOperations<double>::add(propagatedQuaternion, errQuatTerm, quatBJ, 4); VectorOperations<double>::add(propagatedQuaternion, errQuatTerm, quatBJ, 4);
VectorOperations<double>::normalize(quatBJ, quatBJ, 4); VectorOperations<double>::normalize(quatBJ, quatBJ, 4);
outputQuat[0] = quatBJ[0];
outputQuat[1] = quatBJ[1];
outputQuat[2] = quatBJ[2];
outputQuat[3] = quatBJ[3];
double updatedGyroBias[3] = {0, 0, 0}; double updatedGyroBias[3] = {0, 0, 0};
VectorOperations<double>::add(biasGYR, errStateGyroBias, updatedGyroBias, 3); VectorOperations<double>::add(biasGYR, errStateGyroBias, updatedGyroBias, 3);
// Bias GYR State // Bias GYR State
@ -940,11 +966,6 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
{rotRateEst[2], 0, -rotRateEst[0]}, {rotRateEst[2], 0, -rotRateEst[0]},
{-rotRateEst[1], rotRateEst[0], 0}}; {-rotRateEst[1], rotRateEst[0], 0}};
// Corrected Sat Rate via Bias
outputSatRate[0] = rotRateEst[0];
outputSatRate[1] = rotRateEst[1];
outputSatRate[2] = rotRateEst[2];
// Discrete Process Noise Covariance Q // Discrete Process Noise Covariance Q
double discProcessNoiseCov[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, double discProcessNoiseCov[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}; {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
@ -1144,5 +1165,14 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
// Check for new data in measurement -> SensorProcessing ? // Check for new data in measurement -> SensorProcessing ?
{
PoolReadGuard pg(mekfData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mekfData->quatMekf.value, quatBJ, 4 * sizeof(double));
std::memcpy(mekfData->satRotRateMekf.value, rotRateEst, 3 * sizeof(double));
mekfData->setValidity(true, true);
}
}
return returnvalue::OK; return returnvalue::OK;
} }

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

@ -18,10 +18,9 @@
#include <stdint.h> //uint8_t #include <stdint.h> //uint8_t
#include <time.h> /*purpose, timeval ?*/ #include <time.h> /*purpose, timeval ?*/
#include "config/classIds.h" #include "../controllerdefinitions/AcsCtrlDefinitions.h"
//#include <_timeval.h>
#include "AcsParameters.h" #include "AcsParameters.h"
#include "config/classIds.h"
class MultiplicativeKalmanFilter { class MultiplicativeKalmanFilter {
public: public:
@ -40,9 +39,9 @@ class MultiplicativeKalmanFilter {
* sunDirJ sun direction vector in the ECI frame * sunDirJ sun direction vector in the ECI frame
* magFieldJ magnetic field vector in the ECI frame * magFieldJ magnetic field vector in the ECI frame
*/ */
void init(const double *magneticField_, const bool *validMagField_, const double *sunDir_, void init(const double *magneticField_, const bool validMagField_, const double *sunDir_,
const bool *validSS, const double *sunDirJ, const bool *validSSModel, const bool validSS, const double *sunDirJ, const bool validSSModel,
const double *magFieldJ, const bool *validMagModel); const double *magFieldJ, const bool validMagModel);
/* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter /* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
* for the current step after the initalization * for the current step after the initalization
@ -58,11 +57,11 @@ class MultiplicativeKalmanFilter {
* calculations, KALMAN_INVERSION_FAILED if the calculation of the Gain matrix was not possible, * calculations, KALMAN_INVERSION_FAILED if the calculation of the Gain matrix was not possible,
* RETURN_OK else * RETURN_OK else
*/ */
ReturnValue_t mekfEst(const double *quaternionSTR, const bool *validSTR_, const double *rateGYRs_, ReturnValue_t mekfEst(const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_,
const bool *validGYRs_, const double *magneticField_, const bool validGYRs_, const double *magneticField_,
const bool *validMagField_, const double *sunDir_, const bool *validSS, const bool validMagField_, const double *sunDir_, const bool validSS,
const double *sunDirJ, const bool *validSSModel, const double *magFieldJ, const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
const bool *validMagModel, double *outputQuat, double *outputSatRate); const bool validMagModel, acsctrl::MekfData *mekfData);
// Declaration of Events (like init) and memberships // Declaration of Events (like init) and memberships
// static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND // static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND

34
mission/controller/acs/Navigation.cpp
View File

@ -21,28 +21,34 @@ Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilt
Navigation::~Navigation() {} Navigation::~Navigation() {}
void Navigation::useMekf(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, void Navigation::useMekf(ACS::SensorValues *sensorValues,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData,
ReturnValue_t *mekfValid) { ReturnValue_t *mekfValid) {
double quatJB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, double quatJB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
bool quatJBValid = bool quatJBValid = sensorValues->strSet.caliQx.isValid() &&
(sensorValues->strSet.caliQx.isValid() && sensorValues->strSet.caliQy.isValid() && sensorValues->strSet.caliQy.isValid() &&
sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid()); sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid();
if (kalmanInit) { if (kalmanInit) {
*mekfValid = multiplicativeKalmanFilter.mekfEst( *mekfValid = multiplicativeKalmanFilter.mekfEst(
quatJB, &quatJBValid, outputValues->satRateEst, &outputValues->satRateEstValid, quatJB, quatJBValid, gyrDataProcessed->gyrVecTot.value,
outputValues->magFieldEst, &outputValues->magFieldEstValid, outputValues->sunDirEst, gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value,
&outputValues->sunDirEstValid, outputValues->sunDirModel, &outputValues->sunDirModelValid, mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
outputValues->magFieldModel, &outputValues->magFieldModelValid, outputValues->quatMekfBJ, susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
outputValues->satRateMekf); // VALIDS FOR QUAT AND RATE ?? susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
mgmDataProcessed->magIgrfModel.isValid(),
mekfData); // VALIDS FOR QUAT AND RATE ??
} else { } else {
multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid, multiplicativeKalmanFilter.init(
outputValues->sunDirEst, &outputValues->sunDirEstValid, mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
outputValues->sunDirModel, &outputValues->sunDirModelValid, susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
outputValues->magFieldModel, &outputValues->magFieldModelValid); susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid());
kalmanInit = true; kalmanInit = true;
*mekfValid = 0; *mekfValid = returnvalue::OK;
// Maybe we need feedback from kalmanfilter to identify if there was a problem with the // Maybe we need feedback from kalmanfilter to identify if there was a problem with the
// init of kalman filter where does this class know from that kalman filter was not // init of kalman filter where does this class know from that kalman filter was not

22
mission/controller/acs/Navigation.h
View File

@ -8,28 +8,28 @@
#ifndef NAVIGATION_H_ #ifndef NAVIGATION_H_
#define NAVIGATION_H_ #define NAVIGATION_H_
#include "../controllerdefinitions/AcsCtrlDefinitions.h"
#include "AcsParameters.h" #include "AcsParameters.h"
#include "SensorProcessing.h"
#include "MultiplicativeKalmanFilter.h" #include "MultiplicativeKalmanFilter.h"
#include "SensorProcessing.h"
#include "SensorValues.h" #include "SensorValues.h"
#include "OutputValues.h"
class Navigation{ class Navigation {
public: public:
Navigation(AcsParameters *acsParameters_); //Input mode ? Navigation(AcsParameters *acsParameters_); // Input mode ?
virtual ~Navigation(); virtual ~Navigation();
void useMekf(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, ReturnValue_t *mekfValid); void useMekf(ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData,
ReturnValue_t *mekfValid);
void processSensorData(); void processSensorData();
protected: protected:
private:
private:
MultiplicativeKalmanFilter multiplicativeKalmanFilter; MultiplicativeKalmanFilter multiplicativeKalmanFilter;
AcsParameters acsParameters; AcsParameters acsParameters;
bool kalmanInit = false; bool kalmanInit = false;
}; };
#endif /* ACS_NAVIGATION_H_ */ #endif /* ACS_NAVIGATION_H_ */

9
mission/controller/acs/OutputValues.cpp
View File

@ -1,9 +0,0 @@
#include "OutputValues.h"
namespace ACS {
OutputValues::OutputValues() {}
OutputValues::~OutputValues() {}
} // namespace ACS

44
mission/controller/acs/OutputValues.h
View File

@ -1,44 +0,0 @@
#include <fsfw/datapoollocal/HasLocalDataPoolIF.h>
#include "../controllerdefinitions/AcsCtrlDefinitions.h"
#ifndef OUTPUTVALUES_H_
#define OUTPUTVALUES_H_
namespace ACS {
class OutputValues {
public:
OutputValues();
virtual ~OutputValues();
double magFieldEst[3]; // sensor fusion (G) // output value
bool magFieldEstValid;
double magFieldModel[3]; // igrf (IJK) //
bool magFieldModelValid;
double magneticFieldVectorDerivative[3];
bool magneticFieldVectorDerivativeValid;
bool mgmUpdated; // ToDo: relic from FLP. most likely not used
double sunDirEst[3]; // sensor fusion (G)
bool sunDirEstValid;
double sunDirModel[3]; // sun model (IJK)
bool sunDirModelValid;
double quatMekfBJ[4]; // mekf
bool quatMekfBJValid;
double satRateEst[3];
bool satRateEstValid;
double satRateMekf[3]; // after mekf with correction of bias
bool satRateMekfValid;
double sunVectorDerivative[3];
bool sunVectorDerivativeValid;
double gcLatitude; // geocentric latitude, radian
double gdLongitude; // Radian longitude
};
} // namespace ACS
#endif /*OUTPUTVALUES_H_*/

1
mission/controller/acs/SensorProcessing.h
View File

@ -11,7 +11,6 @@
#include "../controllerdefinitions/AcsCtrlDefinitions.h" #include "../controllerdefinitions/AcsCtrlDefinitions.h"
#include "AcsParameters.h" #include "AcsParameters.h"
#include "OutputValues.h"
#include "SensorValues.h" #include "SensorValues.h"
#include "SusConverter.h" #include "SusConverter.h"
#include "config/classIds.h" #include "config/classIds.h"

1
mission/controller/acs/SensorValues.cpp
View File

@ -90,4 +90,3 @@ ReturnValue_t SensorValues::update() {
} }
} // namespace ACS } // namespace ACS

21
mission/controller/acs/control/Detumble.cpp
View File

@ -32,34 +32,27 @@ void Detumble::loadAcsParameters(AcsParameters *acsParameters_){
} }
ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid,
ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool *magRateValid, const double *magField, const bool magFieldValid, double *magMom) {
const double *magField, const bool *magFieldValid,
double *magMom) {
if (!magRateValid || !magFieldValid) { if (!magRateValid || !magFieldValid) {
return DETUMBLE_NO_SENSORDATA; return DETUMBLE_NO_SENSORDATA;
} }
double gain = detumbleCtrlParameters->gainD; double gain = detumbleCtrlParameters->gainD;
double factor = -gain / pow(VectorOperations<double>::norm(magField,3),2); double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2);
VectorOperations<double>::mulScalar(magRate, factor, magMom, 3); VectorOperations<double>::mulScalar(magRate, factor, magMom, 3);
return returnvalue::OK; return returnvalue::OK;
} }
ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom) { ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid,
double *magMom) {
if (!magRateValid) { if (!magRateValid) {
return DETUMBLE_NO_SENSORDATA; return DETUMBLE_NO_SENSORDATA;
} }
double dipolMax = magnetorquesParameter->DipolMax; double dipolMax = magnetorquesParameter->DipolMax;
for (int i = 0; i<3; i++) { for (int i = 0; i < 3; i++) {
magMom[i] = -dipolMax * sign(magRate[i]);
magMom[i] = - dipolMax * sign(magRate[i]);
} }
return returnvalue::OK; return returnvalue::OK;
} }

10
mission/controller/acs/control/Detumble.h
View File

@ -9,7 +9,6 @@
#define ACS_CONTROL_DETUMBLE_H_ #define ACS_CONTROL_DETUMBLE_H_
#include "../SensorValues.h" #include "../SensorValues.h"
#include "../OutputValues.h"
#include "../AcsParameters.h" #include "../AcsParameters.h"
#include "../config/classIds.h" #include "../config/classIds.h"
#include <string.h> #include <string.h>
@ -32,13 +31,12 @@ public:
*/ */
void loadAcsParameters(AcsParameters *acsParameters_); void loadAcsParameters(AcsParameters *acsParameters_);
ReturnValue_t bDotLaw(const double *magRate, const bool *magRateValid, ReturnValue_t bDotLaw(const double *magRate, const bool magRateValid,
const double *magField, const bool *magFieldValid, const double *magField, const bool magFieldValid, double *magMom);
double *magMom);
ReturnValue_t bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom); ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom);
private: private:
AcsParameters::DetumbleCtrlParameters* detumbleCtrlParameters; AcsParameters::DetumbleCtrlParameters* detumbleCtrlParameters;
AcsParameters::MagnetorquesParameter* magnetorquesParameter; AcsParameters::MagnetorquesParameter* magnetorquesParameter;
}; };

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

@ -112,7 +112,7 @@ void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const do
} }
void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate, void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2,
int32_t *speedRw3, double *mgtDpDes) { int32_t *speedRw3, double *mgtDpDes) {
if (!(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) { if (!(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) {

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

@ -19,7 +19,6 @@
#include <time.h> #include <time.h>
#include "../AcsParameters.h" #include "../AcsParameters.h"
#include "../OutputValues.h"
#include "../SensorValues.h" #include "../SensorValues.h"
#include "../config/classIds.h" #include "../config/classIds.h"
@ -45,7 +44,7 @@ class PtgCtrl {
void ptgGroundstation(const double mode, const double *qError, const double *deltaRate, void ptgGroundstation(const double mode, const double *qError, const double *deltaRate,
const double *rwPseudoInv, double *torqueRws); const double *rwPseudoInv, double *torqueRws);
void ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate, void ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3, int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3,
double *mgtDpDes); double *mgtDpDes);

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

@ -28,12 +28,12 @@ void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
inertiaEIVE = &(acsParameters_->inertiaEIVE); inertiaEIVE = &(acsParameters_->inertiaEIVE);
} }
ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool *quatBJValid, ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
double *magFieldModel, bool *magFieldModelValid, double *magFieldModel, bool magFieldModelValid,
double *sunDirModel, bool *sunDirModelValid, double *satRateMekf, double *sunDirModel, bool sunDirModelValid, double *satRateMekf,
bool *rateMekfValid, double *sunDirRef, double *satRatRef, bool rateMekfValid, double *sunDirRef, double *satRatRef,
double *outputMagMomB, bool *outputValid) { double *outputAngle, double *outputMagMomB, bool *outputValid) {
if (!(*quatBJValid) || !(*magFieldModelValid) || !(*sunDirModelValid) || !(*rateMekfValid)) { if (!quatBJValid || !magFieldModelValid || !sunDirModelValid || !rateMekfValid) {
*outputValid = false; *outputValid = false;
return SAFECTRL_MEKF_INPUT_INVALID; return SAFECTRL_MEKF_INPUT_INVALID;
} }
@ -80,16 +80,19 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool *quatBJValid,
VectorOperations<double>::cross(magFieldB, torqueCmd, torqueMgt); VectorOperations<double>::cross(magFieldB, torqueCmd, torqueMgt);
double normMag = VectorOperations<double>::norm(magFieldB, 3); double normMag = VectorOperations<double>::norm(magFieldB, 3);
VectorOperations<double>::mulScalar(torqueMgt, 1 / pow(normMag, 2), outputMagMomB, 3); VectorOperations<double>::mulScalar(torqueMgt, 1 / pow(normMag, 2), outputMagMomB, 3);
*outputAngle = alpha;
*outputValid = true; *outputValid = true;
return returnvalue::OK; return returnvalue::OK;
} }
// Will be the version in worst case scenario in event of no working MEKF (nor RMUs) // Will be the version in worst case scenario in event of no working MEKF (nor RMUs)
void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, double *sunRateB, void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
bool *sunRateBValid, double *magFieldB, bool *magFieldBValid, bool sunRateBValid, double *magFieldB, bool magFieldBValid,
double *magRateB, bool *magRateBValid, double *sunDirRef, double *magRateB, bool magRateBValid, double *sunDirRef,
double *satRateRef, double *outputMagMomB, bool *outputValid) { double *satRateRef, double *outputAngle, double *outputMagMomB,
bool *outputValid) {
// Check for invalid Inputs // Check for invalid Inputs
if (!susDirBValid || !magFieldBValid || !magRateBValid) { if (!susDirBValid || !magFieldBValid || !magRateBValid) {
*outputValid = false; *outputValid = false;
@ -169,9 +172,7 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, doub
double magMomFactor = pow(VectorOperations<double>::norm(magFieldB, 3), 2); double magMomFactor = pow(VectorOperations<double>::norm(magFieldB, 3), 2);
VectorOperations<double>::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3); VectorOperations<double>::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3);
outputMagMomB[0] = magMomB[0]; std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double));
outputMagMomB[1] = magMomB[1]; *outputAngle = angleAlignErr;
outputMagMomB[2] = magMomB[2];
*outputValid = true; *outputValid = true;
} }

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

@ -13,7 +13,6 @@
#include <time.h> #include <time.h>
#include "../AcsParameters.h" #include "../AcsParameters.h"
#include "../OutputValues.h"
#include "../SensorValues.h" #include "../SensorValues.h"
#include "../config/classIds.h" #include "../config/classIds.h"
@ -27,16 +26,16 @@ class SafeCtrl {
void loadAcsParameters(AcsParameters *acsParameters_); void loadAcsParameters(AcsParameters *acsParameters_);
ReturnValue_t safeMekf(timeval now, double *quatBJ, bool *quatBJValid, double *magFieldModel, ReturnValue_t safeMekf(timeval now, double *quatBJ, bool quatBJValid, double *magFieldModel,
bool *magFieldModelValid, double *sunDirModel, bool *sunDirModelValid, bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid,
double *satRateMekf, bool *rateMekfValid, double *sunDirRef, double *satRateMekf, bool rateMekfValid, double *sunDirRef,
double *satRatRef, // From Guidance (!) double *satRatRef, // From Guidance (!)
double *outputMagMomB, bool *outputValid); double *outputAngle, double *outputMagMomB, bool *outputValid);
void safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, double *sunRateB, void safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
bool *sunRateBValid, double *magFieldB, bool *magFieldBValid, double *magRateB, bool sunRateBValid, double *magFieldB, bool magFieldBValid, double *magRateB,
bool *magRateBValid, double *sunDirRef, double *satRateRef, double *outputMagMomB, bool magRateBValid, double *sunDirRef, double *satRateRef, double *outputAngle,
bool *outputValid); double *outputMagMomB, bool *outputValid);
void idleSunPointing(); // with reaction wheels void idleSunPointing(); // with reaction wheels

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

@ -17,6 +17,7 @@ enum SetIds : uint32_t {
GYR_PROCESSED_DATA, GYR_PROCESSED_DATA,
GPS_PROCESSED_DATA, GPS_PROCESSED_DATA,
MEKF_DATA, MEKF_DATA,
CTRL_VAL_DATA,
ACTUATOR_CMD_DATA ACTUATOR_CMD_DATA
}; };
@ -88,6 +89,10 @@ enum PoolIds : lp_id_t {
// MEKF // MEKF
SAT_ROT_RATE_MEKF, SAT_ROT_RATE_MEKF,
QUAT_MEKF, QUAT_MEKF,
// Ctrl Values
TGT_QUAT,
ERROR_QUAT,
ERROR_ANG,
// Actuator Cmd // Actuator Cmd
RW_TARGET_TORQUE, RW_TARGET_TORQUE,
RW_TARGET_SPEED, RW_TARGET_SPEED,
@ -102,6 +107,7 @@ static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5; static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5;
static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 2; static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 2;
static constexpr uint8_t MEKF_SET_ENTRIES = 2; static constexpr uint8_t MEKF_SET_ENTRIES = 2;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 99;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
/** /**
@ -133,9 +139,9 @@ class MgmDataProcessed : public StaticLocalDataSet<MGM_SET_PROCESSED_ENTRIES> {
lp_vec_t<float, 3> mgm2vec = lp_vec_t<float, 3>(sid.objectId, MGM_2_VEC, this); lp_vec_t<float, 3> mgm2vec = lp_vec_t<float, 3>(sid.objectId, MGM_2_VEC, this);
lp_vec_t<float, 3> mgm3vec = lp_vec_t<float, 3>(sid.objectId, MGM_3_VEC, this); lp_vec_t<float, 3> mgm3vec = lp_vec_t<float, 3>(sid.objectId, MGM_3_VEC, this);
lp_vec_t<float, 3> mgm4vec = lp_vec_t<float, 3>(sid.objectId, MGM_4_VEC, this); lp_vec_t<float, 3> mgm4vec = lp_vec_t<float, 3>(sid.objectId, MGM_4_VEC, this);
lp_vec_t<float, 3> mgmVecTot = lp_vec_t<float, 3>(sid.objectId, MGM_VEC_TOT, this); lp_vec_t<double, 3> mgmVecTot = lp_vec_t<double, 3>(sid.objectId, MGM_VEC_TOT, this);
lp_vec_t<float, 3> mgmVecTotDerivative = lp_vec_t<double, 3> mgmVecTotDerivative =
lp_vec_t<float, 3>(sid.objectId, MGM_VEC_TOT_DERIVATIVE, this); lp_vec_t<double, 3>(sid.objectId, MGM_VEC_TOT_DERIVATIVE, this);
lp_vec_t<double, 3> magIgrfModel = lp_vec_t<double, 3>(sid.objectId, MAG_IGRF_MODEL, this); lp_vec_t<double, 3> magIgrfModel = lp_vec_t<double, 3>(sid.objectId, MAG_IGRF_MODEL, this);
private: private:
@ -179,9 +185,9 @@ class SusDataProcessed : public StaticLocalDataSet<SUS_SET_PROCESSED_ENTRIES> {
lp_vec_t<float, 3> sus9vec = lp_vec_t<float, 3>(sid.objectId, SUS_8_VEC, this); lp_vec_t<float, 3> sus9vec = lp_vec_t<float, 3>(sid.objectId, SUS_8_VEC, this);
lp_vec_t<float, 3> sus10vec = lp_vec_t<float, 3>(sid.objectId, SUS_8_VEC, this); lp_vec_t<float, 3> sus10vec = lp_vec_t<float, 3>(sid.objectId, SUS_8_VEC, this);
lp_vec_t<float, 3> sus11vec = lp_vec_t<float, 3>(sid.objectId, SUS_8_VEC, this); lp_vec_t<float, 3> sus11vec = lp_vec_t<float, 3>(sid.objectId, SUS_8_VEC, this);
lp_vec_t<float, 3> susVecTot = lp_vec_t<float, 3>(sid.objectId, SUS_VEC_TOT, this); lp_vec_t<double, 3> susVecTot = lp_vec_t<double, 3>(sid.objectId, SUS_VEC_TOT, this);
lp_vec_t<float, 3> susVecTotDerivative = lp_vec_t<double, 3> susVecTotDerivative =
lp_vec_t<float, 3>(sid.objectId, SUS_VEC_TOT_DERIVATIVE, this); lp_vec_t<double, 3>(sid.objectId, SUS_VEC_TOT_DERIVATIVE, this);
lp_vec_t<double, 3> sunIjkModel = lp_vec_t<double, 3>(sid.objectId, SUN_IJK_MODEL, this); lp_vec_t<double, 3> sunIjkModel = lp_vec_t<double, 3>(sid.objectId, SUN_IJK_MODEL, this);
private: private:
@ -232,6 +238,17 @@ class MekfData : public StaticLocalDataSet<MEKF_SET_ENTRIES> {
private: private:
}; };
class CtrlValData : public StaticLocalDataSet<CTRL_VAL_SET_ENTRIES> {
public:
CtrlValData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, CTRL_VAL_DATA) {}
lp_vec_t<double, 4> tgtQuat = lp_vec_t<double, 4>(sid.objectId, TGT_QUAT, this);
lp_vec_t<double, 4> errQuat = lp_vec_t<double, 4>(sid.objectId, ERROR_QUAT, this);
lp_var_t<double> errAng = lp_var_t<double>(sid.objectId, ERROR_ANG, this);
private:
};
class ActuatorCmdData : public StaticLocalDataSet<ACT_CMD_SET_ENTRIES> { class ActuatorCmdData : public StaticLocalDataSet<ACT_CMD_SET_ENTRIES> {
public: public:
ActuatorCmdData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, ACTUATOR_CMD_DATA) {} ActuatorCmdData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, ACTUATOR_CMD_DATA) {}