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

Telemetry related Changes #394

Merged
muellerr merged 39 commits from eggert/acs-dataset-stuff into develop 2023-02-22 19:56:24 +01:00
Conversation 0 Commits 39 Files Changed 17 +846 -777
17 changed files with 846 additions and 777 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
CHANGELOG.md
View File

@ -12,10 +12,34 @@ Starting at v2.0.0, this project will adhere to semantic versioning and the the
will consitute of a breaking change warranting a new major release: will consitute of a breaking change warranting a new major release:
- The TMTC interface changes in any shape of form. - The TMTC interface changes in any shape of form.
- The behavour of the OBSW changes in a major shape or form relevant for operations - The behaviour of the OBSW changes in a major shape or form relevant for operations
# [unreleased] # [unreleased]
## Changed
- All `targetQuat` functions in `Guidance` now return the target quaternion (target
in ECI frame), which is passed on to `CtrlValData`.
## Added
- `MEKF` now returns an unique returnvalue depending on why the function terminates. These
returnvalues are used in the `AcsController` to determine on how to procede with its
perform functions. In case the `MEKF` did terminate before estimating the quaternion
and rotational rate, an info event will be triggered. Another info event can only be
triggered after the `MEKF` has run successfully again. If the `AcsController` tries to
perform any pointing mode and the `MEKF` fails, the `performPointingCtrl` function will
set the RWs to the last RW speeds and set a zero dipole vector. If the `MEKF` does not
recover within 5 cycles (2 mins) the `AcsController` triggers another event, resulting in
the `AcsSubsystem` being commanded to `SAFE`.
- `MekfData` now includes `mekfStatus`
- `CtrlValData` now includes `tgtRotRate`
## Fixed
- Usage of floats as iterators and using them to calculate a uint8_t index in `SusConverter`
- Removed unused variables in the `AcsController`
# [v1.30.0] # [v1.30.0]
eive-tmtc: v2.14.0 eive-tmtc: v2.14.0

4
mission/acsDefs.h
View File

@ -25,6 +25,10 @@ static const Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM);
static constexpr Event SAFE_RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM); static constexpr Event SAFE_RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM);
//!< Multiple RWs are invalid, not commandable and therefore higher ACS modes cannot be maintained. //!< Multiple RWs are invalid, not commandable and therefore higher ACS modes cannot be maintained.
static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH); static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH);
//!< MEKF was not able to compute a solution.
static constexpr Event MEKF_INVALID_INFO = MAKE_EVENT(3, severity::INFO);
//!< MEKF was not able to compute a solution during any pointing ACS mode for a prolonged time.
static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(4, severity::HIGH);
extern const char* getModeStr(AcsMode mode); extern const char* getModeStr(AcsMode mode);

292
mission/controller/AcsController.cpp
View File

@ -14,8 +14,6 @@ AcsController::AcsController(object_id_t objectId)
safeCtrl(&acsParameters), safeCtrl(&acsParameters),
detumble(&acsParameters), detumble(&acsParameters),
ptgCtrl(&acsParameters), ptgCtrl(&acsParameters),
detumbleCounter{0},
multipleRwUnavailableCounter{0},
parameterHelper(this), parameterHelper(this),
mgmDataRaw(this), mgmDataRaw(this),
mgmDataProcessed(this), mgmDataProcessed(this),
@ -28,6 +26,14 @@ AcsController::AcsController(object_id_t objectId)
ctrlValData(this), ctrlValData(this),
actuatorCmdData(this) {} actuatorCmdData(this) {}
ReturnValue_t AcsController::initialize() {
ReturnValue_t result = parameterHelper.initialize();
if (result != returnvalue::OK) {
return result;
}
return ExtendedControllerBase::initialize();
}
ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
ReturnValue_t result = actionHelper.handleActionMessage(message); ReturnValue_t result = actionHelper.handleActionMessage(message);
if (result == returnvalue::OK) { if (result == returnvalue::OK) {
@ -116,17 +122,24 @@ void AcsController::performSafe() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters); &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t validMekf; ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &susDataProcessed, &mekfData);
&mekfData, &validMekf); if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
// give desired satellite rate and sun direction to align if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_INFO);
mekfInvalidFlag = true;
}
} else {
mekfInvalidFlag = false;
}
// get desired satellite rate and sun direction to align
double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0}; double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
// if MEKF is working // if MEKF is working
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
bool magMomMtqValid = false; bool magMomMtqValid = false;
if (validMekf == returnvalue::OK) { if (result == MultiplicativeKalmanFilter::KALMAN_RUNNING) {
safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(), safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(),
mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(), mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(),
susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(), susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(),
@ -141,24 +154,9 @@ void AcsController::performSafe() {
sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
} }
int16_t cmdDipolMtqs[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs); actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
{ // detumble check and switch
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
double unitQuat[4] = {0, 0, 0, 1};
std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double));
ctrlValData.tgtQuat.setValid(false);
std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double));
ctrlValData.errQuat.setValid(false);
ctrlValData.errAng.value = errAng;
ctrlValData.errAng.setValid(true);
ctrlValData.setValidity(true, false);
}
}
// Detumble check and switch
if (mekfData.satRotRateMekf.isValid() && if (mekfData.satRotRateMekf.isValid() &&
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) > VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) { acsParameters.detumbleParameter.omegaDetumbleStart) {
@ -176,20 +174,8 @@ void AcsController::performSafe() {
triggerEvent(acs::SAFE_RATE_VIOLATION); triggerEvent(acs::SAFE_RATE_VIOLATION);
} }
{ updateCtrlValData(errAng);
PoolReadGuard pg(&actuatorCmdData); updateActuatorCmdData(cmdDipolMtqs);
if (pg.getReadResult() == returnvalue::OK) {
int32_t zeroVec[4] = {0, 0, 0, 0};
std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(int32_t));
actuatorCmdData.rwTargetTorque.setValid(false);
std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t));
actuatorCmdData.rwTargetSpeed.setValid(false);
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
actuatorCmdData.mtqTargetDipole.setValid(true);
actuatorCmdData.setValidity(true, false);
}
}
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
// acsParameters.rwHandlingParameters.rampTime); // acsParameters.rwHandlingParameters.rampTime);
@ -201,15 +187,21 @@ void AcsController::performDetumble() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters); &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t validMekf; ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &susDataProcessed, &mekfData);
&mekfData, &validMekf); if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_INFO);
mekfInvalidFlag = true;
}
} else {
mekfInvalidFlag = false;
}
double magMomMtq[3] = {0, 0, 0}; double magMomMtq[3] = {0, 0, 0};
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
int16_t cmdDipolMtqs[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs); actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
if (mekfData.satRotRateMekf.isValid() && if (mekfData.satRotRateMekf.isValid() &&
@ -229,19 +221,8 @@ void AcsController::performDetumble() {
triggerEvent(acs::SAFE_RATE_RECOVERY); triggerEvent(acs::SAFE_RATE_RECOVERY);
} }
{ disableCtrlValData();
PoolReadGuard pg(&actuatorCmdData); updateActuatorCmdData(cmdDipolMtqs);
if (pg.getReadResult() == returnvalue::OK) {
std::memset(actuatorCmdData.rwTargetTorque.value, 0, 4 * sizeof(double));
actuatorCmdData.rwTargetTorque.setValid(false);
std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t));
actuatorCmdData.rwTargetSpeed.setValid(false);
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
actuatorCmdData.mtqTargetDipole.setValid(true);
actuatorCmdData.setValidity(true, false);
}
}
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
// acsParameters.rwHandlingParameters.rampTime); // acsParameters.rwHandlingParameters.rampTime);
@ -253,23 +234,34 @@ void AcsController::performPointingCtrl() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters); &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t validMekf; ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &susDataProcessed, &mekfData);
&mekfData, &validMekf); if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; if (not mekfInvalidFlag) {
double quatRef[4] = {0, 0, 0, 0}; triggerEvent(acs::MEKF_INVALID_INFO);
mekfInvalidFlag = true;
}
if (mekfInvalidCounter > 4) {
triggerEvent(acs::MEKF_INVALID_MODE_VIOLATION);
}
mekfInvalidCounter++;
commandActuators(0, 0, 0, acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
acsParameters.rwHandlingParameters.rampTime);
return;
} else {
mekfInvalidFlag = false;
mekfInvalidCounter = 0;
}
uint8_t enableAntiStiction = true; uint8_t enableAntiStiction = true;
double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
if (result == returnvalue::FAILED) { if (result == returnvalue::FAILED) {
multipleRwUnavailableCounter++;
if (multipleRwUnavailableCounter > 4) { if (multipleRwUnavailableCounter > 4) {
triggerEvent(acs::MULTIPLE_RW_INVALID); triggerEvent(acs::MULTIPLE_RW_INVALID);
} }
multipleRwUnavailableCounter++;
return; return;
} else { } else {
multipleRwUnavailableCounter = 0; multipleRwUnavailableCounter = 0;
@ -278,40 +270,37 @@ void AcsController::performPointingCtrl() {
double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
double mgtDpDes[3] = {0, 0, 0}; double mgtDpDes[3] = {0, 0, 0};
// Variables required for guidance
double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1},
errorAngle = 0, errorSatRotRate[3] = {0, 0, 0};
switch (submode) { switch (submode) {
case acs::PTG_IDLE: case acs::PTG_IDLE:
guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
targetQuat, refSatRate); guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
4 * sizeof(double));
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
deltaRate);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace( ptgCtrl.ptgNullspace(
&acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), &acsParameters.idleModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
break; break;
case acs::PTG_TARGET: case acs::PTG_TARGET:
guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value,
refSatRate); gpsDataProcessed.gpsVelocity.value, targetQuat,
std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, targetSatRotRate);
4 * sizeof(double)); guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; targetSatRotRate, acsParameters.targetModeControllerParameters.quatRef,
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, acsParameters.targetModeControllerParameters.refRotRate, errorQuat,
deltaRate); errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace( ptgCtrl.ptgNullspace(
&acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
@ -324,17 +313,15 @@ void AcsController::performPointingCtrl() {
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
break; break;
case acs::PTG_TARGET_GS: case acs::PTG_TARGET_GS:
guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, guidance.targetQuatPtgGs(now, gpsDataProcessed.gpsPosition.value,
targetQuat, refSatRate); susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
4 * sizeof(double)); targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
deltaRate);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace( ptgCtrl.ptgNullspace(
&acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
@ -347,16 +334,18 @@ void AcsController::performPointingCtrl() {
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
break; break;
case acs::PTG_NADIR: case acs::PTG_NADIR:
guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, guidance.targetQuatPtgNadirThreeAxes(now, gpsDataProcessed.gpsPosition.value,
refSatRate); gpsDataProcessed.gpsVelocity.value, targetQuat,
std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double)); targetSatRotRate);
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef,
deltaRate); acsParameters.nadirModeControllerParameters.refRotRate, errorQuat,
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, quatError, deltaRate, errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace( ptgCtrl.ptgNullspace(
&acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), &acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
@ -369,16 +358,17 @@ void AcsController::performPointingCtrl() {
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
break; break;
case acs::PTG_INERTIAL: case acs::PTG_INERTIAL:
guidance.inertialQuatPtg(targetQuat, refSatRate); std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef,
4 * sizeof(double)); 4 * sizeof(double));
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef,
deltaRate); acsParameters.inertialModeControllerParameters.refRotRate, errorQuat,
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, quatError, deltaRate, errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace( ptgCtrl.ptgNullspace(
&acsParameters.inertialModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), &acsParameters.inertialModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
@ -391,6 +381,7 @@ void AcsController::performPointingCtrl() {
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
break; break;
} }
@ -398,24 +389,14 @@ void AcsController::performPointingCtrl() {
ptgCtrl.rwAntistiction(&sensorValues, torqueRwsScaled); ptgCtrl.rwAntistiction(&sensorValues, torqueRwsScaled);
} }
int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
actuatorCmd.cmdSpeedToRws(sensorValues.rw1Set.currSpeed.value, actuatorCmd.cmdSpeedToRws(sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws); sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws);
int16_t cmdDipolMtqs[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs); actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs);
{ updateCtrlValData(targetQuat, errorQuat, errorAngle);
PoolReadGuard pg(&actuatorCmdData); updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double));
std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdSpeedRws, 4 * sizeof(int32_t));
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
actuatorCmdData.setValidity(true, true);
}
}
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0], // acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
// cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], // cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
@ -451,6 +432,67 @@ ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole,
return returnvalue::OK; return returnvalue::OK;
} }
void AcsController::updateActuatorCmdData(int16_t mtqTargetDipole[3]) {
double rwTargetTorque[4] = {0.0, 0.0, 0.0, 0.0};
int32_t rwTargetSpeed[4] = {0, 0, 0, 0};
updateActuatorCmdData(rwTargetTorque, rwTargetSpeed, mtqTargetDipole);
}
void AcsController::updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTargetSpeed[4],
int16_t mtqTargetDipole[3]) {
{
PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTargetTorque, 4 * sizeof(double));
std::memcpy(actuatorCmdData.rwTargetSpeed.value, rwTargetSpeed, 4 * sizeof(int32_t));
std::memcpy(actuatorCmdData.mtqTargetDipole.value, mtqTargetDipole, 3 * sizeof(int16_t));
actuatorCmdData.setValidity(true, true);
}
}
}
void AcsController::updateCtrlValData(double errAng) {
double unitQuat[4] = {0, 0, 0, 1};
{
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double));
ctrlValData.tgtQuat.setValid(false);
std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double));
ctrlValData.errQuat.setValid(false);
ctrlValData.errAng.value = errAng;
ctrlValData.errAng.setValid(true);
ctrlValData.setValidity(true, false);
}
}
}
void AcsController::updateCtrlValData(double tgtQuat[4], double errQuat[4], double errAng) {
{
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double));
std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
ctrlValData.errAng.value = errAng;
ctrlValData.setValidity(true, true);
}
}
}
void AcsController::disableCtrlValData() {
double unitQuat[4] = {0, 0, 0, 1};
double errAng = 0;
{
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double));
std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double));
ctrlValData.errAng.value = errAng;
ctrlValData.setValidity(false, true);
}
}
}
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) { LocalDataPoolManager &poolManager) {
// MGM Raw // MGM Raw
@ -524,11 +566,13 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
// MEKF // MEKF
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf); localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf); localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf);
localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus);
poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), false, 5.0}); poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), false, 5.0});
// Ctrl Values // Ctrl Values
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat); localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat); localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng); localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng);
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_ROT_RATE, &tgtRotRate);
poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), false, 5.0}); poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), false, 5.0});
// Actuator CMD // Actuator CMD
localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque); localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque);
@ -795,11 +839,3 @@ void AcsController::copyGyrData() {
} }
} }
} }
ReturnValue_t AcsController::initialize() {
ReturnValue_t result = parameterHelper.initialize();
if (result != returnvalue::OK) {
return result;
}
return ExtendedControllerBase::initialize();
}

