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 +58 -59
6 changed files with 58 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 17c253d19b - Show all commits

29
mission/controller/AcsController.cpp
View File

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

5
mission/controller/AcsController.h
View File

@ -50,6 +50,11 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
bool enableHkSets = false;
timeval timeAbsolute;
timeval timeRelative;
double timeDelta = 0.0;
timeval oldTimeRelative;
AcsParameters acsParameters;
SensorProcessing sensorProcessing;
FusedRotationEstimation fusedRotationEstimation;

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

@ -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,7 +300,7 @@ 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],
@ -362,7 +364,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 +374,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 +410,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 +451,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) {
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 +489,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 double timeDelta, 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 double timeDelta, 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};

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

@ -588,7 +588,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
}
}
void SensorProcessing::process(timeval timeAbsolute, timeval timeRelative,
void SensorProcessing::process(timeval timeAbsolute, double timeDelta,
ACS::SensorValues *sensorValues,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed,
@ -596,11 +596,6 @@ void SensorProcessing::process(timeval timeAbsolute, timeval timeRelative,
acsctrl::GpsDataProcessed *gpsDataProcessed,
const AcsParameters *acsParameters) {
sensorValues->update();
double timeDelta = 0;
if (timeRelative.tv_sec != 0 and savedTimeRelative.tv_sec != 0) {
timeDelta = timevalOperations::toDouble(timeRelative - savedTimeRelative);
}
savedTimeRelative = timeRelative;
processGps(
sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value,

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

@ -24,7 +24,7 @@ class SensorProcessing {
SensorProcessing();
virtual ~SensorProcessing();
void process(timeval timeAbsolute, timeval timeRelative, ACS::SensorValues *sensorValues,
void process(timeval timeAbsolute, double timeDelta, ACS::SensorValues *sensorValues,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed,
@ -74,7 +74,6 @@ class SensorProcessing {
void lowPassFilter(double *newValue, double *oldValue, const double weight);
timeval savedTimeRelative;
double savedMgmVecTot[3] = {0.0, 0.0, 0.0};
double savedSusVecTot[3] = {0.0, 0.0, 0.0};