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

Once upon a time Robin was made happy #800

Merged
meggert merged 20 commits from make-robin-happy-about-timestuff into dev-7.5.0 2023-12-04 11:21:53 +01:00
Conversation 1 Commits 20 Files Changed 10 +197 -235
10 changed files with 197 additions and 235 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CHANGELOG.md
View File

@ -24,6 +24,8 @@ will consitute of a breaking change warranting a new major release:
A filesystem change via `prefSD` on bootup, can lead to the TLE not being read, even A filesystem change via `prefSD` on bootup, can lead to the TLE not being read, even
though it is there. though it is there.
- Added action cmd to read the currently stored TLE. - Added action cmd to read the currently stored TLE.
- Both the `AcsController` and the `PwrController` now use the monotonic clock to calculate
the time difference.
# [v7.4.0] 2023-11-30 # [v7.4.0] 2023-11-30

206
mission/controller/AcsController.cpp
View File

@ -141,25 +141,7 @@ void AcsController::performControlOperation() {
} }
case InternalState::READY: { case InternalState::READY: {
if (mode != MODE_OFF) { if (mode != MODE_OFF) {
switch (mode) { performAttitudeControl();
case acs::SAFE:
switch (submode) {
case SUBMODE_NONE:
performSafe();
break;
case acs::DETUMBLE:
performDetumble();
break;
}
break;
case acs::PTG_IDLE:
case acs::PTG_TARGET:
case acs::PTG_TARGET_GS:
case acs::PTG_NADIR:
case acs::PTG_INERTIAL:
performPointingCtrl();
break;
}
} }
break; break;
} }
@ -168,39 +150,96 @@ void AcsController::performControlOperation() {
} }
} }
void AcsController::performSafe() { void AcsController::performAttitudeControl() {
timeval now; Clock::getClock_timeval(&timeAbsolute);
Clock::getClock_timeval(&now); Clock::getClockMonotonic(&timeRelative);
ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed); 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) { if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
triggerEvent(acs::TLE_TOO_OLD); triggerEvent(acs::TLE_TOO_OLD);
tleTooOldFlag = true; tleTooOldFlag = true;
} else if (result != Sgp4Propagator::TLE_TOO_OLD) { } else if (result != Sgp4Propagator::TLE_TOO_OLD) {
tleTooOldFlag = false; tleTooOldFlag = false;
} }
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters); sensorProcessing.process(timeAbsolute, timeDelta, &sensorValues, &mgmDataProcessed,
&susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed, fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
&gyrDataProcessed, &fusedRotRateData); &gyrDataProcessed, &fusedRotRateData);
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters); &susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
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) {
triggerEvent(acs::MEKF_AUTOMATIC_RESET);
navigation.resetMekf(&mekfData);
mekfLost = true;
}
} else if (mekfInvalidFlag) {
triggerEvent(acs::MEKF_RECOVERY);
mekfInvalidFlag = false;
}
switch (mode) {
case acs::SAFE:
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 and not mekfLost) {
triggerEvent(acs::MEKF_AUTOMATIC_RESET);
navigation.resetMekf(&mekfData);
mekfLost = true;
}
} else if (mekfInvalidFlag) {
triggerEvent(acs::MEKF_RECOVERY);
mekfInvalidFlag = false;
}
switch (submode) {
case SUBMODE_NONE:
performSafe();
break;
case acs::DETUMBLE:
performDetumble();
break;
}
break;
case acs::PTG_IDLE:
case acs::PTG_TARGET:
case acs::PTG_TARGET_GS:
case acs::PTG_NADIR:
case acs::PTG_INERTIAL:
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 and not mekfLost) {
triggerEvent(acs::MEKF_AUTOMATIC_RESET);
navigation.resetMekf(&mekfData);
mekfLost = true;
}
if (mekfInvalidCounter > acsParameters.onBoardParams.mekfViolationTimer) {
// Trigger this so STR FDIR can set the device faulty.
EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0,
0);
mekfInvalidCounter = 0;
}
commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration,
cmdSpeedRws[0], cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
acsParameters.rwHandlingParameters.rampTime);
return;
} else {
if (mekfInvalidFlag) {
triggerEvent(acs::MEKF_RECOVERY);
mekfInvalidFlag = false;
}
mekfInvalidCounter = 0;
}
performPointingCtrl();
break;
}
}
void AcsController::performSafe() {
// get desired satellite rate, sun direction to align to and inertia // get desired satellite rate, sun direction to align to and inertia
double sunTargetDir[3] = {0, 0, 0}; double sunTargetDir[3] = {0, 0, 0};
guidance.getTargetParamsSafe(sunTargetDir); guidance.getTargetParamsSafe(sunTargetDir);
@ -301,37 +340,6 @@ void AcsController::performSafe() {
} }
void AcsController::performDetumble() { void AcsController::performDetumble() {
timeval now;
Clock::getClock_timeval(&now);
ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed);
if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
triggerEvent(acs::TLE_TOO_OLD);
tleTooOldFlag = true;
} else {
tleTooOldFlag = false;
}
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
&gyrDataProcessed, &fusedRotRateData);
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
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) {
triggerEvent(acs::MEKF_AUTOMATIC_RESET);
navigation.resetMekf(&mekfData);
mekfLost = true;
}
} else if (mekfInvalidFlag) {
triggerEvent(acs::MEKF_RECOVERY);
mekfInvalidFlag = false;
}
acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy( acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(), mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTotDerivative.isValid(),
@ -394,51 +402,9 @@ void AcsController::performDetumble() {
} }
void AcsController::performPointingCtrl() { void AcsController::performPointingCtrl() {
timeval now;
Clock::getClock_timeval(&now);
ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed);
if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
triggerEvent(acs::TLE_TOO_OLD);
tleTooOldFlag = true;
} else {
tleTooOldFlag = false;
}
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
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) {
triggerEvent(acs::MEKF_AUTOMATIC_RESET);
navigation.resetMekf(&mekfData);
mekfLost = true;
}
if (mekfInvalidCounter > acsParameters.onBoardParams.mekfViolationTimer) {
// Trigger this so STR FDIR can set the device faulty.
EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0, 0);
mekfInvalidCounter = 0;
}
commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
acsParameters.rwHandlingParameters.rampTime);
return;
} else {
if (mekfInvalidFlag) {
triggerEvent(acs::MEKF_RECOVERY);
mekfInvalidFlag = false;
}
mekfInvalidCounter = 0;
}
uint8_t enableAntiStiction = true; uint8_t enableAntiStiction = true;
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}};
result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
if (result == returnvalue::FAILED) { if (result == returnvalue::FAILED) {
if (multipleRwUnavailableCounter >= if (multipleRwUnavailableCounter >=
acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) { acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) {
@ -460,7 +426,7 @@ void AcsController::performPointingCtrl() {
switch (mode) { switch (mode) {
case acs::PTG_IDLE: case acs::PTG_IDLE:
guidance.targetQuatPtgSun(now, susDataProcessed.sunIjkModel.value, targetQuat, guidance.targetQuatPtgSun(timeDelta, susDataProcessed.sunIjkModel.value, targetQuat,
targetSatRotRate); targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
@ -481,7 +447,7 @@ void AcsController::performPointingCtrl() {
break; break;
case acs::PTG_TARGET: case acs::PTG_TARGET:
guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value, guidance.targetQuatPtgThreeAxes(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
gpsDataProcessed.gpsVelocity.value, targetQuat, gpsDataProcessed.gpsVelocity.value, targetQuat,
targetSatRotRate); targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
@ -505,7 +471,7 @@ void AcsController::performPointingCtrl() {
break; break;
case acs::PTG_TARGET_GS: case acs::PTG_TARGET_GS:
guidance.targetQuatPtgGs(now, gpsDataProcessed.gpsPosition.value, guidance.targetQuatPtgGs(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
@ -526,9 +492,9 @@ void AcsController::performPointingCtrl() {
break; break;
case acs::PTG_NADIR: case acs::PTG_NADIR:
guidance.targetQuatPtgNadirThreeAxes(now, gpsDataProcessed.gpsPosition.value, guidance.targetQuatPtgNadirThreeAxes(
gpsDataProcessed.gpsVelocity.value, targetQuat, timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
targetSatRotRate); gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef, targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef,
acsParameters.nadirModeControllerParameters.refRotRate, errorQuat, acsParameters.nadirModeControllerParameters.refRotRate, errorQuat,

6
mission/controller/AcsController.h
View File

@ -45,6 +45,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
uint16_t startAtIndex) override; uint16_t startAtIndex) override;
protected: protected:
void performAttitudeControl();
void performSafe(); void performSafe();
void performDetumble(); void performDetumble();
void performPointingCtrl(); void performPointingCtrl();
@ -59,6 +60,11 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
SdCardMountedIF& sdcMan; SdCardMountedIF& sdcMan;
timeval timeAbsolute;
timeval timeRelative;
double timeDelta = 0.0;
timeval oldTimeRelative;
AcsParameters acsParameters; AcsParameters acsParameters;
SensorProcessing sensorProcessing; SensorProcessing sensorProcessing;
FusedRotationEstimation fusedRotationEstimation; FusedRotationEstimation fusedRotationEstimation;

32
mission/controller/PowerController.cpp
View File

@ -157,7 +157,12 @@ ReturnValue_t PowerController::checkModeCommand(Mode_t mode, Submode_t submode,
void PowerController::calculateStateOfCharge() { void PowerController::calculateStateOfCharge() {
// get time // get time
Clock::getClock_timeval(&now); Clock::getClockMonotonic(&now);
double timeDelta = 0.0;
if (now.tv_sec != 0 and oldTime.tv_sec != 0) {
timeDelta = timevalOperations::toDouble(now - oldTime);
}
oldTime = now;
// update EPS HK values // update EPS HK values
ReturnValue_t result = updateEpsData(); ReturnValue_t result = updateEpsData();
@ -173,8 +178,6 @@ void PowerController::calculateStateOfCharge() {
pwrCtrlCoreHk.setValidity(false, true); pwrCtrlCoreHk.setValidity(false, true);
} }
} }
// store time for next run
oldTime = now;
return; return;
} }
@ -195,12 +198,10 @@ void PowerController::calculateStateOfCharge() {
pwrCtrlCoreHk.coulombCounterCharge.setValid(false); pwrCtrlCoreHk.coulombCounterCharge.setValid(false);
} }
} }
// store time for next run
oldTime = now;
return; return;
} }
result = calculateCoulombCounterCharge(); result = calculateCoulombCounterCharge(timeDelta);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
// notifying events have already been triggered // notifying events have already been triggered
{ {
@ -215,9 +216,6 @@ void PowerController::calculateStateOfCharge() {
pwrCtrlCoreHk.coulombCounterCharge.setValid(false); pwrCtrlCoreHk.coulombCounterCharge.setValid(false);
} }
} }
// store time for next run
oldTime = now;
return;
} }
// commit to dataset // commit to dataset
@ -231,8 +229,6 @@ void PowerController::calculateStateOfCharge() {
pwrCtrlCoreHk.setValidity(true, true); pwrCtrlCoreHk.setValidity(true, true);
} }
} }
// store time for next run
oldTime = now;
} }
void PowerController::watchStateOfCharge() { void PowerController::watchStateOfCharge() {
@ -285,12 +281,14 @@ ReturnValue_t PowerController::calculateOpenCircuitVoltageCharge() {
return returnvalue::OK; return returnvalue::OK;
} }
ReturnValue_t PowerController::calculateCoulombCounterCharge() { ReturnValue_t PowerController::calculateCoulombCounterCharge(double timeDelta) {
double timeDiff = timevalOperations::toDouble(now - oldTime); if (timeDelta == 0.0) {
if (timeDiff > maxAllowedTimeDiff) { return returnvalue::FAILED;
}
if (timeDelta > maxAllowedTimeDiff) {
// should not be a permanent state so no spam protection required // should not be a permanent state so no spam protection required
triggerEvent(power::TIMEDELTA_OUT_OF_BOUNDS, static_cast<uint32_t>(timeDiff * 10)); triggerEvent(power::TIMEDELTA_OUT_OF_BOUNDS, static_cast<uint32_t>(timeDelta * 10));
sif::error << "Power Controller::Time delta too large for Coulomb Counter: " << timeDiff sif::error << "Power Controller::Time delta too large for Coulomb Counter: " << timeDelta
<< std::endl; << std::endl;
return returnvalue::FAILED; return returnvalue::FAILED;
} }
@ -298,7 +296,7 @@ ReturnValue_t PowerController::calculateCoulombCounterCharge() {
coulombCounterCharge = openCircuitVoltageCharge; coulombCounterCharge = openCircuitVoltageCharge;
} else { } else {
coulombCounterCharge = coulombCounterCharge =
coulombCounterCharge + iBat * CONVERT_FROM_MILLI * timeDiff * SECONDS_TO_HOURS; coulombCounterCharge + iBat * CONVERT_FROM_MILLI * timeDelta * SECONDS_TO_HOURS;
if (coulombCounterCharge >= coulombCounterChargeUpperThreshold) { if (coulombCounterCharge >= coulombCounterChargeUpperThreshold) {
coulombCounterCharge = coulombCounterChargeUpperThreshold; coulombCounterCharge = coulombCounterChargeUpperThreshold;
} }

2
mission/controller/PowerController.h
View File

@ -45,7 +45,7 @@ class PowerController : public ExtendedControllerBase, public ReceivesParameterM
void calculateStateOfCharge(); void calculateStateOfCharge();
void watchStateOfCharge(); void watchStateOfCharge();
ReturnValue_t calculateOpenCircuitVoltageCharge(); ReturnValue_t calculateOpenCircuitVoltageCharge();
ReturnValue_t calculateCoulombCounterCharge(); ReturnValue_t calculateCoulombCounterCharge(double timeDelta);
ReturnValue_t updateEpsData(); ReturnValue_t updateEpsData();
float charge2stateOfCharge(float capacity, bool coulombCounter); float charge2stateOfCharge(float capacity, bool coulombCounter);
ReturnValue_t lookUpTableOcvIdxFinder(float voltage, uint8_t& idx, bool paramCmd); ReturnValue_t lookUpTableOcvIdxFinder(float voltage, uint8_t& idx, bool paramCmd);

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

@ -75,9 +75,9 @@ class AcsParameters : public HasParametersIF {
{-0.007534, 1.253879, 0.006812}, {-0.007534, 1.253879, 0.006812},
{-0.037072, 0.006812, 1.313158}}; {-0.037072, 0.006812, 1.313158}};
float mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)}; double mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)};
float mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)}; double mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)};
float mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)}; double mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)};
float mgmVectorFilterWeight = 0.85; float mgmVectorFilterWeight = 0.85;
float mgmDerivativeFilterWeight = 0.99; float mgmDerivativeFilterWeight = 0.99;
uint8_t useMgm4 = false; uint8_t useMgm4 = false;
@ -787,10 +787,10 @@ class AcsParameters : public HasParametersIF {
/* var = sigma^2, sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is /* var = sigma^2, sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is
* assumed to be equal for the same class of sensors */ * assumed to be equal for the same class of sensors */
float gyr02variance[3] = {pow(4.6e-3, 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms double gyr02variance[3] = {pow(4.6e-3, 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms
pow(4.6e-3, 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms pow(4.6e-3, 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
pow(6.1e-3, 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms pow(6.1e-3, 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms
float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)}; double gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)};
uint8_t preferAdis = false; uint8_t preferAdis = false;
float gyrFilterWeight = 0.6; float gyrFilterWeight = 0.6;
} gyrHandlingParameters; } gyrHandlingParameters;

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

@ -4,21 +4,21 @@
#include <fsfw/globalfunctions/math/MatrixOperations.h> #include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h> #include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h> #include <fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h> #include <mission/controller/acs/util/MathOperations.h>
#include <cmath>
#include <filesystem> #include <filesystem>
#include <string>
#include "string.h"
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
Guidance::~Guidance() {} Guidance::~Guidance() {}
void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], [[deprecated]] void Guidance::targetQuatPtgSingleAxis(const timeval timeAbsolute, double posSatE[3],
double sunDirI[3], double refDirB[3], double quatBI[4], double velSatE[3], double sunDirI[3],
double targetQuat[4], double targetSatRotRate[3]) { double refDirB[3], double quatBI[4],
double targetQuat[4],
double targetSatRotRate[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
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
@ -38,7 +38,7 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
double dcmEI[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 dcmIE[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}}; double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); MathOperations<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE); MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -136,8 +136,9 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
QuaternionOperations::multiply(quatIB, targetQuat, targetQuat); QuaternionOperations::multiply(quatIB, targetQuat, targetQuat);
} }
void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], void Guidance::targetQuatPtgThreeAxes(const timeval timeAbsolute, const double timeDelta,
double targetQuat[4], double targetSatRotRate[3]) { double posSatE[3], double velSatE[3], double targetQuat[4],
double targetSatRotRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion for target pointing // Calculation of target quaternion for target pointing
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
@ -154,7 +155,7 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
double dcmEI[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 dcmIE[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}}; double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); MathOperations<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE); MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -199,11 +200,12 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
QuaternionOperations::fromDcm(dcmIX, targetQuat); QuaternionOperations::fromDcm(dcmIX, targetQuat);
int8_t timeElapsedMax = acsParameters->targetModeControllerParameters.timeElapsedMax; int8_t timeElapsedMax = acsParameters->targetModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate);
} }
void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], void Guidance::targetQuatPtgGs(const timeval timeAbsolute, const double timeDelta,
double targetQuat[4], double targetSatRotRate[3]) { double posSatE[3], double sunDirI[3], double targetQuat[4],
double targetSatRotRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion for ground station pointing // Calculation of target quaternion for ground station pointing
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
@ -221,7 +223,7 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
double dcmEI[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 dcmIE[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}}; double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); MathOperations<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE); MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -263,10 +265,10 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
QuaternionOperations::fromDcm(dcmTgt, targetQuat); QuaternionOperations::fromDcm(dcmTgt, targetQuat);
int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax; int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate);
} }
void Guidance::targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4], void Guidance::targetQuatPtgSun(double timeDelta, double sunDirI[3], double targetQuat[4],
double targetSatRotRate[3]) { double targetSatRotRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion to sun // Calculation of target quaternion to sun
@ -298,12 +300,13 @@ void Guidance::targetQuatPtgSun(timeval now, double sunDirI[3], double targetQua
// Calculation of reference rotation rate // Calculation of reference rotation rate
//---------------------------------------------------------------------------- //----------------------------------------------------------------------------
int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax; int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate);
} }
void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4], [[deprecated]] void Guidance::targetQuatPtgNadirSingleAxis(const timeval timeAbsolute,
double targetQuat[4], double refDirB[3], double posSatE[3], double quatBI[4],
double refSatRate[3]) { double targetQuat[4], double refDirB[3],
double refSatRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing // Calculation of target quaternion for Nadir pointing
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
@ -314,7 +317,7 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], doub
double dcmEI[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 dcmIE[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}}; double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); MathOperations<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE); MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -362,7 +365,8 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], doub
QuaternionOperations::multiply(quatIB, targetQuat, targetQuat); QuaternionOperations::multiply(quatIB, targetQuat, targetQuat);
} }
void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3], void Guidance::targetQuatPtgNadirThreeAxes(const timeval timeAbsolute, const double timeDelta,
double posSatE[3], double velSatE[3],
double targetQuat[4], double refSatRate[3]) { double targetQuat[4], double refSatRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing // Calculation of target quaternion for Nadir pointing
@ -371,7 +375,7 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl
double dcmEI[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 dcmIE[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}}; double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); MathOperations<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE); MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -407,7 +411,7 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl
QuaternionOperations::fromDcm(dcmTgt, targetQuat); QuaternionOperations::fromDcm(dcmTgt, targetQuat);
int8_t timeElapsedMax = acsParameters->nadirModeControllerParameters.timeElapsedMax; int8_t timeElapsedMax = acsParameters->nadirModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate); targetRotationRate(timeElapsedMax, timeDelta, targetQuat, refSatRate);
} }
void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
@ -448,23 +452,21 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
VectorOperations<double>::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3); VectorOperations<double>::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3);
} }
void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double timeDelta,
double *refSatRate) { double quatInertialTarget[4], double *refSatRate) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target rotation rate // Calculation of target rotation rate
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
double timeElapsed = now.tv_sec + now.tv_usec * 1e-6 -
(timeSavedQuaternion.tv_sec + timeSavedQuaternion.tv_usec * 1e-6);
if (VectorOperations<double>::norm(savedQuaternion, 4) == 0) { if (VectorOperations<double>::norm(savedQuaternion, 4) == 0) {
std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion)); std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
} }
if (timeElapsed < timeElapsedMax) { if (timeDelta < timeElapsedMax and timeDelta != 0.0) {
double q[4] = {0, 0, 0, 0}, qS[4] = {0, 0, 0, 0}; double q[4] = {0, 0, 0, 0}, qS[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(quatInertialTarget, q); QuaternionOperations::inverse(quatInertialTarget, q);
QuaternionOperations::inverse(savedQuaternion, qS); QuaternionOperations::inverse(savedQuaternion, qS);
double qDiff[4] = {0, 0, 0, 0}; double qDiff[4] = {0, 0, 0, 0};
VectorOperations<double>::subtract(q, qS, qDiff, 4); VectorOperations<double>::subtract(q, qS, qDiff, 4);
VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4); VectorOperations<double>::mulScalar(qDiff, 1. / timeDelta, qDiff, 4);
double tgtQuatVec[3] = {q[0], q[1], q[2]}; double tgtQuatVec[3] = {q[0], q[1], q[2]};
double qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; double qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
@ -488,11 +490,7 @@ void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double qua
refSatRate[2] = 0; refSatRate[2] = 0;
} }
timeSavedQuaternion = now; std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
savedQuaternion[0] = quatInertialTarget[0];
savedQuaternion[1] = quatInertialTarget[1];
savedQuaternion[2] = quatInertialTarget[2];
savedQuaternion[3] = quatInertialTarget[3];
} }
ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,

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