19
mission/controller/AcsController.h
View File

@ -10,6 +10,7 @@
#include "acs/ActuatorCmd.h" #include "acs/ActuatorCmd.h"
#include "acs/Guidance.h" #include "acs/Guidance.h"
#include "acs/MultiplicativeKalmanFilter.h"
#include "acs/Navigation.h" #include "acs/Navigation.h"
#include "acs/SensorProcessing.h" #include "acs/SensorProcessing.h"
#include "acs/control/Detumble.h" #include "acs/control/Detumble.h"
@ -49,11 +50,15 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
Detumble detumble; Detumble detumble;
PtgCtrl ptgCtrl; PtgCtrl ptgCtrl;
uint8_t detumbleCounter;
uint8_t multipleRwUnavailableCounter;
ParameterHelper parameterHelper; ParameterHelper parameterHelper;
uint8_t detumbleCounter = 0;
uint8_t multipleRwUnavailableCounter = 0;
bool mekfInvalidFlag = true;
uint8_t mekfInvalidCounter = 0;
int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
int16_t cmdDipolMtqs[3] = {0, 0, 0};
#if OBSW_THREAD_TRACING == 1 #if OBSW_THREAD_TRACING == 1
uint32_t opCounter = 0; uint32_t opCounter = 0;
#endif #endif
@ -79,6 +84,12 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole, ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
uint16_t dipoleTorqueDuration, int32_t rw1Speed, int32_t rw2Speed, uint16_t dipoleTorqueDuration, int32_t rw1Speed, int32_t rw2Speed,
int32_t rw3Speed, int32_t rw4Speed, uint16_t rampTime); int32_t rw3Speed, int32_t rw4Speed, uint16_t rampTime);
void updateActuatorCmdData(int16_t mtqTargetDipole[3]);
void updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTargetSpeed[4],
int16_t mtqTargetDipole[3]);
void updateCtrlValData(double errAng);
void updateCtrlValData(double tgtQuat[4], double errQuat[4], double errAng);
void disableCtrlValData();
/* ACS Sensor Values */ /* ACS Sensor Values */
ACS::SensorValues sensorValues; ACS::SensorValues sensorValues;
@ -169,12 +180,14 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
acsctrl::MekfData mekfData; acsctrl::MekfData mekfData;
PoolEntry<double> quatMekf = PoolEntry<double>(4); PoolEntry<double> quatMekf = PoolEntry<double>(4);
PoolEntry<double> satRotRateMekf = PoolEntry<double>(3); PoolEntry<double> satRotRateMekf = PoolEntry<double>(3);
PoolEntry<uint8_t> mekfStatus = PoolEntry<uint8_t>();
// Ctrl Values // Ctrl Values
acsctrl::CtrlValData ctrlValData; acsctrl::CtrlValData ctrlValData;
PoolEntry<double> tgtQuat = PoolEntry<double>(4); PoolEntry<double> tgtQuat = PoolEntry<double>(4);
PoolEntry<double> errQuat = PoolEntry<double>(4); PoolEntry<double> errQuat = PoolEntry<double>(4);
PoolEntry<double> errAng = PoolEntry<double>(); PoolEntry<double> errAng = PoolEntry<double>();
PoolEntry<double> tgtRotRate = PoolEntry<double>(4);
// Actuator CMD // Actuator CMD
acsctrl::ActuatorCmdData actuatorCmdData; acsctrl::ActuatorCmdData actuatorCmdData;

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

