First Version of ACS Controller #329
@ -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) >
|
||||
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) <
|
||||
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;
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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,10 +55,10 @@ 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
|
||||
// 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++) {
|
||||
@ -70,9 +68,7 @@ void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnit
|
||||
}
|
||||
|
||||
if (maxValue > maxDipol) {
|
||||
|
||||
double scalingFactor = maxDipol / maxValue;
|
||||
VectorOperations<double>::mulScalar(dipolMomentUnits, scalingFactor, dipolMomentUnits, 3);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -8,29 +8,28 @@
|
||||
#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 ?
|
||||
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
|
||||
* @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
|
||||
* 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);
|
||||
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
|
||||
@ -40,9 +39,8 @@ public:
|
||||
*/
|
||||
void cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits);
|
||||
|
||||
protected:
|
||||
|
||||
private:
|
||||
protected:
|
||||
private:
|
||||
AcsParameters acsParameters;
|
||||
};
|
||||
|
||||
|
||||
@ -6,7 +6,6 @@ target_sources(
|
||||
Igrf13Model.cpp
|
||||
MultiplicativeKalmanFilter.cpp
|
||||
Navigation.cpp
|
||||
OutputValues.cpp
|
||||
SensorProcessing.cpp
|
||||
SensorValues.cpp
|
||||
SusConverter.cpp)
|
||||
|
||||
@ -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];
|
||||
|
||||
@ -8,33 +8,35 @@
|
||||
#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:
|
||||
public:
|
||||
Guidance(AcsParameters *acsParameters_);
|
||||
virtual ~Guidance();
|
||||
|
||||
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:
|
||||
private:
|
||||
AcsParameters acsParameters;
|
||||
bool strBlindAvoidFlag = false;
|
||||
};
|
||||
|
||||
@ -6,36 +6,34 @@
|
||||
*/
|
||||
|
||||
#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) {
|
||||
|
||||
double phi = longitude, theta = gcLatitude; //geocentric
|
||||
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 -= 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 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;
|
||||
|
||||
@ -60,8 +58,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
|
||||
dP20 = dP10;
|
||||
dP10 = dP2;
|
||||
} else {
|
||||
K = (pow((n - 1), 2) - pow(m, 2))
|
||||
/ ((2 * n - 1) * (2 * n - 3));
|
||||
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;
|
||||
@ -70,14 +67,17 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
|
||||
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);
|
||||
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);
|
||||
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[2] +=
|
||||
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));
|
||||
|
||||
/* Next step: transform into inertial KOS (IJK)*/
|
||||
//Julean Centuries
|
||||
// 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 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;
|
||||
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[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};
|
||||
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
|
||||
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);
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -16,25 +16,26 @@
|
||||
#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*/{
|
||||
|
||||
public:
|
||||
class Igrf13Model /*:public HasParametersIF*/ {
|
||||
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
|
||||
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]
|
||||
@ -44,50 +45,52 @@ public:
|
||||
* Outputs:
|
||||
* - magFieldModelInertial: Magnetic Field Vector in IJK KOS [nT]*/
|
||||
|
||||
|
||||
// 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];
|
||||
|
||||
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
|
||||
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 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 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 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},
|
||||
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},
|
||||
@ -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}}; // [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 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];
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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 ?
|
||||
class Navigation {
|
||||
public:
|
||||
Navigation(AcsParameters *acsParameters_); // Input mode ?
|
||||
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();
|
||||
|
||||
protected:
|
||||
|
||||
private:
|
||||
protected:
|
||||
private:
|
||||
MultiplicativeKalmanFilter multiplicativeKalmanFilter;
|
||||
AcsParameters acsParameters;
|
||||
bool kalmanInit = false;
|
||||
};
|
||||
|
||||
#endif /* ACS_NAVIGATION_H_ */
|
||||
|
||||
|
||||
@ -1,9 +0,0 @@
|
||||
#include "OutputValues.h"
|
||||
|
||||
namespace ACS {
|
||||
|
||||
OutputValues::OutputValues() {}
|
||||
|
||||
OutputValues::~OutputValues() {}
|
||||
|
||||
} // namespace ACS
|
||||
@ -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_*/
|
||||
@ -11,7 +11,6 @@
|
||||
|
||||
#include "../controllerdefinitions/AcsCtrlDefinitions.h"
|
||||
#include "AcsParameters.h"
|
||||
#include "OutputValues.h"
|
||||
#include "SensorValues.h"
|
||||
#include "SusConverter.h"
|
||||
#include "config/classIds.h"
|
||||
|
||||
@ -90,4 +90,3 @@ ReturnValue_t SensorValues::update() {
|
||||
}
|
||||
|
||||
} // namespace ACS
|
||||
|
||||
|
||||
@ -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) {
|
||||
|
||||
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);
|
||||
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;
|
||||
}
|
||||
|
||||
double dipolMax = magnetorquesParameter->DipolMax;
|
||||
for (int i = 0; i<3; i++) {
|
||||
|
||||
magMom[i] = - dipolMax * sign(magRate[i]);
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
magMom[i] = -dipolMax * sign(magRate[i]);
|
||||
}
|
||||
|
||||
return returnvalue::OK;
|
||||
|
||||
}
|
||||
|
||||
@ -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;
|
||||
};
|
||||
|
||||
@ -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)) {
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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,16 +80,19 @@ 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;
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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) {}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user