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

this might just work
Some checks failed
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert
2023-10-16 13:26:56 +02:00
parent 25354ee7b4
commit 17c253d19b
6 changed files with 58 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
mission/controller/AcsController.cpp
View File

@ -146,11 +146,14 @@ void AcsController::performControlOperation() {
}
void AcsController::performAttitudeControl() {
timeval timeAbsolute;
Clock::getClock_timeval(&timeAbsolute);
timeval timeRelative;
Clock::getClockMonotonic(&timeRelative);
if (timeRelative.tv_sec != 0 and oldTimeRelative.tv_sec != 0) {
timeDelta = timevalOperations::toDouble(timeRelative - oldTimeRelative);
}
oldTimeRelative = timeRelative;
ReturnValue_t result = navigation.useSpg4(timeAbsolute, &gpsDataProcessed);
if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
triggerEvent(acs::TLE_TOO_OLD);
@ -159,7 +162,7 @@ void AcsController::performAttitudeControl() {
tleTooOldFlag = false;
}
sensorProcessing.process(timeAbsolute, timeRelative, &sensorValues, &mgmDataProcessed,
sensorProcessing.process(timeAbsolute, timeDelta, &sensorValues, &mgmDataProcessed,
&susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
&gyrDataProcessed, &fusedRotRateData);
@ -168,13 +171,13 @@ void AcsController::performAttitudeControl() {
switch (mode) {
case acs::SAFE:
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
mekfInvalidFlag = true;
}
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE and not mekfLost) {
triggerEvent(acs::MEKF_AUTOMATIC_RESET);
navigation.resetMekf(&mekfData);
mekfLost = true;
@ -197,14 +200,14 @@ void AcsController::performAttitudeControl() {
case acs::PTG_TARGET_GS:
case acs::PTG_NADIR:
case acs::PTG_INERTIAL:
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
mekfInvalidCounter++;
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
mekfInvalidFlag = true;
}
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE and not mekfLost) {
triggerEvent(acs::MEKF_AUTOMATIC_RESET);
navigation.resetMekf(&mekfData);
mekfLost = true;
@ -418,7 +421,7 @@ void AcsController::performPointingCtrl() {
switch (mode) {
case acs::PTG_IDLE:
guidance.targetQuatPtgSun(now, susDataProcessed.sunIjkModel.value, targetQuat,
guidance.targetQuatPtgSun(timeDelta, susDataProcessed.sunIjkModel.value, targetQuat,
targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
@ -439,7 +442,7 @@ void AcsController::performPointingCtrl() {
break;
case acs::PTG_TARGET:
guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value,
guidance.targetQuatPtgThreeAxes(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
gpsDataProcessed.gpsVelocity.value, targetQuat,
targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
@ -463,7 +466,7 @@ void AcsController::performPointingCtrl() {
break;
case acs::PTG_TARGET_GS:
guidance.targetQuatPtgGs(now, gpsDataProcessed.gpsPosition.value,
guidance.targetQuatPtgGs(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
@ -484,9 +487,9 @@ void AcsController::performPointingCtrl() {
break;
case acs::PTG_NADIR:
guidance.targetQuatPtgNadirThreeAxes(now, gpsDataProcessed.gpsPosition.value,
gpsDataProcessed.gpsVelocity.value, targetQuat,
targetSatRotRate);
guidance.targetQuatPtgNadirThreeAxes(
timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef,
acsParameters.nadirModeControllerParameters.refRotRate, errorQuat,