@ -342,7 +342,41 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
break; break;
case (0x9): // TargetModeControllerParameters case (0x9): // IdleModeControllerParameters
switch (parameterId) {
case 0x0:
parameterWrapper->set(targetModeControllerParameters.zeta);
break;
case 0x1:
parameterWrapper->set(targetModeControllerParameters.om);
break;
case 0x2:
parameterWrapper->set(targetModeControllerParameters.omMax);
break;
case 0x3:
parameterWrapper->set(targetModeControllerParameters.qiMin);
break;
case 0x4:
parameterWrapper->set(targetModeControllerParameters.gainNullspace);
break;
case 0x5:
parameterWrapper->set(targetModeControllerParameters.desatMomentumRef);
break;
case 0x6:
parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
break;
case 0x7:
parameterWrapper->set(targetModeControllerParameters.desatOn);
break;
case 0x8:
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0xA): // TargetModeControllerParameters
switch (parameterId) { switch (parameterId) {
case 0x0: case 0x0:
parameterWrapper->set(targetModeControllerParameters.zeta); parameterWrapper->set(targetModeControllerParameters.zeta);
@ -408,7 +442,61 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
break; break;
case (0xA): // NadirModeControllerParameters case (0xB): // GsTargetModeControllerParameters
switch (parameterId) {
case 0x0:
parameterWrapper->set(targetModeControllerParameters.zeta);
break;
case 0x1:
parameterWrapper->set(targetModeControllerParameters.om);
break;
case 0x2:
parameterWrapper->set(targetModeControllerParameters.omMax);
break;
case 0x3:
parameterWrapper->set(targetModeControllerParameters.qiMin);
break;
case 0x4:
parameterWrapper->set(targetModeControllerParameters.gainNullspace);
break;
case 0x5:
parameterWrapper->set(targetModeControllerParameters.desatMomentumRef);
break;
case 0x6:
parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
break;
case 0x7:
parameterWrapper->set(targetModeControllerParameters.desatOn);
break;
case 0x8:
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
break;
case 0x9:
parameterWrapper->set(targetModeControllerParameters.refDirection);
break;
case 0xA:
parameterWrapper->set(targetModeControllerParameters.refRotRate);
break;
case 0xB:
parameterWrapper->set(targetModeControllerParameters.quatRef);
break;
case 0xC:
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
break;
case 0xD:
parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
break;
case 0xE:
parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
break;
case 0xF:
parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0xC): // NadirModeControllerParameters
switch (parameterId) { switch (parameterId) {
case 0x0: case 0x0:
parameterWrapper->set(nadirModeControllerParameters.zeta); parameterWrapper->set(nadirModeControllerParameters.zeta);
@ -450,7 +538,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
break; break;
case (0xB): // InertialModeControllerParameters case (0xD): // InertialModeControllerParameters
switch (parameterId) { switch (parameterId) {
case 0x0: case 0x0:
parameterWrapper->set(inertialModeControllerParameters.zeta); parameterWrapper->set(inertialModeControllerParameters.zeta);
@ -492,7 +580,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
break; break;
case (0xC): // StrParameters case (0xE): // StrParameters
switch (parameterId) { switch (parameterId) {
case 0x0: case 0x0:
parameterWrapper->set(strParameters.exclusionAngle); parameterWrapper->set(strParameters.exclusionAngle);
@ -504,7 +592,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
break; break;
case (0xD): // GpsParameters case (0xF): // GpsParameters
switch (parameterId) { switch (parameterId) {
case 0x0: case 0x0:
parameterWrapper->set(gpsParameters.timeDiffVelocityMax); parameterWrapper->set(gpsParameters.timeDiffVelocityMax);
@ -513,7 +601,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
break; break;
case (0xE): // SunModelParameters case (0x10): // SunModelParameters
switch (parameterId) { switch (parameterId) {
case 0x0: case 0x0:
parameterWrapper->set(sunModelParameters.domega); parameterWrapper->set(sunModelParameters.domega);
@ -543,7 +631,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
break; break;
case (0xF): // KalmanFilterParameters case (0x11): // KalmanFilterParameters
switch (parameterId) { switch (parameterId) {
case 0x0: case 0x0:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR); parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR);
@ -567,7 +655,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
break; break;
case (0x10): // MagnetorquesParameter case (0x12): // MagnetorquesParameter
switch (parameterId) { switch (parameterId) {
case 0x0: case 0x0:
parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix); parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix);
@ -594,7 +682,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
break; break;
case (0x11): // DetumbleParameter case (0x13): // DetumbleParameter
switch (parameterId) { switch (parameterId) {
case 0x0: case 0x0:
parameterWrapper->set(detumbleParameter.detumblecounter); parameterWrapper->set(detumbleParameter.detumblecounter);

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

@ -841,10 +841,12 @@ class AcsParameters : public HasParametersIF {
uint8_t enableAntiStiction = true; uint8_t enableAntiStiction = true;
} pointingLawParameters; } pointingLawParameters;
struct IdleModeControllerParameters : PointingLawParameters {
} idleModeControllerParameters;
struct TargetModeControllerParameters : PointingLawParameters { struct TargetModeControllerParameters : PointingLawParameters {
double refDirection[3] = {-1, 0, 0}; // Antenna double refDirection[3] = {-1, 0, 0}; // Antenna
double refRotRate[3] = {0, 0, 0}; // Not used atm, do we want an option to double refRotRate[3] = {0, 0, 0};
// give this as an input- currently en calculation is done
double quatRef[4] = {0, 0, 0, 1}; double quatRef[4] = {0, 0, 0, 1};
int8_t timeElapsedMax = 10; // rot rate calculations int8_t timeElapsedMax = 10; // rot rate calculations
@ -860,9 +862,20 @@ class AcsParameters : public HasParametersIF {
double blindRotRate = 1 * M_PI / 180; double blindRotRate = 1 * M_PI / 180;
} targetModeControllerParameters; } targetModeControllerParameters;
struct GsTargetModeControllerParameters : PointingLawParameters {
double refDirection[3] = {-1, 0, 0}; // Antenna
int8_t timeElapsedMax = 10; // rot rate calculations
// Default is Stuttgart GS
double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude
double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude
double altitudeTgt = 500; // [m]
} gsTargetModeControllerParameters;
struct NadirModeControllerParameters : PointingLawParameters { struct NadirModeControllerParameters : PointingLawParameters {
double refDirection[3] = {-1, 0, 0}; // Antenna double refDirection[3] = {-1, 0, 0}; // Antenna
double quatRef[4] = {0, 0, 0, 1}; double quatRef[4] = {0, 0, 0, 1};
double refRotRate[3] = {0, 0, 0};
int8_t timeElapsedMax = 10; // rot rate calculations int8_t timeElapsedMax = 10; // rot rate calculations
} nadirModeControllerParameters; } nadirModeControllerParameters;

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

@ -1,10 +1,3 @@
/*
* Guidance.cpp
*
* Created on: 6 Jun 2022
* Author: Robin Marquardt
*/
#include "Guidance.h" #include "Guidance.h"
#include <fsfw/datapool/PoolReadGuard.h> #include <fsfw/datapool/PoolReadGuard.h>
@ -19,74 +12,50 @@
#include "util/CholeskyDecomposition.h" #include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h" #include "util/MathOperations.h"
Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; } Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters_) {}
Guidance::~Guidance() {} Guidance::~Guidance() {}
void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) { void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3],
if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) or double sunDirI[3], double refDirB[3], double quatBI[4],
not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { // ToDo: if file does not exist anymore double targetQuat[4], double targetSatRotRate[3]) {
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir,
3 * sizeof(double));
} else {
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop,
3 * sizeof(double));
}
std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef,
3 * sizeof(double));
}
void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
double targetQuat[4], double refSatRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion to groundstation or given latitude, longitude and altitude // Calculation of target quaternion to groundstation or given latitude, longitude and altitude
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Transform longitude, latitude and altitude to cartesian coordiantes (earth // transform longitude, latitude and altitude to ECEF
// fixed/centered frame) double targetE[3] = {0, 0, 0};
double targetCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt( MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.latitudeTgt,
acsParameters.targetModeControllerParameters.longitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.targetModeControllerParameters.altitudeTgt, targetCart); acsParameters.targetModeControllerParameters.altitudeTgt, targetE);
// Position of the satellite in the earth/fixed frame via GPS // target direction in the ECEF frame
double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
// Target direction in the ECEF frame
double targetDirE[3] = {0, 0, 0}; double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3); VectorOperations<double>::subtract(targetE, posSatE, targetDirE, 3);
// Transformation between ECEF and IJK frame // transformation between ECEF and ECI frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE); MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
// Transformation between ECEF and Body frame // transformation between ECEF and Body frame
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmBI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double quatBJ[4] = {0, 0, 0, 0};
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
QuaternionOperations::toDcm(quatBJ, dcmBJ); QuaternionOperations::toDcm(quatBI, dcmBI);
MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3); MatrixOperations<double>::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3);
// Target Direction in the body frame // target Direction in the body frame
double targetDirB[3] = {0, 0, 0}; double targetDirB[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1); MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
// rotation quaternion from two vectors // rotation quaternion from two vectors
double refDir[3] = {0, 0, 0}; double refDir[3] = {0, 0, 0};
refDir[0] = acsParameters.targetModeControllerParameters.refDirection[0]; refDir[0] = acsParameters.targetModeControllerParameters.refDirection[0];
refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1]; refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1];
@ -106,15 +75,13 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
VectorOperations<double>::normalize(targetQuat, targetQuat, 4); VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of reference rotation rate // calculation of reference rotation rate
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
double velSatE[3] = {0, 0, 0};
std::memcpy(velSatE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, velSatBPart2[3] = {0, 0, 0}; double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, velSatBPart2[3] = {0, 0, 0};
// Velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E // velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E
MatrixOperations<double>::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1); MatrixOperations<double>::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1);
double dcmBEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmBEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(*dcmBJ, *dcmJEDot, *dcmBEDot, 3, 3, 3); MatrixOperations<double>::multiply(*dcmBI, *dcmIEDot, *dcmBEDot, 3, 3, 3);
MatrixOperations<double>::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1); MatrixOperations<double>::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1);
VectorOperations<double>::add(velSatBPart1, velSatBPart2, velSatB, 3); VectorOperations<double>::add(velSatBPart1, velSatBPart2, velSatB, 3);
@ -124,21 +91,14 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
double satRateDir[3] = {0, 0, 0}; double satRateDir[3] = {0, 0, 0};
VectorOperations<double>::cross(velSatB, targetDirB, satRateDir); VectorOperations<double>::cross(velSatB, targetDirB, satRateDir);
VectorOperations<double>::normalize(satRateDir, satRateDir, 3); VectorOperations<double>::normalize(satRateDir, satRateDir, 3);
VectorOperations<double>::mulScalar(satRateDir, normRefSatRate, refSatRate, 3); VectorOperations<double>::mulScalar(satRateDir, normRefSatRate, targetSatRotRate, 3);
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of reference rotation rate in case of star tracker blinding // Calculation of reference rotation rate in case of star tracker blinding
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
if (acsParameters.targetModeControllerParameters.avoidBlindStr) { if (acsParameters.targetModeControllerParameters.avoidBlindStr) {
double sunDirB[3] = {0, 0, 0}; double sunDirB[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmBI, sunDirI, sunDirB, 3, 3, 1);
if (susDataProcessed->sunIjkModel.isValid()) {
double sunDirJ[3] = {0, 0, 0};
std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
} else {
std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double));
}
double exclAngle = acsParameters.strParameters.exclusionAngle, double exclAngle = acsParameters.strParameters.exclusionAngle,
blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart, blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart,
@ -148,18 +108,14 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
if (!(strBlindAvoidFlag)) { if (!(strBlindAvoidFlag)) {
double critSightAngle = blindStart * exclAngle; double critSightAngle = blindStart * exclAngle;
if (sightAngleSun < critSightAngle) { if (sightAngleSun < critSightAngle) {
strBlindAvoidFlag = true; strBlindAvoidFlag = true;
} }
} }
else { else {
if (sightAngleSun < blindEnd * exclAngle) { if (sightAngleSun < blindEnd * exclAngle) {
double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate; double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate;
double blindRefRate[3] = {0, 0, 0}; double blindRefRate[3] = {0, 0, 0};
if (sunDirB[1] < 0) { if (sunDirB[1] < 0) {
blindRefRate[0] = normBlindRefRate; blindRefRate[0] = normBlindRefRate;
blindRefRate[1] = 0; blindRefRate[1] = 0;
@ -169,21 +125,353 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
blindRefRate[1] = 0; blindRefRate[1] = 0;
blindRefRate[2] = 0; blindRefRate[2] = 0;
} }
VectorOperations<double>::add(blindRefRate, targetSatRotRate, targetSatRotRate, 3);
VectorOperations<double>::add(blindRefRate, refSatRate, refSatRate, 3);
} else { } else {
strBlindAvoidFlag = false; strBlindAvoidFlag = false;
} }
} }
} }
// revert calculated quaternion from qBX to qIX
double quatIB[4] = {0, 0, 0, 1};
QuaternionOperations::inverse(quatBI, quatIB);
QuaternionOperations::multiply(quatIB, targetQuat, targetQuat);
} }
void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3],
double *refSatRate) { double targetQuat[4], double targetSatRotRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for target pointing
//-------------------------------------------------------------------------------------
// transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
double targetE[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.targetModeControllerParameters.latitudeTgt,
acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.targetModeControllerParameters.altitudeTgt, targetE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(targetE, posSatE, targetDirE, 3);
// transformation between ECEF and ECI frame
double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
// target direction in the ECI frame
double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmIE, targetE, targetI, 3, 3, 1);
VectorOperations<double>::subtract(targetI, posSatI, targetDirI, 3);
// x-axis aligned with target direction
// this aligns with the camera, E- and S-band antennas
double xAxis[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirI, xAxis, 3);
// transform velocity into inertial frame
double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1);
VectorOperations<double>::add(velPart1, velPart2, velocityI, 3);
// orbital normal vector of target and velocity vector
double orbitalNormalI[3] = {0, 0, 0};
VectorOperations<double>::cross(posSatI, velocityI, orbitalNormalI);
VectorOperations<double>::normalize(orbitalNormalI, orbitalNormalI, 3);
// y-axis of satellite in orbit plane so that z-axis is parallel to long side of picture
// resolution
double yAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(orbitalNormalI, xAxis, yAxis);
VectorOperations<double>::normalize(yAxis, yAxis, 3);
// z-axis completes RHS
double zAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(xAxis, yAxis, zAxis);
// join transformation matrix
double dcmIX[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
{xAxis[1], yAxis[1], zAxis[1]},
{xAxis[2], yAxis[2], zAxis[2]}};
QuaternionOperations::fromDcm(dcmIX, targetQuat);
int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
}
void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3],
double targetQuat[4], double targetSatRotRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for ground station pointing
//-------------------------------------------------------------------------------------
// transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
double groundStationE[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.gsTargetModeControllerParameters.latitudeTgt,
acsParameters.gsTargetModeControllerParameters.longitudeTgt,
acsParameters.gsTargetModeControllerParameters.altitudeTgt, groundStationE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(groundStationE, posSatE, targetDirE, 3);
// transformation between ECEF and ECI frame
double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
// target direction in the ECI frame
double posSatI[3] = {0, 0, 0}, groundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmIE, groundStationE, groundStationI, 3, 3, 1);
VectorOperations<double>::subtract(groundStationI, posSatI, groundStationDirI, 3);
// negative x-axis aligned with target direction
// this aligns with the camera, E- and S-band antennas
double xAxis[3] = {0, 0, 0};
VectorOperations<double>::normalize(groundStationDirI, xAxis, 3);
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
// get sun vector model in ECI
VectorOperations<double>::normalize(sunDirI, sunDirI, 3);
// calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector
// z = sPerpenticular = s - sParallel = s - (x*s)/norm(x)^2 * x
double xDotS = VectorOperations<double>::dot(xAxis, sunDirI);
xDotS /= pow(VectorOperations<double>::norm(xAxis, 3), 2);
double sunParallel[3], zAxis[3];
VectorOperations<double>::mulScalar(xAxis, xDotS, sunParallel, 3);
VectorOperations<double>::subtract(sunDirI, sunParallel, zAxis, 3);
VectorOperations<double>::normalize(zAxis, zAxis, 3);
// y-axis completes RHS
double yAxis[3];
VectorOperations<double>::cross(zAxis, xAxis, yAxis);
VectorOperations<double>::normalize(yAxis, yAxis, 3);
// join transformation matrix
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
{xAxis[1], yAxis[1], zAxis[1]},
{xAxis[2], yAxis[2], zAxis[2]}};
QuaternionOperations::fromDcm(dcmTgt, targetQuat);
int8_t timeElapsedMax = acsParameters.gsTargetModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
}
void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion to sun
//-------------------------------------------------------------------------------------
// positive z-Axis of EIVE in direction of sun
double zAxis[3] = {0, 0, 0};
VectorOperations<double>::normalize(sunDirI, zAxis, 3);
// assign helper vector (north pole inertial)
double helperVec[3] = {0, 0, 1};
// construct y-axis from helper vector and z-axis
double yAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(zAxis, helperVec, yAxis);
VectorOperations<double>::normalize(yAxis, yAxis, 3);
// x-axis completes RHS
double xAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(yAxis, zAxis, xAxis);
VectorOperations<double>::normalize(xAxis, xAxis, 3);
// join transformation matrix
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
{xAxis[1], yAxis[1], zAxis[1]},
{xAxis[2], yAxis[2], zAxis[2]}};
QuaternionOperations::fromDcm(dcmTgt, targetQuat);
//----------------------------------------------------------------------------
// Calculation of reference rotation rate
//----------------------------------------------------------------------------
refSatRate[0] = 0;
refSatRate[1] = 0;
refSatRate[2] = 0;
}
void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4],
double targetQuat[4], double refDirB[3],
double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing
//-------------------------------------------------------------------------------------
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
// transformation between ECEF and ECI frame
double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
// transformation between ECEF and Body frame
double dcmBI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
QuaternionOperations::toDcm(quatBI, dcmBI);
MatrixOperations<double>::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3);
// target Direction in the body frame
double targetDirB[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
// rotation quaternion from two vectors
double refDir[3] = {0, 0, 0};
refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0];
refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1];
refDir[2] = acsParameters.nadirModeControllerParameters.refDirection[2];
double noramlizedTargetDirB[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
VectorOperations<double>::normalize(refDir, refDir, 3);
double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
double normRefDir = VectorOperations<double>::norm(refDir, 3);
double crossDir[3] = {0, 0, 0};
double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir);
targetQuat[0] = crossDir[0];
targetQuat[1] = crossDir[1];
targetQuat[2] = crossDir[2];
targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections);
VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of reference rotation rate // Calculation of reference rotation rate
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
refSatRate[0] = 0;
refSatRate[1] = 0;
refSatRate[2] = 0;
// revert calculated quaternion from qBX to qIX
double quatIB[4] = {0, 0, 0, 1};
QuaternionOperations::inverse(quatBI, quatIB);
QuaternionOperations::multiply(quatIB, targetQuat, targetQuat);
}
void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3],
double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing
//-------------------------------------------------------------------------------------
// transformation between ECEF and ECI frame
double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
// satellite position in inertial reference frame
double posSatI[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
// negative x-axis aligned with position vector
// this aligns with the camera, E- and S-band antennas
double xAxis[3] = {0, 0, 0};
VectorOperations<double>::normalize(posSatI, xAxis, 3);
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
// make z-Axis parallel to major part of camera resolution
double zAxis[3] = {0, 0, 0};
double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1);
VectorOperations<double>::add(velPart1, velPart2, velocityI, 3);
VectorOperations<double>::cross(xAxis, velocityI, zAxis);
VectorOperations<double>::normalize(zAxis, zAxis, 3);
// y-Axis completes RHS
double yAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(zAxis, xAxis, yAxis);
// join transformation matrix
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
{xAxis[1], yAxis[1], zAxis[1]},
{xAxis[2], yAxis[2], zAxis[2]}};
QuaternionOperations::fromDcm(dcmTgt, targetQuat);
int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate);
}
void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
double errorQuat[4], double errorSatRotRate[3], double errorAngle) {
// First calculate error quaternion between current and target orientation
QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
// Last calculate add rotation from reference quaternion
QuaternionOperations::multiply(refQuat, errorQuat, errorQuat);
// Keep scalar part of quaternion positive
if (errorQuat[3] < 0) {
VectorOperations<double>::mulScalar(errorQuat, -1, errorQuat, 4);
}
// Calculate error angle
errorAngle = QuaternionOperations::getAngle(errorQuat, true);
// Only give back error satellite rotational rate if orientation has already been aquired
if (errorAngle < 2. / 180. * M_PI) {
// First combine the target and reference satellite rotational rates
double combinedRefSatRotRate[3] = {0, 0, 0};
VectorOperations<double>::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3);
// Then substract the combined required satellite rotational rates from the actual rate
VectorOperations<double>::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate,
3);
} else {
// If orientation has not been aquired yet set satellite rotational rate to zero
errorSatRotRate = 0;
}
// target flag in matlab, importance, does look like it only gives feedback if pointing control is
// under 150 arcsec ??
}
void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
double targetSatRotRate[3], double errorQuat[4],
double errorSatRotRate[3], double errorAngle) {
// First calculate error quaternion between current and target orientation
QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
// Keep scalar part of quaternion positive
if (errorQuat[3] < 0) {
VectorOperations<double>::mulScalar(errorQuat, -1, errorQuat, 4);
}
// Calculate error angle
errorAngle = QuaternionOperations::getAngle(errorQuat, true);
// Only give back error satellite rotational rate if orientation has already been aquired
if (errorAngle < 2. / 180. * M_PI) {
// Then substract the combined required satellite rotational rates from the actual rate
VectorOperations<double>::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3);
} else {
// If orientation has not been aquired yet set satellite rotational rate to zero
errorSatRotRate = 0;
}
// target flag in matlab, importance, does look like it only gives feedback if pointing control is
// under 150 arcsec ??
}
void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
double *refSatRate) {
//-------------------------------------------------------------------------------------
// Calculation of target rotation rate
//-------------------------------------------------------------------------------------
double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) - double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) -
(timeSavedQuaternion.tv_sec + (timeSavedQuaternion.tv_sec +
timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6)); timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6));
@ -221,395 +509,6 @@ void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatIn
savedQuaternion[3] = quatInertialTarget[3]; savedQuaternion[3] = quatInertialTarget[3];
} }
void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
acsctrl::GpsDataProcessed *gpsDataProcessed,
acsctrl::MekfData *mekfData, timeval now,
double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for target pointing
//-------------------------------------------------------------------------------------
// Transform longitude, latitude and altitude to cartesian coordiantes (earth
// fixed/centered frame)
double targetCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.targetModeControllerParameters.latitudeTgt,
acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
// Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0};
std::memcpy(posSatE, gpsDataProcessed->gpsPosition.value, 3 * sizeof(double));
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
// Transformation between ECEF and IJK frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// Target Direction and position vector in the inertial frame
double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1);
// negative x-Axis aligned with target (Camera/E-band transmitter position)
double xAxis[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
// Transform velocity into inertial frame
double velocityE[3];
std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3);
// orbital normal vector
double orbitalNormalJ[3] = {0, 0, 0};
VectorOperations<double>::cross(posSatJ, velocityJ, orbitalNormalJ);
VectorOperations<double>::normalize(orbitalNormalJ, orbitalNormalJ, 3);
// y-Axis of satellite in orbit plane so that z-axis parallel to long side of picture resolution
double yAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(orbitalNormalJ, xAxis, yAxis);
VectorOperations<double>::normalize(yAxis, yAxis, 3);
// z-Axis completes RHS
double zAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(xAxis, yAxis, zAxis);
// Complete transformation matrix
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
{xAxis[1], yAxis[1], zAxis[1]},
{xAxis[2], yAxis[2], zAxis[2]}};
double quatInertialTarget[4] = {0, 0, 0, 0};
QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
// Transform in system relative to satellite frame
double quatBJ[4] = {0, 0, 0, 0};
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
}
void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for ground station pointing
//-------------------------------------------------------------------------------------
// Transform longitude, latitude and altitude to cartesian coordiantes (earth
// fixed/centered frame)
double groundStationCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.targetModeControllerParameters.latitudeTgt,
acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.targetModeControllerParameters.altitudeTgt, groundStationCart);
// Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);
// Transformation between ECEF and IJK frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// Target Direction and position vector in the inertial frame
double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1);
// negative x-Axis aligned with target (Camera/E-band transmitter position)
double xAxis[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
// get Sun Vector Model in ECI
double sunJ[3];
std::memcpy(sunJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
VectorOperations<double>::normalize(sunJ, sunJ, 3);
// calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector
// z = sPerpenticular = s - sParallel = s - (x*s)/norm(x)^2 * x
double xDotS = VectorOperations<double>::dot(xAxis, sunJ);
xDotS /= pow(VectorOperations<double>::norm(xAxis, 3), 2);
double sunParallel[3], zAxis[3];
VectorOperations<double>::mulScalar(xAxis, xDotS, sunParallel, 3);
VectorOperations<double>::subtract(sunJ, sunParallel, zAxis, 3);
VectorOperations<double>::normalize(zAxis, zAxis, 3);
// calculate y-axis
double yAxis[3];
VectorOperations<double>::cross(zAxis, xAxis, yAxis);
VectorOperations<double>::normalize(yAxis, yAxis, 3);
// Complete transformation matrix
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
{xAxis[1], yAxis[1], zAxis[1]},
{xAxis[2], yAxis[2], zAxis[2]}};
double quatInertialTarget[4] = {0, 0, 0, 0};
QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
// Transform in system relative to satellite frame
double quatBJ[4] = {0, 0, 0, 0};
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
}
void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion to sun
//-------------------------------------------------------------------------------------
double quatBJ[4] = {0, 0, 0, 0};
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
QuaternionOperations::toDcm(quatBJ, dcmBJ);
double sunDirJ[3] = {0, 0, 0}, sunDirB[3] = {0, 0, 0};
if (susDataProcessed->sunIjkModel.isValid()) {
std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
} else if (susDataProcessed->susVecTot.isValid()) {
std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double));
} else {
return;
}
// Transformation between ECEF and IJK frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// positive z-Axis of EIVE in direction of sun
double zAxis[3] = {0, 0, 0};
VectorOperations<double>::normalize(sunDirB, zAxis, 3);
// Assign helper vector (north pole inertial)
double helperVec[3] = {0, 0, 1};
//
double yAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(zAxis, helperVec, yAxis);
VectorOperations<double>::normalize(yAxis, yAxis, 3);
//
double xAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(yAxis, zAxis, xAxis);
VectorOperations<double>::normalize(xAxis, xAxis, 3);
// Transformation matrix to Sun, no further transforamtions, reference is already
// the EIVE body frame
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
{xAxis[1], yAxis[1], zAxis[1]},
{xAxis[2], yAxis[2], zAxis[2]}};
double quatSun[4] = {0, 0, 0, 0};
QuaternionOperations::fromDcm(dcmTgt, quatSun);
targetQuat[0] = quatSun[0];
targetQuat[1] = quatSun[1];
targetQuat[2] = quatSun[2];
targetQuat[3] = quatSun[3];
//----------------------------------------------------------------------------
// Calculation of reference rotation rate
//----------------------------------------------------------------------------
refSatRate[0] = 0;
refSatRate[1] = 0;
refSatRate[2] = 0;
}
void Guidance::quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
timeval now, double targetQuat[4],
double refSatRate[3]) { // old version of Nadir Pointing
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing
//-------------------------------------------------------------------------------------
// Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
// Transformation between ECEF and IJK frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// Transformation between ECEF and Body frame
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};
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
QuaternionOperations::toDcm(quatBJ, dcmBJ);
MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
// Target Direction in the body frame
double targetDirB[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
// rotation quaternion from two vectors
double refDir[3] = {0, 0, 0};
refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0];
refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1];
refDir[2] = acsParameters.nadirModeControllerParameters.refDirection[2];
double noramlizedTargetDirB[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
VectorOperations<double>::normalize(refDir, refDir, 3);
double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
double normRefDir = VectorOperations<double>::norm(refDir, 3);
double crossDir[3] = {0, 0, 0};
double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir);
targetQuat[0] = crossDir[0];
targetQuat[1] = crossDir[1];
targetQuat[2] = crossDir[2];
targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections);
VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
//-------------------------------------------------------------------------------------
// Calculation of reference rotation rate
//-------------------------------------------------------------------------------------
refSatRate[0] = 0;
refSatRate[1] = 0;
refSatRate[2] = 0;
}
void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
acsctrl::GpsDataProcessed *gpsDataProcessed,
acsctrl::MekfData *mekfData, timeval now, double targetQuat[4],
double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing
//-------------------------------------------------------------------------------------
// Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
// Transformation between ECEF and IJK frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// Target Direction in the body frame
double targetDirJ[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
// negative x-Axis aligned with target (Camera position)
double xAxis[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
// z-Axis parallel to long side of picture resolution
double zAxis[3] = {0, 0, 0}, velocityE[3];
std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3);
VectorOperations<double>::cross(xAxis, velocityJ, zAxis);
VectorOperations<double>::normalize(zAxis, zAxis, 3);
// y-Axis completes RHS
double yAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(zAxis, xAxis, yAxis);
// Complete transformation matrix
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
{xAxis[1], yAxis[1], zAxis[1]},
{xAxis[2], yAxis[2], zAxis[2]}};
double quatInertialTarget[4] = {0, 0, 0, 0};
QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax;
refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
// Transform in system relative to satellite frame
double quatBJ[4] = {0, 0, 0, 0};
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
}
void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) {
std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
4 * sizeof(double));
std::memcpy(refSatRate, acsParameters.inertialModeControllerParameters.refRotRate,
3 * sizeof(double));
}
void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4],
double refSatRate[3], double quatErrorComplete[4], double quatError[3],
double deltaRate[3]) {
double satRate[3] = {0, 0, 0};
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]},
{-quatRef[2], quatRef[3], quatRef[0], -quatRef[1]},
{quatRef[1], -quatRef[0], quatRef[3], -quatRef[2]},
{quatRef[0], -quatRef[1], quatRef[2], quatRef[3]}};
MatrixOperations<double>::multiply(*quatErrorMtx, targetQuat, quatErrorComplete, 4, 4, 1);
if (quatErrorComplete[3] < 0) {
quatErrorComplete[3] *= -1;
}
quatError[0] = quatErrorComplete[0];
quatError[1] = quatErrorComplete[1];
quatError[2] = quatErrorComplete[2];
// target flag in matlab, importance, does look like it only gives feedback if pointing control is
// under 150 arcsec ??
}
ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
double *rwPseudoInv) { double *rwPseudoInv) {
bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()); bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid());
@ -640,3 +539,16 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
return returnvalue::FAILED; return returnvalue::FAILED;
} }
} }
void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) {
if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) or
not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { // ToDo: if file does not exist anymore
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir,
3 * sizeof(double));
} else {
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop,
3 * sizeof(double));
}
std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef,
3 * sizeof(double));
}

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

