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
though it is there.
- 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

206
mission/controller/AcsController.cpp
View File

@ -141,25 +141,7 @@ void AcsController::performControlOperation() {
}
case InternalState::READY: {
if (mode != MODE_OFF) {
switch (mode) {
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;
}
performAttitudeControl();
}
break;
}
@ -168,39 +150,96 @@ void AcsController::performControlOperation() {
}
}
void AcsController::performSafe() {
timeval now;
Clock::getClock_timeval(&now);
void AcsController::performAttitudeControl() {
Clock::getClock_timeval(&timeAbsolute);
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) {
triggerEvent(acs::TLE_TOO_OLD);
tleTooOldFlag = true;
} else if (result != Sgp4Propagator::TLE_TOO_OLD) {
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,
&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;
}
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
double sunTargetDir[3] = {0, 0, 0};
guidance.getTargetParamsSafe(sunTargetDir);
@ -301,37 +340,6 @@ void AcsController::performSafe() {
}
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(
mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
mgmDataProcessed.mgmVecTotDerivative.isValid(),
@ -394,51 +402,9 @@ void AcsController::performDetumble() {
}
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;
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 (multipleRwUnavailableCounter >=
acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) {
@ -460,7 +426,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);
@ -481,7 +447,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,
@ -505,7 +471,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);
@ -526,9 +492,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,

6
mission/controller/AcsController.h
View File

@ -45,6 +45,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
uint16_t startAtIndex) override;
protected:
void performAttitudeControl();
void performSafe();
void performDetumble();
void performPointingCtrl();
@ -59,6 +60,11 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
SdCardMountedIF& sdcMan;
timeval timeAbsolute;
timeval timeRelative;
double timeDelta = 0.0;
timeval oldTimeRelative;
AcsParameters acsParameters;
SensorProcessing sensorProcessing;
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() {
// 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
ReturnValue_t result = updateEpsData();
@ -173,8 +178,6 @@ void PowerController::calculateStateOfCharge() {
pwrCtrlCoreHk.setValidity(false, true);
}
}
// store time for next run
oldTime = now;
return;
}
@ -195,12 +198,10 @@ void PowerController::calculateStateOfCharge() {
pwrCtrlCoreHk.coulombCounterCharge.setValid(false);
}
}
// store time for next run
oldTime = now;
return;
}
result = calculateCoulombCounterCharge();
result = calculateCoulombCounterCharge(timeDelta);
if (result != returnvalue::OK) {
// notifying events have already been triggered
{
@ -215,9 +216,6 @@ void PowerController::calculateStateOfCharge() {
pwrCtrlCoreHk.coulombCounterCharge.setValid(false);
}
}
// store time for next run
oldTime = now;
return;
}
// commit to dataset
@ -231,8 +229,6 @@ void PowerController::calculateStateOfCharge() {
pwrCtrlCoreHk.setValidity(true, true);
}
}
// store time for next run
oldTime = now;
}
void PowerController::watchStateOfCharge() {
@ -285,12 +281,14 @@ ReturnValue_t PowerController::calculateOpenCircuitVoltageCharge() {
return returnvalue::OK;
}
ReturnValue_t PowerController::calculateCoulombCounterCharge() {
double timeDiff = timevalOperations::toDouble(now - oldTime);
if (timeDiff > maxAllowedTimeDiff) {
ReturnValue_t PowerController::calculateCoulombCounterCharge(double timeDelta) {
if (timeDelta == 0.0) {
return returnvalue::FAILED;
}
if (timeDelta > maxAllowedTimeDiff) {
// should not be a permanent state so no spam protection required
triggerEvent(power::TIMEDELTA_OUT_OF_BOUNDS, static_cast<uint32_t>(timeDiff * 10));
sif::error << "Power Controller::Time delta too large for Coulomb Counter: " << timeDiff
triggerEvent(power::TIMEDELTA_OUT_OF_BOUNDS, static_cast<uint32_t>(timeDelta * 10));
sif::error << "Power Controller::Time delta too large for Coulomb Counter: " << timeDelta
<< std::endl;
return returnvalue::FAILED;
}
@ -298,7 +296,7 @@ ReturnValue_t PowerController::calculateCoulombCounterCharge() {
coulombCounterCharge = openCircuitVoltageCharge;
} else {
coulombCounterCharge =
coulombCounterCharge + iBat * CONVERT_FROM_MILLI * timeDiff * SECONDS_TO_HOURS;
coulombCounterCharge + iBat * CONVERT_FROM_MILLI * timeDelta * SECONDS_TO_HOURS;
if (coulombCounterCharge >= coulombCounterChargeUpperThreshold) {
coulombCounterCharge = coulombCounterChargeUpperThreshold;
}

2
mission/controller/PowerController.h
View File

@ -45,7 +45,7 @@ class PowerController : public ExtendedControllerBase, public ReceivesParameterM
void calculateStateOfCharge();
void watchStateOfCharge();
ReturnValue_t calculateOpenCircuitVoltageCharge();
ReturnValue_t calculateCoulombCounterCharge();
ReturnValue_t calculateCoulombCounterCharge(double timeDelta);
ReturnValue_t updateEpsData();
float charge2stateOfCharge(float capacity, bool coulombCounter);
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.037072, 0.006812, 1.313158}};
float 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)};
float mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)};
double mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)};
double mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)};
double mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)};
float mgmVectorFilterWeight = 0.85;
float mgmDerivativeFilterWeight = 0.99;
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
* 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(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;
float gyrFilterWeight = 0.6;
} gyrHandlingParameters;

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

