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

166
mission/controller/AcsController.cpp
View File

@ -20,6 +20,7 @@ AcsController::AcsController(object_id_t objectId)
gyrDataProcessed(this),
gpsDataProcessed(this),
mekfData(this),
ctrlValData(this),
actuatorCmdData(this) {}
ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
@ -48,7 +49,9 @@ void AcsController::performControlOperation() {
case SUBMODE_DETUMBLE:
performDetumble();
break;
case SUBMODE_PTG_GS:
case SUBMODE_PTG_TARGET:
case SUBMODE_PTG_NADIR:
case SUBMODE_PTG_INERTIAL:
performPointingCtrl();
break;
}
@ -80,7 +83,7 @@ void AcsController::performControlOperation() {
// DEBUG : REMOVE AFTER COMPLETION
mode = MODE_ON;
submode = SUBMODE_DETUMBLE;
submode = SUBMODE_SAFE;
// 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
ACS::SensorValues sensorValues;
ACS::OutputValues outputValues;
timeval now;
Clock::getClock_timeval(&now);
@ -98,38 +100,54 @@ void AcsController::performSafe() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
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
double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
// IF MEKF is working
double magMomMtq[3] = {0, 0, 0};
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
bool magMomMtqValid = false;
if (validMekf == returnvalue::OK) {
safeCtrl.safeMekf(now, (outputValues.quatMekfBJ), &(outputValues.quatMekfBJValid),
(outputValues.magFieldModel), &(outputValues.magFieldModelValid),
(outputValues.sunDirModel), &(outputValues.sunDirModelValid),
(outputValues.satRateMekf), &(outputValues.satRateMekfValid), sunTargetDir,
satRateSafe, magMomMtq, &magMomMtqValid);
safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(),
mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(),
susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(),
mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(),
sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
} else {
safeCtrl.safeNoMekf(now, outputValues.sunDirEst, &outputValues.sunDirEstValid,
outputValues.sunVectorDerivative, &(outputValues.sunVectorDerivativeValid),
outputValues.magFieldEst, &(outputValues.magFieldEstValid),
outputValues.magneticFieldVectorDerivative,
&(outputValues.magneticFieldVectorDerivativeValid), sunTargetDir,
satRateSafe, magMomMtq, &magMomMtqValid);
safeCtrl.safeNoMekf(
now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(),
susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.isValid(),
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(),
sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
}
double dipolCmdUnits[3] = {0, 0, 0};
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
if (outputValues.satRateMekfValid && VectorOperations<double>::norm(outputValues.satRateMekf, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
if (mekfData.satRotRateMekf.isValid() &&
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
} else if (outputValues.satRateEstValid &&
VectorOperations<double>::norm(outputValues.satRateEst, 3) >
} else if (gyrDataProcessed.gyrVecTot.isValid() &&
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
} else {
@ -139,12 +157,31 @@ void AcsController::performSafe() {
submode = SUBMODE_DETUMBLE;
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() {
ACS::SensorValues sensorValues;
ACS::OutputValues outputValues;
timeval now;
Clock::getClock_timeval(&now);
@ -152,20 +189,22 @@ void AcsController::performDetumble() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
&mekfData, &validMekf);
double magMomMtq[3] = {0, 0, 0};
detumble.bDotLaw(outputValues.magneticFieldVectorDerivative,
&outputValues.magneticFieldVectorDerivativeValid, outputValues.magFieldEst,
&outputValues.magFieldEstValid, magMomMtq);
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
double dipolCmdUnits[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
if (outputValues.satRateMekfValid && VectorOperations<double>::norm(outputValues.satRateMekf, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
if (mekfData.satRotRateMekf.isValid() &&
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++;
} else if (outputValues.satRateEstValid &&
VectorOperations<double>::norm(outputValues.satRateEst, 3) <
} else if (gyrDataProcessed.gyrVecTot.isValid() &&
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++;
} else {
@ -175,11 +214,31 @@ void AcsController::performDetumble() {
submode = SUBMODE_SAFE;
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() {
ACS::SensorValues sensorValues;
ACS::OutputValues outputValues;
timeval now;
Clock::getClock_timeval(&now);
@ -187,12 +246,14 @@ void AcsController::performPointingCtrl() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
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};
guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0};
guidance.comparePtg(targetQuat, &outputValues, refSatRate, quatError, deltaRate);
guidance.targetQuatPtg(&sensorValues, &mekfData, &susDataProcessed, now, targetQuat, refSatRate);
double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
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}};
guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
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,
rwTrqNs, cmdSpeedRws);
double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol
ptgCtrl.ptgDesaturation(
outputValues.magFieldEst, &outputValues.magFieldEstValid, outputValues.satRateMekf,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
ptgCtrl.ptgDesaturation(mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
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,
@ -286,6 +365,11 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf);
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
localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque);
localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed);
@ -312,6 +396,8 @@ LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
return &gpsDataProcessed;
case acsctrl::MEKF_DATA:
return &mekfData;
case acsctrl::CTRL_VAL_DATA:
return &ctrlValData;
case acsctrl::ACTUATOR_CMD_DATA:
return &actuatorCmdData;
default:
@ -328,7 +414,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
return INVALID_SUBMODE;
}
} else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) {
if ((submode > 5) || (submode < 2)) {
if ((submode > 6) || (submode < 2)) {
return INVALID_SUBMODE;
} else {
return returnvalue::OK;

18
mission/controller/AcsController.h
View File

@ -8,7 +8,6 @@
#include "acs/ActuatorCmd.h"
#include "acs/Guidance.h"
#include "acs/Navigation.h"
#include "acs/OutputValues.h"
#include "acs/SensorProcessing.h"
#include "acs/control/Detumble.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_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_INERTIAL = 6;
protected:
void performSafe();
@ -83,7 +83,7 @@ class AcsController : public ExtendedControllerBase {
PoolEntry<float> mgm3VecProc = PoolEntry<float>(3);
PoolEntry<float> mgm4VecProc = PoolEntry<float>(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);
// SUSs
@ -115,9 +115,9 @@ class AcsController : public ExtendedControllerBase {
PoolEntry<float> sus9VecProc = PoolEntry<float>(3);
PoolEntry<float> sus10VecProc = PoolEntry<float>(3);
PoolEntry<float> sus11VecProc = PoolEntry<float>(3);
PoolEntry<float> susVecTot = PoolEntry<float>(3);
PoolEntry<float> susVecTotDer = PoolEntry<float>(3);
PoolEntry<float> sunIjk = PoolEntry<float>(3);
PoolEntry<double> susVecTot = PoolEntry<double>(3);
PoolEntry<double> susVecTotDer = PoolEntry<double>(3);
PoolEntry<double> sunIjk = PoolEntry<double>(3);
// GYRs
acsctrl::GyrDataRaw gyrDataRaw;
@ -144,6 +144,12 @@ class AcsController : public ExtendedControllerBase {
PoolEntry<double> quatMekf = PoolEntry<double>(4);
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
acsctrl::ActuatorCmdData actuatorCmdData;
PoolEntry<double> rwTargetTorque = PoolEntry<double>(4);

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

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

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

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

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

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

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

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

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

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

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

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

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

@ -16,103 +16,107 @@
#ifndef IGRF13MODEL_H_
#define IGRF13MODEL_H_
#include <cmath>
#include <fsfw/parameters/HasParametersIF.h>
#include <stdint.h>
#include <string.h>
#include <time.h>
#include <fsfw/parameters/HasParametersIF.h>
#include <cmath>
// Output should be transformed to [T] instead of [nT]
// Updating Coefficients has to be implemented yet. Question, updating everyday ?
class Igrf13Model/*:public HasParametersIF*/{
class Igrf13Model /*:public HasParametersIF*/ {
public:
Igrf13Model();
virtual ~Igrf13Model();
public:
Igrf13Model();
virtual ~Igrf13Model();
// Main Function
void magFieldComp(const double longitude, const double gcLatitude, const double altitude,
timeval timeOfMagMeasurement, double* magFieldModelInertial);
// 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:
* - longitude: geocentric longitude [rad]
* - latitude: geocentric latitude [rad]
* - altitude: [m]
* - timeOfMagMeasurement: time of actual measurement [s]
*
* Outputs:
* - magFieldModelInertial: Magnetic Field Vector in IJK KOS [nT]*/
// Main Function
void magFieldComp(const double longitude, const double gcLatitude, const double altitude, timeval timeOfMagMeasurement,double* magFieldModelInertial);
// 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:
* - longitude: geocentric longitude [rad]
* - latitude: geocentric latitude [rad]
* - altitude: [m]
* - timeOfMagMeasurement: time of actual measurement [s]
*
* Outputs:
* - magFieldModelInertial: Magnetic Field Vector in IJK KOS [nT]*/
// Coefficient wary over year, could be updated sometimes.
void updateCoeffGH(timeval timeOfMagMeasurement); // Secular variation (SV)
double magFieldModel[3];
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},
{-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, 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, 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, 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, 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, -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, 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.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, -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, -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, 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.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
// Coefficient wary over year, could be updated sometimes.
void updateCoeffGH(timeval timeOfMagMeasurement); //Secular variation (SV)
double magFieldModel[3];
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},
{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, -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, -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, -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, 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, 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, -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, 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, 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, -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, -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.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
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},
{ -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, 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, 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, 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, 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, -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, 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.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,-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,-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, 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.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 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},
{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, -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, -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, -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.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.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.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 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 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 },
{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, -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,-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,-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, 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, 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, -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, 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, 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,-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,-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.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 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},
{-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, -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.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, -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.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, 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.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}}; // [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},
{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, -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,-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,-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.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.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.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 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},
{-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,-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.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,-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.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, 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.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}}; // [m][n] in nT
double updatedG[14][13];
double updatedH[14][13];
static const int igrfOrder = 13; // degree of truncation
double updatedG[14][13];
double updatedH[14][13];
static const int igrfOrder = 13; // degree of truncation
};
#endif /* IGRF13MODEL_H_ */

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

@ -7,6 +7,7 @@
#include "MultiplicativeKalmanFilter.h"
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
@ -34,11 +35,11 @@ void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_
void MultiplicativeKalmanFilter::reset() {}
void MultiplicativeKalmanFilter::init(
const double *magneticField_, const bool *validMagField_, const double *sunDir_,
const bool *validSS, const double *sunDirJ, const bool *validSSModel, const double *magFieldJ,
const bool *validMagModel) { // valids for "model measurements"?
const double *magneticField_, const bool validMagField_, const double *sunDir_,
const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
const bool validMagModel) { // valids for "model measurements"?
// check for valid mag/sun
if (*validMagField_ && *validSS && *validSSModel && *validMagModel) {
if (validMagField_ && validSS && validSSModel && validMagModel) {
validInit = true;
// AcsParameters mekfEstParams;
// loadAcsParameters(&mekfEstParams);
@ -208,48 +209,78 @@ void MultiplicativeKalmanFilter::init(
// --------------- MEKF DISCRETE TIME STEP -------------------------------
ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
const double *quaternionSTR, const bool *validSTR_, const double *rateGYRs_,
const bool *validGYRs_, const double *magneticField_, const bool *validMagField_,
const double *sunDir_, const bool *validSS, const double *sunDirJ, const bool *validSSModel,
const double *magFieldJ, const bool *validMagModel, double *outputQuat, double *outputSatRate) {
const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_,
const bool validGYRs_, const double *magneticField_, const bool validMagField_,
const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel,
const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData) {
// Check for GYR Measurements
// AcsParameters mekfEstParams;
// loadAcsParameters(&mekfEstParams);
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;
return KALMAN_NO_GYR_MEAS;
}
// 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;
return KALMAN_NO_MODEL;
}
// Check Measurements available from SS, MAG, STR
if (*validSS && *validMagField_ && *validSTR_) {
if (validSS && validMagField_ && validSTR_) {
sensorsAvail = 1;
MDF = 9;
} else if (*validSS && *validMagField_ && !(*validSTR_)) {
} else if (validSS && validMagField_ && !validSTR_) {
sensorsAvail = 2;
MDF = 6;
} else if (*validSS && !(*validMagField_) && *validSTR_) {
} else if (validSS && !validMagField_ && validSTR_) {
sensorsAvail = 3;
MDF = 6;
} else if (!(*validSS) && *validMagField_ && *validSTR_) {
} else if (!validSS && validMagField_ && validSTR_) {
sensorsAvail = 4;
MDF = 6;
} else if (*validSS && !(*validMagField_) && !(*validSTR_)) {
} else if (validSS && !validMagField_ && !(validSTR_)) {
sensorsAvail = 5;
MDF = 3;
} else if (!(*validSS) && *validMagField_ && !(*validSTR_)) {
} else if (!validSS && validMagField_ && !validSTR_) {
sensorsAvail = 6;
MDF = 3;
} else if (!(*validSS) && !(*validMagField_) && *validSTR_) {
} else if (!validSS && !validMagField_ && validSTR_) {
sensorsAvail = 7;
MDF = 3;
} else {
sensorsAvail = 8; // no measurements
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;
}
@ -289,7 +320,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
MathOperations<double>::skewMatrix(magEstB, *measSensMatrix22);
double measVecQuat[3] = {0, 0, 0};
if (*validSTR_) {
if (validSTR_) {
double quatError[4] = {0, 0, 0, 0};
double invPropagatedQuat[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(propagatedQuaternion, invPropagatedQuat);
@ -913,11 +944,6 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
VectorOperations<double>::add(propagatedQuaternion, errQuatTerm, 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};
VectorOperations<double>::add(biasGYR, errStateGyroBias, updatedGyroBias, 3);
// Bias GYR State
@ -940,11 +966,6 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
{rotRateEst[2], 0, -rotRateEst[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
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}};
@ -1144,5 +1165,14 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
// 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;
}

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

@ -18,10 +18,9 @@
#include <stdint.h> //uint8_t
#include <time.h> /*purpose, timeval ?*/
#include "config/classIds.h"
//#include <_timeval.h>
#include "../controllerdefinitions/AcsCtrlDefinitions.h"
#include "AcsParameters.h"
#include "config/classIds.h"
class MultiplicativeKalmanFilter {
public:
@ -40,9 +39,9 @@ class MultiplicativeKalmanFilter {
* sunDirJ sun direction vector in the ECI frame
* magFieldJ magnetic field vector in the ECI frame
*/
void init(const double *magneticField_, const bool *validMagField_, const double *sunDir_,
const bool *validSS, const double *sunDirJ, const bool *validSSModel,
const double *magFieldJ, const bool *validMagModel);
void init(const double *magneticField_, const bool validMagField_, const double *sunDir_,
const bool validSS, const double *sunDirJ, const bool validSSModel,
const double *magFieldJ, const bool validMagModel);
/* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
* 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,
* RETURN_OK else
*/
ReturnValue_t mekfEst(const double *quaternionSTR, const bool *validSTR_, const double *rateGYRs_,
const bool *validGYRs_, const double *magneticField_,
const bool *validMagField_, const double *sunDir_, const bool *validSS,
const double *sunDirJ, const bool *validSSModel, const double *magFieldJ,
const bool *validMagModel, double *outputQuat, double *outputSatRate);
ReturnValue_t mekfEst(const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_,
const bool validGYRs_, const double *magneticField_,
const bool validMagField_, const double *sunDir_, const bool validSS,
const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
const bool validMagModel, acsctrl::MekfData *mekfData);
// Declaration of Events (like init) and memberships
// 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() {}
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) {
double quatJB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
bool quatJBValid =
(sensorValues->strSet.caliQx.isValid() && sensorValues->strSet.caliQy.isValid() &&
sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid());
bool quatJBValid = sensorValues->strSet.caliQx.isValid() &&
sensorValues->strSet.caliQy.isValid() &&
sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid();
if (kalmanInit) {
*mekfValid = multiplicativeKalmanFilter.mekfEst(
quatJB, &quatJBValid, outputValues->satRateEst, &outputValues->satRateEstValid,
outputValues->magFieldEst, &outputValues->magFieldEstValid, outputValues->sunDirEst,
&outputValues->sunDirEstValid, outputValues->sunDirModel, &outputValues->sunDirModelValid,
outputValues->magFieldModel, &outputValues->magFieldModelValid, outputValues->quatMekfBJ,
outputValues->satRateMekf); // VALIDS FOR QUAT AND RATE ??
quatJB, quatJBValid, gyrDataProcessed->gyrVecTot.value,
gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value,
mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
mgmDataProcessed->magIgrfModel.isValid(),
mekfData); // VALIDS FOR QUAT AND RATE ??
} else {
multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid,
outputValues->sunDirEst, &outputValues->sunDirEstValid,
outputValues->sunDirModel, &outputValues->sunDirModelValid,
outputValues->magFieldModel, &outputValues->magFieldModelValid);
multiplicativeKalmanFilter.init(
mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid());
kalmanInit = true;
*mekfValid = 0;
*mekfValid = returnvalue::OK;
// 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

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

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

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

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

45
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,
const double *magField, const bool *magFieldValid,
double *magMom) {
if (!magRateValid || !magFieldValid) {
return DETUMBLE_NO_SENSORDATA;
}
double gain = detumbleCtrlParameters->gainD;
double factor = -gain / pow(VectorOperations<double>::norm(magField,3),2);
VectorOperations<double>::mulScalar(magRate, factor, magMom, 3);
return returnvalue::OK;
ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid,
const double *magField, const bool magFieldValid, double *magMom) {
if (!magRateValid || !magFieldValid) {
return DETUMBLE_NO_SENSORDATA;
}
double gain = detumbleCtrlParameters->gainD;
double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2);
VectorOperations<double>::mulScalar(magRate, factor, magMom, 3);
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) {
return DETUMBLE_NO_SENSORDATA;
}
if (!magRateValid) {
return DETUMBLE_NO_SENSORDATA;
}
double dipolMax = magnetorquesParameter->DipolMax;
for (int i = 0; i<3; i++) {
magMom[i] = - dipolMax * sign(magRate[i]);
}
return returnvalue::OK;
double dipolMax = magnetorquesParameter->DipolMax;
for (int i = 0; i < 3; i++) {
magMom[i] = -dipolMax * sign(magRate[i]);
}
return returnvalue::OK;
}

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

@ -9,7 +9,6 @@
#define ACS_CONTROL_DETUMBLE_H_
#include "../SensorValues.h"
#include "../OutputValues.h"
#include "../AcsParameters.h"
#include "../config/classIds.h"
#include <string.h>
@ -32,13 +31,12 @@ public:
*/
void loadAcsParameters(AcsParameters *acsParameters_);
ReturnValue_t bDotLaw(const double *magRate, const bool *magRateValid,
const double *magField, const bool *magFieldValid,
double *magMom);
ReturnValue_t bDotLaw(const double *magRate, const bool magRateValid,
const double *magField, const bool magFieldValid, 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::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 *speedRw3, double *mgtDpDes) {
if (!(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) {

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

@ -19,7 +19,6 @@
#include <time.h>
#include "../AcsParameters.h"
#include "../OutputValues.h"
#include "../SensorValues.h"
#include "../config/classIds.h"
@ -45,7 +44,7 @@ class PtgCtrl {
void ptgGroundstation(const double mode, const double *qError, const double *deltaRate,
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,
double *mgtDpDes);

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

@ -28,12 +28,12 @@ void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
inertiaEIVE = &(acsParameters_->inertiaEIVE);
}
ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool *quatBJValid,
double *magFieldModel, bool *magFieldModelValid,
double *sunDirModel, bool *sunDirModelValid, double *satRateMekf,
bool *rateMekfValid, double *sunDirRef, double *satRatRef,
double *outputMagMomB, bool *outputValid) {
if (!(*quatBJValid) || !(*magFieldModelValid) || !(*sunDirModelValid) || !(*rateMekfValid)) {
ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
double *magFieldModel, bool magFieldModelValid,
double *sunDirModel, bool sunDirModelValid, double *satRateMekf,
bool rateMekfValid, double *sunDirRef, double *satRatRef,
double *outputAngle, double *outputMagMomB, bool *outputValid) {
if (!quatBJValid || !magFieldModelValid || !sunDirModelValid || !rateMekfValid) {
*outputValid = false;
return SAFECTRL_MEKF_INPUT_INVALID;
}
@ -80,31 +80,34 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool *quatBJValid,
VectorOperations<double>::cross(magFieldB, torqueCmd, torqueMgt);
double normMag = VectorOperations<double>::norm(magFieldB, 3);
VectorOperations<double>::mulScalar(torqueMgt, 1 / pow(normMag, 2), outputMagMomB, 3);
*outputAngle = alpha;
*outputValid = true;
return returnvalue::OK;
}
// 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,
bool *sunRateBValid, double *magFieldB, bool *magFieldBValid,
double *magRateB, bool *magRateBValid, double *sunDirRef,
double *satRateRef, double *outputMagMomB, bool *outputValid) {
void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
bool sunRateBValid, double *magFieldB, bool magFieldBValid,
double *magRateB, bool magRateBValid, double *sunDirRef,
double *satRateRef, double *outputAngle, double *outputMagMomB,
bool *outputValid) {
// Check for invalid Inputs
if (!susDirBValid || !magFieldBValid || !magRateBValid) {
*outputValid = false;
return;
}
// normalize sunDir and magDir
// normalize sunDir and magDir
double magDirB[3] = {0, 0, 0};
VectorOperations<double>::normalize(magFieldB, magDirB, 3);
VectorOperations<double>::normalize(susDirB, susDirB, 3);
// Cosinus angle between sunDir and magDir
// Cosinus angle between sunDir and magDir
double cosAngleSunMag = VectorOperations<double>::dot(magDirB, susDirB);
// Rate parallel to sun direction and magnetic field direction
// Rate parallel to sun direction and magnetic field direction
double rateParaSun = 0, rateParaMag = 0;
double dotSunRateMag = 0, dotmagRateSun = 0, rateFactor = 0;
dotSunRateMag = VectorOperations<double>::dot(sunRateB, magDirB);
@ -113,7 +116,7 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, doub
rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor;
rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor;
// Full rate or estimate
// Full rate or estimate
double estSatRate[3] = {0, 0, 0};
double estSatRateMag[3] = {0, 0, 0}, estSatRateSun[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(susDirB, rateParaSun, estSatRateSun, 3);
@ -123,8 +126,8 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, doub
VectorOperations<double>::add(estSatRateSun, estSatRateMag, estSatRate, 3);
VectorOperations<double>::mulScalar(estSatRate, 0.5, estSatRate, 3);
/* Only valid if angle between sun direction and magnetic field direction
is sufficiently large */
/* Only valid if angle between sun direction and magnetic field direction
is sufficiently large */
double sinAngle = 0;
sinAngle = sin(acos(cos(cosAngleSunMag)));
@ -169,9 +172,7 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, doub
double magMomFactor = pow(VectorOperations<double>::norm(magFieldB, 3), 2);
VectorOperations<double>::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3);
outputMagMomB[0] = magMomB[0];
outputMagMomB[1] = magMomB[1];
outputMagMomB[2] = magMomB[2];
std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double));
*outputAngle = angleAlignErr;
*outputValid = true;
}

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

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

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

@ -17,6 +17,7 @@ enum SetIds : uint32_t {
GYR_PROCESSED_DATA,
GPS_PROCESSED_DATA,
MEKF_DATA,
CTRL_VAL_DATA,
ACTUATOR_CMD_DATA
};
@ -88,6 +89,10 @@ enum PoolIds : lp_id_t {
// MEKF
SAT_ROT_RATE_MEKF,
QUAT_MEKF,
// Ctrl Values
TGT_QUAT,
ERROR_QUAT,
ERROR_ANG,
// Actuator Cmd
RW_TARGET_TORQUE,
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 GPS_SET_PROCESSED_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;
/**
@ -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> 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> mgmVecTot = lp_vec_t<float, 3>(sid.objectId, MGM_VEC_TOT, this);
lp_vec_t<float, 3> mgmVecTotDerivative =
lp_vec_t<float, 3>(sid.objectId, MGM_VEC_TOT_DERIVATIVE, this);
lp_vec_t<double, 3> mgmVecTot = lp_vec_t<double, 3>(sid.objectId, MGM_VEC_TOT, this);
lp_vec_t<double, 3> mgmVecTotDerivative =
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);
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> 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> susVecTot = lp_vec_t<float, 3>(sid.objectId, SUS_VEC_TOT, this);
lp_vec_t<float, 3> susVecTotDerivative =
lp_vec_t<float, 3>(sid.objectId, SUS_VEC_TOT_DERIVATIVE, this);
lp_vec_t<double, 3> susVecTot = lp_vec_t<double, 3>(sid.objectId, SUS_VEC_TOT, this);
lp_vec_t<double, 3> susVecTotDerivative =
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);
private:
@ -232,6 +238,17 @@ class MekfData : public StaticLocalDataSet<MEKF_SET_ENTRIES> {
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> {
public:
ActuatorCmdData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, ACTUATOR_CMD_DATA) {}