@ -1,10 +1,3 @@
/*
* Guidance.h
*
* Created on: 6 Jun 2022
* Author: Robin Marquardt
*/
#ifndef GUIDANCE_H_ #ifndef GUIDANCE_H_
#define GUIDANCE_H_ #define GUIDANCE_H_
@ -23,49 +16,40 @@ class Guidance {
// Function to get the target quaternion and refence rotation rate from gps position and // Function to get the target quaternion and refence rotation rate from gps position and
// position of the ground station // position of the ground station
void targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3],
acsctrl::GpsDataProcessed *gpsDataProcessed, double refDirB[3], double quatBI[4], double targetQuat[4],
acsctrl::MekfData *mekfData, timeval now, double targetQuat[4], double targetSatRotRate[3]);
double refSatRate[3]); void targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double quatIX[4],
void targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, double targetSatRotRate[3]);
acsctrl::SusDataProcessed *susDataProcessed, void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4],
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, double targetSatRotRate[3]);
double targetQuat[4], double refSatRate[3]);
void targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
double targetQuat[4], double refSatRate[3]);
// Function to get the target quaternion and refence rotation rate for sun pointing after ground // Function to get the target quaternion and refence rotation rate for sun pointing after ground
// station // station
void sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, void targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]);
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, double targetQuat[4],
double refSatRate[3]);
// Function to get the target quaternion and refence rotation rate from gps position for Nadir // Function to get the target quaternion and refence rotation rate from gps position for Nadir
// pointing // pointing
void quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues, void targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4],
acsctrl::GpsDataProcessed *gpsDataProcessed, double targetQuat[4], double refDirB[3], double refSatRate[3]);
acsctrl::MekfData *mekfData, timeval now, double targetQuat[4], void targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3],
double refSatRate[3]); double targetQuat[4], double refSatRate[3]);
void quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
timeval now, double targetQuat[4], double refSatRate[3]);
// Function to get the target quaternion and refence rotation rate from parameters for inertial // @note: Calculates the error quaternion between the current orientation and the target
// pointing // quaternion, considering a reference quaternion. Additionally the difference between the actual
void inertialQuatPtg(double targetQuat[4], double refSatRate[3]); // and a desired satellite rotational rate is calculated, again considering a reference rotational
// rate. Lastly gives back the error angle of the error quaternion.
void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
double errorQuat[4], double errorSatRotRate[3], double errorAngle);
void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3],
double errorAngle);
// @note: compares target Quaternion and reference quaternion, also actual satellite rate and void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
// desired double *targetSatRotRate);
void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4],
double refSatRate[3], double quatErrorComplete[4], double quatError[3],
double deltaRate[3]);
void refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], // @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
double *refSatRate);
// @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
// reation wheel maybe can be done in "commanding.h" // reation wheel maybe can be done in "commanding.h"
ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv); ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);

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