@ -4,21 +4,21 @@
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h>
#include <mission/controller/acs/util/MathOperations.h>
#include <cmath>
#include <filesystem>
#include "string.h"
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
#include <string>
Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
Guidance::~Guidance() {}
void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3],
double sunDirI[3], double refDirB[3], double quatBI[4],
double targetQuat[4], double targetSatRotRate[3]) {
[[deprecated]] void Guidance::targetQuatPtgSingleAxis(const timeval timeAbsolute, double posSatE[3],
double velSatE[3], double sunDirI[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
//-------------------------------------------------------------------------------------
@ -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 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>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
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);
}
void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3],
double targetQuat[4], double targetSatRotRate[3]) {
void Guidance::targetQuatPtgThreeAxes(const timeval timeAbsolute, const double timeDelta,
double posSatE[3], double velSatE[3], double targetQuat[4],
double targetSatRotRate[3]) {
//-------------------------------------------------------------------------------------
// 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 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>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
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);
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],
double targetQuat[4], double targetSatRotRate[3]) {
void Guidance::targetQuatPtgGs(const timeval timeAbsolute, const double timeDelta,
double posSatE[3], double sunDirI[3], double targetQuat[4],
double targetSatRotRate[3]) {
//-------------------------------------------------------------------------------------
// 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 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>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
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);
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]) {
//-------------------------------------------------------------------------------------
// 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
//----------------------------------------------------------------------------
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],
double targetQuat[4], double refDirB[3],
double refSatRate[3]) {
[[deprecated]] void Guidance::targetQuatPtgNadirSingleAxis(const timeval timeAbsolute,
double posSatE[3], double quatBI[4],
double targetQuat[4], double refDirB[3],
double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// 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 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>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
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);
}
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]) {
//-------------------------------------------------------------------------------------
// 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 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>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
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);
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],
@ -448,23 +452,21 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
VectorOperations<double>::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3);
}
void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
double *refSatRate) {
void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double timeDelta,
double quatInertialTarget[4], double *refSatRate) {
//-------------------------------------------------------------------------------------
// 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) {
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};
QuaternionOperations::inverse(quatInertialTarget, q);
QuaternionOperations::inverse(savedQuaternion, qS);
double qDiff[4] = {0, 0, 0, 0};
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 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;
}
timeSavedQuaternion = now;
savedQuaternion[0] = quatInertialTarget[0];
savedQuaternion[1] = quatInertialTarget[1];
savedQuaternion[2] = quatInertialTarget[2];
savedQuaternion[3] = quatInertialTarget[3];
std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
}
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
// position of the ground station
void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3],
double refDirB[3], double quatBI[4], double targetQuat[4],
double targetSatRotRate[3]);
void targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double quatIX[4],
double targetSatRotRate[3]);
void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4],
double targetSatRotRate[3]);
void targetQuatPtgSingleAxis(const timeval timeAbsolute, double posSatE[3], double velSatE[3],
double sunDirI[3], double refDirB[3], double quatBI[4],
double targetQuat[4], double targetSatRotRate[3]);
void targetQuatPtgThreeAxes(const timeval timeAbsolute, const double timeDelta, double posSatE[3],
double velSatE[3], double quatIX[4], double targetSatRotRate[3]);
void targetQuatPtgGs(const timeval timeAbsolute, const double timeDelta, double posSatE[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
// 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]);
// Function to get the target quaternion and refence rotation rate from gps position for Nadir
// 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]);
void targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3],
double targetQuat[4], double refSatRate[3]);
void targetQuatPtgNadirThreeAxes(const timeval timeAbsolute, const double timeDelta,
double posSatE[3], double velSatE[3], double targetQuat[4],
double refSatRate[3]);
// @note: Calculates the error quaternion between the current orientation and the target
// 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 &errorAngle);
void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
double *targetSatRotRate);
void targetRotationRate(const int8_t timeElapsedMax, const double timeDelta,
double quatInertialTarget[4], double *targetSatRotRate);
// @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
// reation wheel maybe can be done in "commanding.h"
@ -59,7 +60,6 @@ class Guidance {
const AcsParameters *acsParameters;
bool strBlindAvoidFlag = false;
timeval timeSavedQuaternion;
double savedQuaternion[4] = {0, 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 <fsfw/timemanager/Stopwatch.h>
SensorProcessing::SensorProcessing() {}
SensorProcessing::~SensorProcessing() {}
@ -7,7 +9,7 @@ SensorProcessing::~SensorProcessing() {}
void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value,
bool mgm1valid, const float *mgm2Value, bool mgm2valid,
const float *mgm3Value, bool mgm3valid, const float *mgm4Value,
bool mgm4valid, timeval timeOfMgmMeasurement,
bool mgm4valid, timeval timeAbsolute, double timeDelta,
const AcsParameters::MgmHandlingParameters *mgmParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed,
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};
bool gpsValid = false;
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;
igrf13.schmidtNormalization();
igrf13.updateCoeffGH(timeOfMgmMeasurement);
// maybe put a condition here, to only update after a full day, this
// class function has around 700 steps to perform
igrf13.updateCoeffGH(timeAbsolute);
igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value,
gpsDataProcessed->altitude.value, timeOfMgmMeasurement, magIgrfModel);
gpsDataProcessed->altitude.value, timeAbsolute, magIgrfModel);
gpsValid = true;
}
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 ---------------------------------------------------
double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0};
bool mgmVecTotDerivativeValid = false;
double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst);
if (timeOfSavedMagFieldEst.tv_sec != 0 and timeDiff > 0 and
VectorOperations<double>::norm(savedMgmVecTot, 3) != 0) {
if (timeDelta > 0 and VectorOperations<double>::norm(savedMgmVecTot, 3) != 0) {
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;
}
timeOfSavedMagFieldEst = timeOfMgmMeasurement;
std::memcpy(savedMgmVecTot, mgmVecTot, sizeof(savedMgmVecTot));
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 *sus8Value, bool sus8valid, const uint16_t *sus9Value, bool sus9valid,
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,
acsctrl::SusDataProcessed *susDataProcessed) {
/* -------- Sun Model Direction (IJK frame) ------- */
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfSusMeasurement);
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeAbsolute);
// Julean Centuries
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};
bool susVecTotDerivativeValid = false;
double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst);
if (timeOfSavedSusDirEst.tv_sec != 0 and timeDiff > 0 and
VectorOperations<double>::norm(savedSusVecTot, 3) != 0) {
if (timeDelta > 0 and VectorOperations<double>::norm(savedSusVecTot, 3) != 0) {
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;
}
std::memcpy(savedSusVecTot, susVecTot, sizeof(savedSusVecTot));
@ -367,7 +367,6 @@ void SensorProcessing::processSus(
lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value,
susParameters->susRateFilterWeight);
}
timeOfSavedSusDirEst = timeOfSusMeasurement;
{
PoolReadGuard pg(susDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
@ -414,7 +413,7 @@ void SensorProcessing::processGyr(
const double gyr2axXvalue, bool gyr2axXvalid, const double gyr2axYvalue, bool gyr2axYvalid,
const double gyr2axZvalue, bool gyr2axZvalid, const double gyr3axXvalue, bool gyr3axXvalid,
const double gyr3axYvalue, bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid,
timeval timeOfGyrMeasurement, const AcsParameters::GyrHandlingParameters *gyrParameters,
const AcsParameters::GyrHandlingParameters *gyrParameters,
acsctrl::GyrDataProcessed *gyrDataProcessed) {
bool gyr0valid = (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid);
bool gyr1valid = (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid);
@ -521,7 +520,7 @@ void SensorProcessing::processGyr(
}
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 AcsParameters::GpsParameters *gpsParameters,
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
double deltaDistance[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE);
if (validSavedPosSatE and
(gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax) and
(gpsUnixSeconds - timeOfSavedPosSatE) > 0) {
if (validSavedPosSatE and timeDelta < (gpsParameters->timeDiffVelocityMax) and timeDelta > 0) {
VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
double timeDiffGpsMeas = gpsUnixSeconds - timeOfSavedPosSatE;
VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDiffGpsMeas, gpsVelocityE, 3);
VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDelta, gpsVelocityE, 3);
}
savedPosSatE[0] = posSatE[0];
savedPosSatE[1] = posSatE[1];
savedPosSatE[2] = posSatE[2];
timeOfSavedPosSatE = gpsUnixSeconds;
validSavedPosSatE = true;
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::SusDataProcessed *susDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed,
const AcsParameters *acsParameters) {
sensorValues->update();
processGps(
sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.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.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(),
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[10].channels.value, sensorValues->susSets[10].channels.isValid(),
sensorValues->susSets[11].channels.value, sensorValues->susSets[11].channels.isValid(),
now, &acsParameters->susHandlingParameters, &acsParameters->sunModelParameters,
susDataProcessed);
timeAbsolute, timeDelta, &acsParameters->susHandlingParameters,
&acsParameters->sunModelParameters, susDataProcessed);
processGyr(
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->gyr3L3gSet.angVelocX.value, sensorValues->gyr3L3gSet.angVelocX.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);
}

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

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