@ -17,25 +17,26 @@ class Guidance {
// Function to get the target quaternion and reference rotation rate from gps position and // Function to get the target quaternion and reference rotation rate from gps position and
// position of the ground station // position of the ground station
void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3], void targetQuatPtgSingleAxis(const timeval timeAbsolute, double posSatE[3], double velSatE[3],
double refDirB[3], double quatBI[4], double targetQuat[4], double sunDirI[3], double refDirB[3], double quatBI[4],
double targetSatRotRate[3]); double targetQuat[4], double targetSatRotRate[3]);
void targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double quatIX[4], void targetQuatPtgThreeAxes(const timeval timeAbsolute, const double timeDelta, double posSatE[3],
double targetSatRotRate[3]); double velSatE[3], double quatIX[4], double targetSatRotRate[3]);
void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4], void targetQuatPtgGs(const timeval timeAbsolute, const double timeDelta, double posSatE[3],
double targetSatRotRate[3]); double sunDirI[3], double quatIX[4], double targetSatRotRate[3]);
// Function to get the target quaternion and reference rotation rate for sun pointing after ground // Function to get the target quaternion and reference rotation rate for sun pointing after ground
// station // station
void targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4], void targetQuatPtgSun(const double timeDelta, double sunDirI[3], double targetQuat[4],
double targetSatRotRate[3]); double targetSatRotRate[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 targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4], void targetQuatPtgNadirSingleAxis(const timeval timeAbsolute, double posSatE[3], double quatBI[4],
double targetQuat[4], double refDirB[3], double refSatRate[3]); double targetQuat[4], double refDirB[3], double refSatRate[3]);
void targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3], void targetQuatPtgNadirThreeAxes(const timeval timeAbsolute, const double timeDelta,
double targetQuat[4], double refSatRate[3]); double posSatE[3], double velSatE[3], double targetQuat[4],
double refSatRate[3]);
// @note: Calculates the error quaternion between the current orientation and the target // @note: Calculates the error quaternion between the current orientation and the target
// quaternion, considering a reference quaternion. Additionally the difference between the actual // quaternion, considering a reference quaternion. Additionally the difference between the actual
@ -48,8 +49,8 @@ class Guidance {
double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3],
double &errorAngle); double &errorAngle);
void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], void targetRotationRate(const int8_t timeElapsedMax, const double timeDelta,
double *targetSatRotRate); double quatInertialTarget[4], double *targetSatRotRate);
// @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid // @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"
@ -59,7 +60,6 @@ class Guidance {
const AcsParameters *acsParameters; const AcsParameters *acsParameters;
bool strBlindAvoidFlag = false; bool strBlindAvoidFlag = false;
timeval timeSavedQuaternion;
double savedQuaternion[4] = {0, 0, 0, 0}; double savedQuaternion[4] = {0, 0, 0, 0};
double omegaRefSaved[3] = {0, 0, 0}; double omegaRefSaved[3] = {0, 0, 0};

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

@ -1,5 +1,7 @@
#include "SensorProcessing.h" #include "SensorProcessing.h"
#include <fsfw/timemanager/Stopwatch.h>
SensorProcessing::SensorProcessing() {} SensorProcessing::SensorProcessing() {}
SensorProcessing::~SensorProcessing() {} SensorProcessing::~SensorProcessing() {}
@ -7,7 +9,7 @@ SensorProcessing::~SensorProcessing() {}
void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value,
bool mgm1valid, const float *mgm2Value, bool mgm2valid, bool mgm1valid, const float *mgm2Value, bool mgm2valid,
const float *mgm3Value, bool mgm3valid, const float *mgm4Value, const float *mgm3Value, bool mgm3valid, const float *mgm4Value,
bool mgm4valid, timeval timeOfMgmMeasurement, bool mgm4valid, timeval timeAbsolute, double timeDelta,
const AcsParameters::MgmHandlingParameters *mgmParameters, const AcsParameters::MgmHandlingParameters *mgmParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed, acsctrl::GpsDataProcessed *gpsDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed) { acsctrl::MgmDataProcessed *mgmDataProcessed) {
@ -16,13 +18,13 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
double magIgrfModel[3] = {0.0, 0.0, 0.0}; double magIgrfModel[3] = {0.0, 0.0, 0.0};
bool gpsValid = false; bool gpsValid = false;
if (gpsDataProcessed->source.value != acs::GpsSource::NONE) { if (gpsDataProcessed->source.value != acs::GpsSource::NONE) {
// There seems to be a bug here, which causes the model vector to drift until infinity, if the
// model class is not initialized new every time. Works for now, but should be investigated.
Igrf13Model igrf13; Igrf13Model igrf13;
igrf13.schmidtNormalization(); igrf13.schmidtNormalization();
meggert marked this conversation as resolved Outdated
muellerr commented 2023-12-04 09:57:02 +01:00
Outdated
Review
Copy Link

that's a long way to walk, how long do those 7000 steps take?

that's a long way to walk, how long do those 7000 steps take?
meggert commented 2023-12-04 11:00:08 +01:00
Outdated
Review
Copy Link

0ms seems to be an okay length for this

0ms seems to be an okay length for this
igrf13.updateCoeffGH(timeOfMgmMeasurement); igrf13.updateCoeffGH(timeAbsolute);
// maybe put a condition here, to only update after a full day, this
// class function has around 700 steps to perform
igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value, igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value,
gpsDataProcessed->altitude.value, timeOfMgmMeasurement, magIgrfModel); gpsDataProcessed->altitude.value, timeAbsolute, magIgrfModel);
gpsValid = true; gpsValid = true;
} }
if (not mgm0valid and not mgm1valid and not mgm2valid and not mgm3valid and if (not mgm0valid and not mgm1valid and not mgm2valid and not mgm3valid and
@ -129,14 +131,12 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
//-----------------------Mgm Rate Computation --------------------------------------------------- //-----------------------Mgm Rate Computation ---------------------------------------------------
double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0}; double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0};
bool mgmVecTotDerivativeValid = false; bool mgmVecTotDerivativeValid = false;
double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst); if (timeDelta > 0 and VectorOperations<double>::norm(savedMgmVecTot, 3) != 0) {
if (timeOfSavedMagFieldEst.tv_sec != 0 and timeDiff > 0 and
VectorOperations<double>::norm(savedMgmVecTot, 3) != 0) {
VectorOperations<double>::subtract(mgmVecTot, savedMgmVecTot, mgmVecTotDerivative, 3); VectorOperations<double>::subtract(mgmVecTot, savedMgmVecTot, mgmVecTotDerivative, 3);
VectorOperations<double>::mulScalar(mgmVecTotDerivative, 1. / timeDiff, mgmVecTotDerivative, 3); VectorOperations<double>::mulScalar(mgmVecTotDerivative, 1. / timeDelta, mgmVecTotDerivative,
3);
mgmVecTotDerivativeValid = true; mgmVecTotDerivativeValid = true;
} }
timeOfSavedMagFieldEst = timeOfMgmMeasurement;
std::memcpy(savedMgmVecTot, mgmVecTot, sizeof(savedMgmVecTot)); std::memcpy(savedMgmVecTot, mgmVecTot, sizeof(savedMgmVecTot));
if (VectorOperations<double>::norm(mgmVecTotDerivative, 3) != 0 and if (VectorOperations<double>::norm(mgmVecTotDerivative, 3) != 0 and
@ -177,11 +177,12 @@ void SensorProcessing::processSus(
const uint16_t *sus6Value, bool sus6valid, const uint16_t *sus7Value, bool sus7valid, const uint16_t *sus6Value, bool sus6valid, const uint16_t *sus7Value, bool sus7valid,
const uint16_t *sus8Value, bool sus8valid, const uint16_t *sus9Value, bool sus9valid, const uint16_t *sus8Value, bool sus8valid, const uint16_t *sus9Value, bool sus9valid,
const uint16_t *sus10Value, bool sus10valid, const uint16_t *sus11Value, bool sus11valid, const uint16_t *sus10Value, bool sus10valid, const uint16_t *sus11Value, bool sus11valid,
timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters, timeval timeAbsolute, double timeDelta,
const AcsParameters::SusHandlingParameters *susParameters,
const AcsParameters::SunModelParameters *sunModelParameters, const AcsParameters::SunModelParameters *sunModelParameters,
acsctrl::SusDataProcessed *susDataProcessed) { acsctrl::SusDataProcessed *susDataProcessed) {
/* -------- Sun Model Direction (IJK frame) ------- */ /* -------- Sun Model Direction (IJK frame) ------- */
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfSusMeasurement); double JD2000 = MathOperations<double>::convertUnixToJD2000(timeAbsolute);
// Julean Centuries // Julean Centuries
double sunIjkModel[3] = {0.0, 0.0, 0.0}; double sunIjkModel[3] = {0.0, 0.0, 0.0};
@ -354,11 +355,10 @@ void SensorProcessing::processSus(
double susVecTotDerivative[3] = {0.0, 0.0, 0.0}; double susVecTotDerivative[3] = {0.0, 0.0, 0.0};
bool susVecTotDerivativeValid = false; bool susVecTotDerivativeValid = false;
double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst); if (timeDelta > 0 and VectorOperations<double>::norm(savedSusVecTot, 3) != 0) {
if (timeOfSavedSusDirEst.tv_sec != 0 and timeDiff > 0 and
VectorOperations<double>::norm(savedSusVecTot, 3) != 0) {
VectorOperations<double>::subtract(susVecTot, savedSusVecTot, susVecTotDerivative, 3); VectorOperations<double>::subtract(susVecTot, savedSusVecTot, susVecTotDerivative, 3);
VectorOperations<double>::mulScalar(susVecTotDerivative, 1. / timeDiff, susVecTotDerivative, 3); VectorOperations<double>::mulScalar(susVecTotDerivative, 1. / timeDelta, susVecTotDerivative,
3);
susVecTotDerivativeValid = true; susVecTotDerivativeValid = true;
} }
std::memcpy(savedSusVecTot, susVecTot, sizeof(savedSusVecTot)); std::memcpy(savedSusVecTot, susVecTot, sizeof(savedSusVecTot));
@ -367,7 +367,6 @@ void SensorProcessing::processSus(
lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value, lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value,
susParameters->susRateFilterWeight); susParameters->susRateFilterWeight);
} }
timeOfSavedSusDirEst = timeOfSusMeasurement;
{ {
PoolReadGuard pg(susDataProcessed); PoolReadGuard pg(susDataProcessed);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
@ -414,7 +413,7 @@ void SensorProcessing::processGyr(
const double gyr2axXvalue, bool gyr2axXvalid, const double gyr2axYvalue, bool gyr2axYvalid, const double gyr2axXvalue, bool gyr2axXvalid, const double gyr2axYvalue, bool gyr2axYvalid,
const double gyr2axZvalue, bool gyr2axZvalid, const double gyr3axXvalue, bool gyr3axXvalid, const double gyr2axZvalue, bool gyr2axZvalid, const double gyr3axXvalue, bool gyr3axXvalid,
const double gyr3axYvalue, bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid, const double gyr3axYvalue, bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid,
timeval timeOfGyrMeasurement, const AcsParameters::GyrHandlingParameters *gyrParameters, const AcsParameters::GyrHandlingParameters *gyrParameters,
acsctrl::GyrDataProcessed *gyrDataProcessed) { acsctrl::GyrDataProcessed *gyrDataProcessed) {
bool gyr0valid = (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid); bool gyr0valid = (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid);
bool gyr1valid = (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid); bool gyr1valid = (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid);
@ -521,7 +520,7 @@ void SensorProcessing::processGyr(
} }
void SensorProcessing::processGps(const double gpsLatitude, const double gpsLongitude, void SensorProcessing::processGps(const double gpsLatitude, const double gpsLongitude,
const double gpsAltitude, const double gpsUnixSeconds, const double gpsAltitude, const double timeDelta,
const bool validGps, const bool validGps,
const AcsParameters::GpsParameters *gpsParameters, const AcsParameters::GpsParameters *gpsParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed) { acsctrl::GpsDataProcessed *gpsDataProcessed) {
@ -563,18 +562,14 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
// Calculation of the satellite velocity in earth fixed frame // Calculation of the satellite velocity in earth fixed frame
double deltaDistance[3] = {0, 0, 0}; double deltaDistance[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE); MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE);
if (validSavedPosSatE and if (validSavedPosSatE and timeDelta < (gpsParameters->timeDiffVelocityMax) and timeDelta > 0) {
(gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax) and
(gpsUnixSeconds - timeOfSavedPosSatE) > 0) {
VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3); VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
double timeDiffGpsMeas = gpsUnixSeconds - timeOfSavedPosSatE; VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDelta, gpsVelocityE, 3);
VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDiffGpsMeas, gpsVelocityE, 3);
} }
savedPosSatE[0] = posSatE[0]; savedPosSatE[0] = posSatE[0];
savedPosSatE[1] = posSatE[1]; savedPosSatE[1] = posSatE[1];
savedPosSatE[2] = posSatE[2]; savedPosSatE[2] = posSatE[2];
timeOfSavedPosSatE = gpsUnixSeconds;
validSavedPosSatE = true; validSavedPosSatE = true;
gpsSource = acs::GpsSource::GPS; gpsSource = acs::GpsSource::GPS;
@ -594,13 +589,15 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
} }
} }
void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, void SensorProcessing::process(timeval timeAbsolute, double timeDelta,
ACS::SensorValues *sensorValues,
acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed, acsctrl::GpsDataProcessed *gpsDataProcessed,
const AcsParameters *acsParameters) { const AcsParameters *acsParameters) {
sensorValues->update(); sensorValues->update();
processGps( processGps(
sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value, sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value,
sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value, sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value,
@ -617,7 +614,8 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
sensorValues->mgm3Rm3100Set.fieldStrengths.value, sensorValues->mgm3Rm3100Set.fieldStrengths.value,
sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(), sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(),
sensorValues->imtqMgmSet.mtmRawNt.value, sensorValues->imtqMgmSet.mtmRawNt.isValid(), sensorValues->imtqMgmSet.mtmRawNt.value, sensorValues->imtqMgmSet.mtmRawNt.isValid(),
now, &acsParameters->mgmHandlingParameters, gpsDataProcessed, mgmDataProcessed); timeAbsolute, timeDelta, &acsParameters->mgmHandlingParameters, gpsDataProcessed,
mgmDataProcessed);
processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(), processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(),
sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(), sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(),
@ -631,8 +629,8 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
sensorValues->susSets[9].channels.value, sensorValues->susSets[9].channels.isValid(), sensorValues->susSets[9].channels.value, sensorValues->susSets[9].channels.isValid(),
sensorValues->susSets[10].channels.value, sensorValues->susSets[10].channels.isValid(), sensorValues->susSets[10].channels.value, sensorValues->susSets[10].channels.isValid(),
sensorValues->susSets[11].channels.value, sensorValues->susSets[11].channels.isValid(), sensorValues->susSets[11].channels.value, sensorValues->susSets[11].channels.isValid(),
now, &acsParameters->susHandlingParameters, &acsParameters->sunModelParameters, timeAbsolute, timeDelta, &acsParameters->susHandlingParameters,
susDataProcessed); &acsParameters->sunModelParameters, susDataProcessed);
processGyr( processGyr(
sensorValues->gyr0AdisSet.angVelocX.value, sensorValues->gyr0AdisSet.angVelocX.isValid(), sensorValues->gyr0AdisSet.angVelocX.value, sensorValues->gyr0AdisSet.angVelocX.isValid(),
@ -646,7 +644,7 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
sensorValues->gyr2AdisSet.angVelocZ.value, sensorValues->gyr2AdisSet.angVelocZ.isValid(), sensorValues->gyr2AdisSet.angVelocZ.value, sensorValues->gyr2AdisSet.angVelocZ.isValid(),
sensorValues->gyr3L3gSet.angVelocX.value, sensorValues->gyr3L3gSet.angVelocX.isValid(), sensorValues->gyr3L3gSet.angVelocX.value, sensorValues->gyr3L3gSet.angVelocX.isValid(),
sensorValues->gyr3L3gSet.angVelocY.value, sensorValues->gyr3L3gSet.angVelocY.isValid(), sensorValues->gyr3L3gSet.angVelocY.value, sensorValues->gyr3L3gSet.angVelocY.isValid(),
sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(), now, sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(),
&acsParameters->gyrHandlingParameters, gyrDataProcessed); &acsParameters->gyrHandlingParameters, gyrDataProcessed);
} }

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