@ -14,7 +14,7 @@
/*Initialisation of values for parameters in constructor*/ /*Initialisation of values for parameters in constructor*/
MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_) MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_)
: initialQuaternion{0.5, 0.5, 0.5, 0.5}, : initialQuaternion{0, 0, 0, 1},
initialCovarianceMatrix{{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, initialCovarianceMatrix{{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}} { {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}} {
loadAcsParameters(acsParameters_); loadAcsParameters(acsParameters_);
@ -27,12 +27,10 @@ void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_
kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters);
} }
void MultiplicativeKalmanFilter::reset() {} ReturnValue_t MultiplicativeKalmanFilter::init(
void MultiplicativeKalmanFilter::init(
const double *magneticField_, const bool validMagField_, const double *sunDir_, const double *magneticField_, const bool validMagField_, const double *sunDir_,
const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ, const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
const bool validMagModel) { // valids for "model measurements"? const bool validMagModel, acsctrl::MekfData *mekfData) { // valids for "model measurements"?
// check for valid mag/sun // check for valid mag/sun
if (validMagField_ && validSS && validSSModel && validMagModel) { if (validMagField_ && validSS && validSSModel && validMagModel) {
validInit = true; validInit = true;
@ -190,9 +188,13 @@ void MultiplicativeKalmanFilter::init(
initialCovarianceMatrix[5][3] = initGyroCov[2][0]; initialCovarianceMatrix[5][3] = initGyroCov[2][0];
initialCovarianceMatrix[5][4] = initGyroCov[2][1]; initialCovarianceMatrix[5][4] = initGyroCov[2][1];
initialCovarianceMatrix[5][5] = initGyroCov[2][2]; initialCovarianceMatrix[5][5] = initGyroCov[2][2];
updateDataSetWithoutData(mekfData, MekfStatus::INITIALIZED);
return KALMAN_INITIALIZED;
} else { } else {
// no initialisation possible, no valid measurements // no initialisation possible, no valid measurements
validInit = false; validInit = false;
updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
return KALMAN_UNINITIALIZED;
} }
} }
@ -208,33 +210,13 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
// Check for GYR Measurements // Check for GYR Measurements
int MDF = 0; // Matrix Dimension Factor int MDF = 0; // Matrix Dimension Factor
if (!validGYRs_) { if (!validGYRs_) {
{ updateDataSetWithoutData(mekfData, MekfStatus::NO_GYR_DATA);
PoolReadGuard pg(mekfData); return KALMAN_NO_GYR_DATA;
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 // Check for Model Calculations
else if (!validSSModel || !validMagModel) { else if (!validSSModel || !validMagModel) {
{ updateDataSetWithoutData(mekfData, MekfStatus::NO_MODEL_VECTORS);
PoolReadGuard pg(mekfData); return KALMAN_NO_MODEL_VECTORS;
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 // Check Measurements available from SS, MAG, STR
if (validSS && validMagField_ && validSTR_) { if (validSS && validMagField_ && validSTR_) {
@ -260,17 +242,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
MDF = 3; MDF = 3;
} else { } else {
sensorsAvail = 8; // no measurements sensorsAvail = 8; // no measurements
validMekf = false; updateDataSetWithoutData(mekfData, MekfStatus::NO_SUS_MGM_STR_DATA);
{
PoolReadGuard pg(mekfData);
if (pg.getReadResult() == returnvalue::OK) {
double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
double zeroVec[3] = {0.0, 0.0, 0.0};
std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
mekfData->setValidity(false, true);
}
}
return returnvalue::FAILED; return returnvalue::FAILED;
} }
@ -881,18 +853,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
double invResidualCov[MDF][MDF] = {{0}}; double invResidualCov[MDF][MDF] = {{0}};
int inversionFailed = MathOperations<double>::inverseMatrix(*residualCov, *invResidualCov, MDF); int inversionFailed = MathOperations<double>::inverseMatrix(*residualCov, *invResidualCov, MDF);
if (inversionFailed) { if (inversionFailed) {
{ updateDataSetWithoutData(mekfData, MekfStatus::COVARIANCE_INVERSION_FAILED);
PoolReadGuard pg(mekfData); return KALMAN_COVARIANCE_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed
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_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed
} }
// [K = P * H' / (H * P * H' + R)] // [K = P * H' / (H * P * H' + R)]
@ -1121,20 +1083,47 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
MatrixOperations<double>::multiply(*discTimeMatrix, *cov1, *cov1, 6, 6, 6); MatrixOperations<double>::multiply(*discTimeMatrix, *cov1, *cov1, 6, 6, 6);
MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6); MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6);
validMekf = true;
// Discrete Time Step updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst);
return KALMAN_RUNNING;
}
// Check for new data in measurement -> SensorProcessing ? void MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) {
double resetQuaternion[4] = {0, 0, 0, 1};
double resetCovarianceMatrix[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}};
std::memcpy(initialQuaternion, resetQuaternion, 4 * sizeof(double));
std::memcpy(initialCovarianceMatrix, resetCovarianceMatrix, 6 * 6 * sizeof(double));
updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
}
void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData,
MekfStatus mekfStatus) {
{ {
PoolReadGuard pg(mekfData); PoolReadGuard pg(mekfData);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mekfData->quatMekf.value, quatBJ, 4 * sizeof(double)); double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
std::memcpy(mekfData->satRotRateMekf.value, rotRateEst, 3 * sizeof(double)); double zeroVec[3] = {0.0, 0.0, 0.0};
std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
mekfData->quatMekf.setValid(false);
std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
mekfData->satRotRateMekf.setValid(false);
mekfData->mekfStatus = mekfStatus;
mekfData->setValidity(true, false);
}
}
}
void MultiplicativeKalmanFilter::updateDataSet(acsctrl::MekfData *mekfData,
MekfStatus mekfStatus, double quat[4],
double satRotRate[3]) {
{
PoolReadGuard pg(mekfData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mekfData->quatMekf.value, quat, 4 * sizeof(double));
std::memcpy(mekfData->satRotRateMekf.value, satRotRate, 3 * sizeof(double));
mekfData->mekfStatus = mekfStatus;
mekfData->setValidity(true, true); mekfData->setValidity(true, true);
} }
} }
return returnvalue::OK;
} }

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

@ -15,8 +15,7 @@
#ifndef MULTIPLICATIVEKALMANFILTER_H_ #ifndef MULTIPLICATIVEKALMANFILTER_H_
#define MULTIPLICATIVEKALMANFILTER_H_ #define MULTIPLICATIVEKALMANFILTER_H_
#include <stdint.h> //uint8_t #include <stdint.h>
#include <time.h> /*purpose, timeval ?*/
#include "../controllerdefinitions/AcsCtrlDefinitions.h" #include "../controllerdefinitions/AcsCtrlDefinitions.h"
#include "AcsParameters.h" #include "AcsParameters.h"
@ -30,18 +29,19 @@ class MultiplicativeKalmanFilter {
MultiplicativeKalmanFilter(AcsParameters *acsParameters_); MultiplicativeKalmanFilter(AcsParameters *acsParameters_);
virtual ~MultiplicativeKalmanFilter(); virtual ~MultiplicativeKalmanFilter();
void reset(); // NOT YET DEFINED - should only reset all mekf variables void reset(acsctrl::MekfData *mekfData);
/* @brief: init() - This function initializes the Kalman Filter and will provide the first /* @brief: init() - This function initializes the Kalman Filter and will provide the first
* quaternion through the QUEST algorithm * quaternion through the QUEST algorithm
* @param: magneticField_ magnetic field vector in the body frame * @param: magneticField_ magnetic field vector in the body frame
* sunDir_ sun direction vector in the body frame * sunDir_ sun direction vector in the body frame
* sunDirJ sun direction vector in the ECI frame * sunDirJ sun direction vector in the ECI frame
* magFieldJ magnetic field vector in the ECI frame * magFieldJ magnetic field vector in the ECI frame
*/ */
void init(const double *magneticField_, const bool validMagField_, const double *sunDir_, ReturnValue_t init(const double *magneticField_, const bool validMagField_, const double *sunDir_,
const bool validSS, const double *sunDirJ, const bool validSSModel, const bool validSS, const double *sunDirJ, const bool validSSModel,
const double *magFieldJ, const bool validMagModel); const double *magFieldJ, const bool validMagModel,
acsctrl::MekfData *mekfData);
/* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter /* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
* for the current step after the initalization * for the current step after the initalization
@ -63,11 +63,26 @@ class MultiplicativeKalmanFilter {
const double *sunDirJ, const bool validSSModel, const double *magFieldJ, const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
const bool validMagModel, double sampleTime, acsctrl::MekfData *mekfData); const bool validMagModel, double sampleTime, acsctrl::MekfData *mekfData);
enum MekfStatus : uint8_t {
UNINITIALIZED = 0,
NO_GYR_DATA = 1,
NO_MODEL_VECTORS = 2,
NO_SUS_MGM_STR_DATA = 3,
COVARIANCE_INVERSION_FAILED = 4,
INITIALIZED = 10,
RUNNING = 11,
};
// resetting Mekf // resetting Mekf
static constexpr uint8_t IF_KAL_ID = CLASS_ID::ACS_KALMAN; static constexpr uint8_t IF_KAL_ID = CLASS_ID::ACS_KALMAN;
static constexpr ReturnValue_t KALMAN_NO_GYR_MEAS = returnvalue::makeCode(IF_KAL_ID, 1); static constexpr ReturnValue_t KALMAN_UNINITIALIZED = returnvalue::makeCode(IF_KAL_ID, 2);
static constexpr ReturnValue_t KALMAN_NO_MODEL = returnvalue::makeCode(IF_KAL_ID, 2); static constexpr ReturnValue_t KALMAN_NO_GYR_DATA = returnvalue::makeCode(IF_KAL_ID, 3);
static constexpr ReturnValue_t KALMAN_INVERSION_FAILED = returnvalue::makeCode(IF_KAL_ID, 3); static constexpr ReturnValue_t KALMAN_NO_MODEL_VECTORS = returnvalue::makeCode(IF_KAL_ID, 4);
static constexpr ReturnValue_t KALMAN_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_KAL_ID, 5);
static constexpr ReturnValue_t KALMAN_COVARIANCE_INVERSION_FAILED =
returnvalue::makeCode(IF_KAL_ID, 6);
static constexpr ReturnValue_t KALMAN_INITIALIZED = returnvalue::makeCode(IF_KAL_ID, 7);
static constexpr ReturnValue_t KALMAN_RUNNING = returnvalue::makeCode(IF_KAL_ID, 8);
private: private:
/*Parameters*/ /*Parameters*/
@ -80,16 +95,17 @@ class MultiplicativeKalmanFilter {
double initialQuaternion[4]; /*after reset?QUEST*/ double initialQuaternion[4]; /*after reset?QUEST*/
double initialCovarianceMatrix[6][6]; /*after reset?QUEST*/ double initialCovarianceMatrix[6][6]; /*after reset?QUEST*/
double propagatedQuaternion[4]; /*Filter Quaternion for next step*/ double propagatedQuaternion[4]; /*Filter Quaternion for next step*/
bool validMekf;
uint8_t sensorsAvail; uint8_t sensorsAvail;
/*Outputs*/ /*Outputs*/
double quatBJ[4]; /* Output Quaternion */ double quatBJ[4]; /* Output Quaternion */
double biasGYR[3]; /*Between measured and estimated sat Rate*/ double biasGYR[3]; /*Between measured and estimated sat Rate*/
/*Parameter INIT*/ /*Parameter INIT*/
// double alpha, gamma, beta;
/*Functions*/ /*Functions*/
void loadAcsParameters(AcsParameters *acsParameters_); void loadAcsParameters(AcsParameters *acsParameters_);
void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus);
void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4],
double satRotRate[3]);
}; };
#endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */ #endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */

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