@ -24,22 +24,21 @@ class SensorProcessing {
SensorProcessing(); SensorProcessing();
virtual ~SensorProcessing(); virtual ~SensorProcessing();
void process(timeval now, ACS::SensorValues *sensorValues, void process(timeval timeAbsolute, double timeDelta, ACS::SensorValues *sensorValues,
acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed, acsctrl::GpsDataProcessed *gpsDataProcessed, const AcsParameters *acsParameters);
const AcsParameters *acsParameters); // Will call protected functions
private: private:
static constexpr float ZERO_VEC_F[3] = {0, 0, 0}; static constexpr float ZERO_VEC_F[3] = {0, 0, 0};
static constexpr double ZERO_VEC_D[3] = {0, 0, 0}; static constexpr double ZERO_VEC_D[3] = {0, 0, 0};
static constexpr double ECCENTRICITY_WGS84 = 0.0818195; static constexpr double ECCENTRICITY_WGS84 = 0.0818195;
protected: protected:
// short description needed for every function
void processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid, void processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid,
const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid, const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid,
const float *mgm4Value, bool mgm4valid, timeval timeOfMgmMeasurement, const float *mgm4Value, bool mgm4valid, timeval timeAbsolute, double timeDelta,
const AcsParameters::MgmHandlingParameters *mgmParameters, const AcsParameters::MgmHandlingParameters *mgmParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed, acsctrl::GpsDataProcessed *gpsDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed); acsctrl::MgmDataProcessed *mgmDataProcessed);
@ -52,7 +51,7 @@ class SensorProcessing {
bool sus7valid, const uint16_t *sus8Value, bool sus8valid, bool sus7valid, const uint16_t *sus8Value, bool sus8valid,
const uint16_t *sus9Value, bool sus9valid, const uint16_t *sus10Value, const uint16_t *sus9Value, bool sus9valid, const uint16_t *sus10Value,
bool sus10valid, const uint16_t *sus11Value, bool sus11valid, bool sus10valid, const uint16_t *sus11Value, bool sus11valid,
timeval timeOfSusMeasurement, timeval timeAbsolute, double timeDelta,
const AcsParameters::SusHandlingParameters *susParameters, const AcsParameters::SusHandlingParameters *susParameters,
const AcsParameters::SunModelParameters *sunModelParameters, const AcsParameters::SunModelParameters *sunModelParameters,
acsctrl::SusDataProcessed *susDataProcessed); acsctrl::SusDataProcessed *susDataProcessed);
@ -65,7 +64,6 @@ class SensorProcessing {
bool gyr2axYvalid, const double gyr2axZvalue, bool gyr2axZvalid, bool gyr2axYvalid, const double gyr2axZvalue, bool gyr2axZvalid,
const double gyr3axXvalue, bool gyr3axXvalid, const double gyr3axYvalue, const double gyr3axXvalue, bool gyr3axXvalid, const double gyr3axYvalue,
bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid, bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid,
timeval timeOfGyrMeasurement,
const AcsParameters::GyrHandlingParameters *gyrParameters, const AcsParameters::GyrHandlingParameters *gyrParameters,
acsctrl::GyrDataProcessed *gyrDataProcessed); acsctrl::GyrDataProcessed *gyrDataProcessed);
@ -77,13 +75,9 @@ class SensorProcessing {
void lowPassFilter(double *newValue, double *oldValue, const double weight); void lowPassFilter(double *newValue, double *oldValue, const double weight);
double savedMgmVecTot[3] = {0.0, 0.0, 0.0}; double savedMgmVecTot[3] = {0.0, 0.0, 0.0};
timeval timeOfSavedMagFieldEst;
double savedSusVecTot[3] = {0.0, 0.0, 0.0}; double savedSusVecTot[3] = {0.0, 0.0, 0.0};
timeval timeOfSavedSusDirEst;
bool validMagField = false;
double savedPosSatE[3] = {0.0, 0.0, 0.0}; double savedPosSatE[3] = {0.0, 0.0, 0.0};
double timeOfSavedPosSatE = 0.0;
bool validSavedPosSatE = false; bool validSavedPosSatE = false;
SusConverter susConverter; SusConverter susConverter;