@ -1,10 +1,3 @@
/*
* Navigation.cpp
*
* Created on: 23 May 2022
* Author: Robin Marquardt
*/
#include "Navigation.h" #include "Navigation.h"
#include <fsfw/globalfunctions/math/MatrixOperations.h> #include <fsfw/globalfunctions/math/MatrixOperations.h>
@ -21,37 +14,37 @@ Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilt
Navigation::~Navigation() {} Navigation::~Navigation() {}
void Navigation::useMekf(ACS::SensorValues *sensorValues, ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData, acsctrl::SusDataProcessed *susDataProcessed,
ReturnValue_t *mekfValid) { acsctrl::MekfData *mekfData) {
double quatJB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
bool quatJBValid = sensorValues->strSet.caliQx.isValid() && bool quatIBValid = sensorValues->strSet.caliQx.isValid() &&
sensorValues->strSet.caliQy.isValid() && sensorValues->strSet.caliQy.isValid() &&
sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid(); sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid();
if (kalmanInit) { if (kalmanInit) {
*mekfValid = multiplicativeKalmanFilter.mekfEst( return multiplicativeKalmanFilter.mekfEst(
quatJB, quatJBValid, gyrDataProcessed->gyrVecTot.value, quatIB, quatIBValid, gyrDataProcessed->gyrVecTot.value,
gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value, gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value,
mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value, mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value, susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value, susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime, mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime, mekfData);
mekfData); // VALIDS FOR QUAT AND RATE ??
} else { } else {
multiplicativeKalmanFilter.init( ReturnValue_t result;
result = multiplicativeKalmanFilter.init(
mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(), mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(), susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(), susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid()); mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData);
kalmanInit = true; kalmanInit = true;
*mekfValid = returnvalue::OK; return result;
// 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
// initialized ?
} }
} }
void Navigation::resetMekf(acsctrl::MekfData *mekfData) {
multiplicativeKalmanFilter.reset(mekfData);
}

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

@ -1,10 +1,3 @@
/*
* Navigation.h
*
* Created on: 19 Apr 2022
* Author: Robin Marquardt
*/
#ifndef NAVIGATION_H_ #ifndef NAVIGATION_H_
#define NAVIGATION_H_ #define NAVIGATION_H_
@ -16,14 +9,14 @@
class Navigation { class Navigation {
public: public:
Navigation(AcsParameters *acsParameters_); // Input mode ? Navigation(AcsParameters *acsParameters_);
virtual ~Navigation(); virtual ~Navigation();
void useMekf(ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed, ReturnValue_t useMekf(ACS::SensorValues *sensorValues,
acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData, acsctrl::MgmDataProcessed *mgmDataProcessed,
ReturnValue_t *mekfValid); acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData);
void processSensorData(); void resetMekf(acsctrl::MekfData *mekfData);
protected: protected:
private: private:

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

@ -1,10 +1,3 @@
/*
* SensorProcessing.cpp
*
* Created on: 7 Mar 2022
* Author: Robin Marquardt
*/
#include "SensorProcessing.h" #include "SensorProcessing.h"
#include <fsfw/datapool/PoolReadGuard.h> #include <fsfw/datapool/PoolReadGuard.h>

11
mission/controller/acs/SusConverter.cpp
View File

@ -63,8 +63,7 @@ void SusConverter::calcAngle(const uint16_t susChannel[6]) {
} }
void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]) { void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]) {
uint8_t index; uint8_t index, k, l;
float k, l;
// while loop iterates above all calibration cells to use the different calibration functions in // while loop iterates above all calibration cells to use the different calibration functions in
// each cell // each cell
@ -75,10 +74,10 @@ void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffB
while (l < 3) { while (l < 3) {
l++; l++;
// if-condition to check in which cell the data point has to be // if-condition to check in which cell the data point has to be
if ((alphaBetaRaw[0] > ((completeCellWidth * ((k - 1) / 3)) - halfCellWidth) && if ((alphaBetaRaw[0] > ((completeCellWidth * ((k - 1) / 3.)) - halfCellWidth) &&
alphaBetaRaw[0] < ((completeCellWidth * (k / 3)) - halfCellWidth)) && alphaBetaRaw[0] < ((completeCellWidth * (k / 3.)) - halfCellWidth)) &&
(alphaBetaRaw[1] > ((completeCellWidth * ((l - 1) / 3)) - halfCellWidth) && (alphaBetaRaw[1] > ((completeCellWidth * ((l - 1) / 3.)) - halfCellWidth) &&
alphaBetaRaw[1] < ((completeCellWidth * (l / 3)) - halfCellWidth))) { alphaBetaRaw[1] < ((completeCellWidth * (l / 3.)) - halfCellWidth))) {
index = (3 * (k - 1) + l) - 1; // calculate the index of the datapoint for the right cell index = (3 * (k - 1) + l) - 1; // calculate the index of the datapoint for the right cell
alphaBetaCalibrated[0] = alphaBetaCalibrated[0] =
coeffAlpha[index][0] + coeffAlpha[index][1] * alphaBetaRaw[0] + coeffAlpha[index][0] + coeffAlpha[index][1] * alphaBetaRaw[0] +

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

@ -27,7 +27,7 @@ void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
} }
void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
const double *qError, const double *deltaRate, const double *rwPseudoInv, const double *errorQuat, const double *deltaRate, const double *rwPseudoInv,
double *torqueRws) { double *torqueRws) {
//------------------------------------------------------------------------------------------------ //------------------------------------------------------------------------------------------------
// Compute gain matrix K and P matrix // Compute gain matrix K and P matrix
@ -37,6 +37,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
double qErrorMin = pointingLawParameters->qiMin; double qErrorMin = pointingLawParameters->qiMin;
double omMax = pointingLawParameters->omMax; double omMax = pointingLawParameters->omMax;
double qError[3] = {errorQuat[0], errorQuat[1], errorQuat[2]};
double cInt = 2 * om * zeta; double cInt = 2 * om * zeta;
double kInt = 2 * pow(om, 2); double kInt = 2 * pow(om, 2);

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

@ -91,10 +91,12 @@ enum PoolIds : lp_id_t {
// MEKF // MEKF
SAT_ROT_RATE_MEKF, SAT_ROT_RATE_MEKF,
QUAT_MEKF, QUAT_MEKF,
MEKF_STATUS,
// Ctrl Values // Ctrl Values
TGT_QUAT, TGT_QUAT,
ERROR_QUAT, ERROR_QUAT,
ERROR_ANG, ERROR_ANG,
TGT_ROT_RATE,
// Actuator Cmd // Actuator Cmd
RW_TARGET_TORQUE, RW_TARGET_TORQUE,
RW_TARGET_SPEED, RW_TARGET_SPEED,
@ -109,7 +111,7 @@ static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5; static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5;
static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 4; static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 4;
static constexpr uint8_t MEKF_SET_ENTRIES = 2; static constexpr uint8_t MEKF_SET_ENTRIES = 2;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 3; static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 4;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
/** /**
@ -238,6 +240,7 @@ class MekfData : public StaticLocalDataSet<MEKF_SET_ENTRIES> {
lp_vec_t<double, 4> quatMekf = lp_vec_t<double, 4>(sid.objectId, QUAT_MEKF, this); lp_vec_t<double, 4> quatMekf = lp_vec_t<double, 4>(sid.objectId, QUAT_MEKF, this);
lp_vec_t<double, 3> satRotRateMekf = lp_vec_t<double, 3>(sid.objectId, SAT_ROT_RATE_MEKF, this); lp_vec_t<double, 3> satRotRateMekf = lp_vec_t<double, 3>(sid.objectId, SAT_ROT_RATE_MEKF, this);
lp_var_t<uint8_t> mekfStatus = lp_var_t<uint8_t>(sid.objectId, MEKF_STATUS, this);
private: private:
}; };
@ -249,6 +252,7 @@ class CtrlValData : public StaticLocalDataSet<CTRL_VAL_SET_ENTRIES> {
lp_vec_t<double, 4> tgtQuat = lp_vec_t<double, 4>(sid.objectId, TGT_QUAT, this); 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_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); lp_var_t<double> errAng = lp_var_t<double>(sid.objectId, ERROR_ANG, this);
lp_vec_t<double, 3> tgtRotRate = lp_vec_t<double, 3>(sid.objectId, TGT_ROT_RATE, this);
private: private:
}; };

8
mission/system/objects/AcsSubsystem.cpp
View File

@ -47,6 +47,11 @@ ReturnValue_t AcsSubsystem::initialize() {
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
sif::error << "AcsSubsystem: Subscribing for acs::MULTIPLE_RW_INVALID failed" << std::endl; sif::error << "AcsSubsystem: Subscribing for acs::MULTIPLE_RW_INVALID failed" << std::endl;
} }
result = manager->subscribeToEvent(eventQueue->getId(),
event::getEventId(acs::MEKF_INVALID_MODE_VIOLATION));
if (result != returnvalue::OK) {
sif::error << "AcsSubsystem: Subscribing for acs::MULTIPLE_RW_INVALID failed" << std::endl;
}
return Subsystem::initialize(); return Subsystem::initialize();
} }
@ -71,7 +76,8 @@ void AcsSubsystem::handleEventMessages() {
} }
} }
if (event.getEvent() == acs::SAFE_RATE_RECOVERY || if (event.getEvent() == acs::SAFE_RATE_RECOVERY ||
event.getEvent() == acs::MULTIPLE_RW_INVALID) { event.getEvent() == acs::MULTIPLE_RW_INVALID ||
event.getEvent() == acs::MEKF_INVALID_MODE_VIOLATION) {
CommandMessage msg; CommandMessage msg;
ModeMessage::setCmdModeMessage(msg, acs::AcsMode::SAFE, 0); ModeMessage::setCmdModeMessage(msg, acs::AcsMode::SAFE, 0);
status = commandQueue->sendMessage(commandQueue->getId(), &msg); status = commandQueue->sendMessage(commandQueue->getId(), &msg);