From 8e41885ca182bcddcef7747556221195e6e175a5 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 23 Aug 2023 15:21:40 +0200 Subject: [PATCH 01/68] tle gets stored presistent now --- mission/controller/AcsController.cpp | 76 +++++++++++++++---- mission/controller/AcsController.h | 15 ++-- .../AcsCtrlDefinitions.h | 14 ---- 3 files changed, 71 insertions(+), 34 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 1350bcf0..f0c1f3e0 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -22,8 +22,7 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets) mekfData(this), ctrlValData(this), actuatorCmdData(this), - fusedRotRateData(this), - tleData(this) {} + fusedRotRateData(this) {} ReturnValue_t AcsController::initialize() { ReturnValue_t result = parameterHelper.initialize(); @@ -67,19 +66,13 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t if (size != 69 * 2) { return INVALID_PARAMETERS; } - ReturnValue_t result = navigation.updateTle(data, data + 69); + ReturnValue_t result = updateTle(data, data + 69, false); if (result != returnvalue::OK) { - PoolReadGuard pg(&tleData); - navigation.updateTle(tleData.line1.value, tleData.line2.value); return result; } - { - PoolReadGuard pg(&tleData); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(tleData.line1.value, data, 69); - std::memcpy(tleData.line2.value, data + 69, 69); - tleData.setValidity(true, true); - } + result = writeTleToFs(data); + if (result != returnvalue::OK) { + return result; } return HasActionsIF::EXECUTION_FINISHED; } @@ -130,6 +123,10 @@ void AcsController::performControlOperation() { } case InternalState::INITIAL_DELAY: { if (initialCountdown.hasTimedOut()) { + uint8_t line1[69] = {}; + uint8_t line2[69] = {}; + readTleFromFs(line1, line2); + updateTle(line1, line2, true); internalState = InternalState::READY; } return; @@ -801,9 +798,6 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel); localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL, &rotRateTotal); poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0}); - // TLE Data - localDataPoolMap.emplace(acsctrl::PoolIds::TLE_LINE_1, &line1); - localDataPoolMap.emplace(acsctrl::PoolIds::TLE_LINE_2, &line2); return returnvalue::OK; } @@ -1031,6 +1025,58 @@ void AcsController::copySusData() { } } +ReturnValue_t AcsController::updateTle(const uint8_t *line1, const uint8_t *line2, bool fromFile) { + ReturnValue_t result = navigation.updateTle(line1, line2); + if (result != returnvalue::OK) { + if (not fromFile) { + uint8_t fileLine1[69] = {}; + uint8_t fileLine2[69] = {}; + readTleFromFs(fileLine1, fileLine2); + navigation.updateTle(fileLine1, fileLine2); + } + return result; + } + return returnvalue::OK; +} + +ReturnValue_t AcsController::writeTleToFs(const uint8_t *tle) { + // split TLE to two lines + // not sure why these need to be 70 long + uint8_t tleLine1[70] = {}; + uint8_t tleLine2[70] = {}; + std::memcpy(tleLine1, tle, 69); + std::memcpy(tleLine2, tle + 69, 69); + // ToDo: check which SD card is active via sdcMan + std::string path = SD_0 + TLE_FILE; + // Clear existing TLE from file + std::ofstream tleFile(path.c_str(), std::ofstream::out | std::ofstream::trunc); + if (tleFile.is_open()) { + tleFile << tleLine1 << "\n" << tleLine2 << "\n"; + } else { + // return error + } + tleFile.close(); + return returnvalue::OK; +} + +ReturnValue_t AcsController::readTleFromFs(uint8_t *line1, uint8_t *line2) { + // ToDo: check which SD card is active via sdcMan + std::string path = SD_0 + TLE_FILE; + // Read existing TLE from file + std::fstream tleFile = std::fstream(path.c_str(), std::fstream::in); + if (tleFile.is_open()) { + std::string tleLine1, tleLine2; + getline(tleFile, tleLine1); + std::memcpy(line1, tleLine1.c_str(), 69); + getline(tleFile, tleLine2); + std::memcpy(line2, tleLine2.c_str(), 69); + } else { + // return error + } + tleFile.close(); + return returnvalue::OK; +} + void AcsController::copyGyrData() { { PoolReadGuard pg(&sensorValues.gyr0AdisSet); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index e03f2a55..9d91ae10 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -25,6 +25,8 @@ #include #include +#include + class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF { public: static constexpr dur_millis_t INIT_DELAY = 500; @@ -125,6 +127,14 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng, const double* tgtRotRate); + ReturnValue_t updateTle(const uint8_t* line1, const uint8_t* line2, bool fromFile); + ReturnValue_t writeTleToFs(const uint8_t* tle); + ReturnValue_t readTleFromFs(uint8_t* line1, uint8_t* line2); + + const std::string SD_0 = "/mnt/sd0/"; + const std::string SD_1 = "/mnt/sd1/"; + const std::string TLE_FILE = "conf/tle.txt"; + /* ACS Sensor Values */ ACS::SensorValues sensorValues; @@ -238,11 +248,6 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes PoolEntry rotRateParallel = PoolEntry(3); PoolEntry rotRateTotal = PoolEntry(3); - // TLE Dataset - acsctrl::TleData tleData; - PoolEntry line1 = PoolEntry(69); - PoolEntry line2 = PoolEntry(69); - // Initial delay to make sure all pool variables have been initialized their owners Countdown initialCountdown = Countdown(INIT_DELAY); }; diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index b843ca0c..ac348cf9 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -110,9 +110,6 @@ enum PoolIds : lp_id_t { ROT_RATE_ORTHOGONAL, ROT_RATE_PARALLEL, ROT_RATE_TOTAL, - // TLE - TLE_LINE_1, - TLE_LINE_2, }; static constexpr uint8_t MGM_SET_RAW_ENTRIES = 6; @@ -126,7 +123,6 @@ static constexpr uint8_t MEKF_SET_ENTRIES = 3; static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 3; -static constexpr uint8_t TLE_SET_ENTRIES = 2; /** * @brief Raw MGM sensor data. Includes the IMTQ sensor data and actuator status. @@ -299,16 +295,6 @@ class FusedRotRateData : public StaticLocalDataSet { private: }; -class TleData : public StaticLocalDataSet { - public: - TleData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, TLE_SET) {} - - lp_vec_t line1 = lp_vec_t(sid.objectId, TLE_LINE_1, this); - lp_vec_t line2 = lp_vec_t(sid.objectId, TLE_LINE_1, this); - - private: -}; - } // namespace acsctrl #endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ */ From e40dd74f396b41590563a83af4ab9af1e58f9e5c Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 14 Sep 2023 13:54:05 +0200 Subject: [PATCH 02/68] lets see if this robin guy knows what he is talking about --- mission/controller/AcsController.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index f0c1f3e0..c78ed47a 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -1040,18 +1040,14 @@ ReturnValue_t AcsController::updateTle(const uint8_t *line1, const uint8_t *line } ReturnValue_t AcsController::writeTleToFs(const uint8_t *tle) { - // split TLE to two lines - // not sure why these need to be 70 long - uint8_t tleLine1[70] = {}; - uint8_t tleLine2[70] = {}; - std::memcpy(tleLine1, tle, 69); - std::memcpy(tleLine2, tle + 69, 69); // ToDo: check which SD card is active via sdcMan std::string path = SD_0 + TLE_FILE; // Clear existing TLE from file std::ofstream tleFile(path.c_str(), std::ofstream::out | std::ofstream::trunc); if (tleFile.is_open()) { - tleFile << tleLine1 << "\n" << tleLine2 << "\n"; + tleFile.write(static_cast(tle), 69); + tleFile << "\n"; + tleFile.write(static_cast(tle + 69), 69); } else { // return error } From 1d51bfba3d203f3b34e7f1896d340729028bafe6 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 15 Sep 2023 13:34:16 +0200 Subject: [PATCH 03/68] boop --- mission/controller/AcsController.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index c78ed47a..de226515 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -1045,9 +1045,9 @@ ReturnValue_t AcsController::writeTleToFs(const uint8_t *tle) { // Clear existing TLE from file std::ofstream tleFile(path.c_str(), std::ofstream::out | std::ofstream::trunc); if (tleFile.is_open()) { - tleFile.write(static_cast(tle), 69); + tleFile.write(reinterpret_cast(tle), 69); tleFile << "\n"; - tleFile.write(static_cast(tle + 69), 69); + tleFile.write(reinterpret_cast(tle + 69), 69); } else { // return error } From 740167ce99d53e35102cfb9a29caedc48ea0fb0c Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 11 Oct 2023 14:30:57 +0200 Subject: [PATCH 04/68] monotonic for pwr ctrl --- mission/controller/PowerController.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/mission/controller/PowerController.cpp b/mission/controller/PowerController.cpp index 7b7db9e8..202e9906 100644 --- a/mission/controller/PowerController.cpp +++ b/mission/controller/PowerController.cpp @@ -156,7 +156,7 @@ ReturnValue_t PowerController::checkModeCommand(Mode_t mode, Submode_t submode, void PowerController::calculateStateOfCharge() { // get time - Clock::getClock_timeval(&now); + Clock::getClockMonotonic(&now); // update EPS HK values ReturnValue_t result = updateEpsData(); @@ -285,7 +285,12 @@ ReturnValue_t PowerController::calculateOpenCircuitVoltageCharge() { } ReturnValue_t PowerController::calculateCoulombCounterCharge() { - double timeDiff = timevalOperations::toDouble(now - oldTime); + double timeDiff = 0.0; + if (timevalOperations::toDouble(oldTime) != 0) { + timeDiff = timevalOperations::toDouble(now - oldTime); + } else { + return returnvalue::FAILED; + } if (timeDiff > maxAllowedTimeDiff) { // should not be a permanent state so no spam protection required triggerEvent(power::TIMEDELTA_OUT_OF_BOUNDS, static_cast(timeDiff * 10)); From f61da1002f8354020789159363bbf34a509435d5 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 16 Oct 2023 11:38:07 +0200 Subject: [PATCH 05/68] breaking the ACS controller only for Robin --- mission/controller/AcsController.cpp | 20 +++++++ mission/controller/acs/SensorProcessing.cpp | 58 +++++++++++---------- mission/controller/acs/SensorProcessing.h | 17 +++--- 3 files changed, 56 insertions(+), 39 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 1350bcf0..1aac02c2 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -122,6 +122,26 @@ void AcsController::performControlOperation() { } } + timeval timeAbsolute; + Clock::getClock_timeval(&timeAbsolute); + timeval timeRelative; + Clock::getClockMonotonic(&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); + fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed, + &gyrDataProcessed, &fusedRotRateData); + result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, + &susDataProcessed, &mekfData, &acsParameters); + switch (internalState) { case InternalState::STARTUP: { initialCountdown.resetTimer(); diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 4405790c..c0d2a128 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -7,7 +7,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) { @@ -18,11 +18,12 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const if (gpsDataProcessed->source.value != acs::GpsSource::NONE) { Igrf13Model igrf13; igrf13.schmidtNormalization(); - igrf13.updateCoeffGH(timeOfMgmMeasurement); + igrf13.updateCoeffGH(timeAbsolute); + // ToDo // 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, - 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 +130,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::norm(savedMgmVecTot, 3) != 0) { + if (timeDelta > 0 and VectorOperations::norm(savedMgmVecTot, 3) != 0) { VectorOperations::subtract(mgmVecTot, savedMgmVecTot, mgmVecTotDerivative, 3); - VectorOperations::mulScalar(mgmVecTotDerivative, 1. / timeDiff, mgmVecTotDerivative, 3); + VectorOperations::mulScalar(mgmVecTotDerivative, 1. / timeDelta, mgmVecTotDerivative, + 3); mgmVecTotDerivativeValid = true; } - timeOfSavedMagFieldEst = timeOfMgmMeasurement; std::memcpy(savedMgmVecTot, mgmVecTot, sizeof(savedMgmVecTot)); if (VectorOperations::norm(mgmVecTotDerivative, 3) != 0 and @@ -177,11 +176,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::convertUnixToJD2000(timeOfSusMeasurement); + double JD2000 = MathOperations::convertUnixToJD2000(timeAbsolute); // Julean Centuries double sunIjkModel[3] = {0.0, 0.0, 0.0}; @@ -354,11 +354,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::norm(savedSusVecTot, 3) != 0) { + if (timeDelta > 0 and VectorOperations::norm(savedSusVecTot, 3) != 0) { VectorOperations::subtract(susVecTot, savedSusVecTot, susVecTotDerivative, 3); - VectorOperations::mulScalar(susVecTotDerivative, 1. / timeDiff, susVecTotDerivative, 3); + VectorOperations::mulScalar(susVecTotDerivative, 1. / timeDelta, susVecTotDerivative, + 3); susVecTotDerivativeValid = true; } std::memcpy(savedSusVecTot, susVecTot, sizeof(savedSusVecTot)); @@ -367,7 +366,6 @@ void SensorProcessing::processSus( lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value, susParameters->susRateFilterWeight); } - timeOfSavedSusDirEst = timeOfSusMeasurement; { PoolReadGuard pg(susDataProcessed); if (pg.getReadResult() == returnvalue::OK) { @@ -414,7 +412,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 +519,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 +561,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::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::subtract(posSatE, savedPosSatE, deltaDistance, 3); - double timeDiffGpsMeas = gpsUnixSeconds - timeOfSavedPosSatE; - VectorOperations::mulScalar(deltaDistance, 1. / timeDiffGpsMeas, gpsVelocityE, 3); + VectorOperations::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 +588,20 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong } } -void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, +void SensorProcessing::process(timeval timeAbsolute, timeval timeRelative, + ACS::SensorValues *sensorValues, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::SusDataProcessed *susDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, 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, sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value, @@ -617,7 +618,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 +633,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 +648,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); } diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 77ff2869..70b78845 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -24,22 +24,21 @@ class SensorProcessing { SensorProcessing(); virtual ~SensorProcessing(); - void process(timeval now, ACS::SensorValues *sensorValues, + void process(timeval timeAbsolute, timeval timeRelative, 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); @@ -76,14 +74,11 @@ class SensorProcessing { void lowPassFilter(double *newValue, double *oldValue, const double weight); + timeval savedTimeRelative; 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; From 25354ee7b43915df253e536c40c766a0a4e8bcde Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 16 Oct 2023 11:50:31 +0200 Subject: [PATCH 06/68] i hope i get a medal from Robin for this --- mission/controller/AcsController.cpp | 211 ++++++++++----------------- mission/controller/AcsController.h | 1 + 2 files changed, 78 insertions(+), 134 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 1aac02c2..14a2d83c 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -122,26 +122,6 @@ void AcsController::performControlOperation() { } } - timeval timeAbsolute; - Clock::getClock_timeval(&timeAbsolute); - timeval timeRelative; - Clock::getClockMonotonic(&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); - fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed, - &gyrDataProcessed, &fusedRotRateData); - result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, - &susDataProcessed, &mekfData, &acsParameters); - switch (internalState) { case InternalState::STARTUP: { initialCountdown.resetTimer(); @@ -156,25 +136,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; } @@ -183,39 +145,93 @@ void AcsController::performControlOperation() { } } -void AcsController::performSafe() { - timeval now; - Clock::getClock_timeval(&now); +void AcsController::performAttitudeControl() { + timeval timeAbsolute; + Clock::getClock_timeval(&timeAbsolute); + timeval timeRelative; + Clock::getClockMonotonic(&timeRelative); - ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed); + 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, timeRelative, &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 && + 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 (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 && + 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; + } + 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); @@ -316,37 +332,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(), @@ -409,51 +394,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) { diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index e03f2a55..89919ccc 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -37,6 +37,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes uint16_t startAtIndex) override; protected: + void performAttitudeControl(); void performSafe(); void performDetumble(); void performPointingCtrl(); From 17c253d19b3ec20593ebdefe06aeeaa0b2dbddf0 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 16 Oct 2023 13:26:56 +0200 Subject: [PATCH 07/68] this might just work --- mission/controller/AcsController.cpp | 29 +++++++------ mission/controller/AcsController.h | 5 +++ mission/controller/acs/Guidance.cpp | 45 ++++++++++----------- mission/controller/acs/Guidance.h | 28 ++++++------- mission/controller/acs/SensorProcessing.cpp | 7 +--- mission/controller/acs/SensorProcessing.h | 3 +- 6 files changed, 58 insertions(+), 59 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 14a2d83c..89853b9f 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -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, diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 89919ccc..1b184b80 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -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; diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 1d82d317..b483372d 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -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::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); + MathOperations::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot); MathOperations::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::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); + MathOperations::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot); MathOperations::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::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); + MathOperations::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot); MathOperations::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::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::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::subtract(q, qS, qDiff, 4); - VectorOperations::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4); + VectorOperations::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, diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 7b81e411..371ae147 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -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}; diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index c0d2a128..56217a8f 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -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, diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 70b78845..fea0fd01 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -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}; From 823aa709542bd202b83ceab0f84894795f3ec645 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 16 Oct 2023 17:01:42 +0200 Subject: [PATCH 08/68] fixed deprecated functions --- mission/controller/acs/Guidance.cpp | 19 +++++++++++-------- mission/controller/acs/Guidance.h | 4 ++-- 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index b483372d..6067676e 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -16,9 +16,11 @@ Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameter 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 +40,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::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); + MathOperations::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot); MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -303,9 +305,10 @@ void Guidance::targetQuatPtgSun(double timeDelta, double sunDirI[3], double targ 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 //------------------------------------------------------------------------------------- @@ -316,7 +319,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::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); + MathOperations::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot); MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 371ae147..1cf5b8a9 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -17,7 +17,7 @@ class Guidance { // Function to get the target quaternion and reference rotation rate from gps position and // position of the ground station - void targetQuatPtgSingleAxis(const double timeDelta, double posSatE[3], double velSatE[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], @@ -32,7 +32,7 @@ class Guidance { // Function to get the target quaternion and refence rotation rate from gps position for Nadir // pointing - void targetQuatPtgNadirSingleAxis(const double timeDelta, 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(const timeval timeAbsolute, const double timeDelta, double posSatE[3], double velSatE[3], double targetQuat[4], From 9482ceb2064216390e6b5a765a66a165ef9043a9 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 17 Oct 2023 17:01:29 +0200 Subject: [PATCH 09/68] noice --- mission/controller/acs/Guidance.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 6067676e..fe0ba4dd 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -4,13 +4,11 @@ #include #include #include -#include +#include +#include #include - -#include "string.h" -#include "util/CholeskyDecomposition.h" -#include "util/MathOperations.h" +#include Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } From 77a555debc13ba746a6001d97f3120209d232a8f Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 18 Oct 2023 11:00:43 +0200 Subject: [PATCH 10/68] small improvement --- mission/controller/PowerController.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/PowerController.cpp b/mission/controller/PowerController.cpp index e2862b80..f8a02c09 100644 --- a/mission/controller/PowerController.cpp +++ b/mission/controller/PowerController.cpp @@ -287,7 +287,7 @@ ReturnValue_t PowerController::calculateOpenCircuitVoltageCharge() { ReturnValue_t PowerController::calculateCoulombCounterCharge() { double timeDiff = 0.0; - if (timevalOperations::toDouble(oldTime) != 0) { + if (oldTime.tv_sec != 0) { timeDiff = timevalOperations::toDouble(now - oldTime); } else { return returnvalue::FAILED; From d5e57501befabb25347d6c8f9557f235997c18ec Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 18 Oct 2023 11:03:52 +0200 Subject: [PATCH 11/68] div by 0 check --- mission/controller/acs/Guidance.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index fe0ba4dd..bf834d12 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -460,13 +460,13 @@ void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double time if (VectorOperations::norm(savedQuaternion, 4) == 0) { std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion)); } - if (timeDelta < 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::subtract(q, qS, qDiff, 4); - VectorOperations::mulScalar(qDiff, 1 / timeDelta, qDiff, 4); + VectorOperations::mulScalar(qDiff, 1. / timeDelta, qDiff, 4); double tgtQuatVec[3] = {q[0], q[1], q[2]}; double qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; From 6621b20aef2e7096a18b5b93289f548cc3492ebb Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 18 Oct 2023 14:36:34 +0200 Subject: [PATCH 12/68] changelog --- CHANGELOG.md | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 09055dd7..9cace690 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -22,6 +22,16 @@ will consitute of a breaking change warranting a new major release: is not in normal mode. - MPSoC debug mode. +## Changed + +- `Power Controller` now uses monotonic clock for calculating time difference +- `ACS Controller` now uses monotonic clock for calculating time difference and the normal clock + for model calculations. The `timeDelta` is now calculated in the controller instead of + everywhere where it is needed. +- `ACS Controller` now has the function `performAttitudeControl` which is called prior to passing + on to the relevant mode functions. It handles all telemetry relevant functions, which were + always called, regardless of the mode. + # [v7.1.0] 2023-10-11 - Bumped `eive-tmtc` to v5.8.0. From 60a348a08fc74aab55e8306c9f35edccd0672cee Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 2 Nov 2023 16:59:09 +0100 Subject: [PATCH 13/68] this gonna be fun --- mission/acs/defs.h | 5 ++- mission/controller/AcsController.cpp | 30 +++++++++--------- mission/controller/acs/Guidance.cpp | 18 +++++------ mission/controller/acs/control/Detumble.cpp | 10 +++--- mission/controller/acs/control/Detumble.h | 2 +- mission/controller/acs/control/PtgCtrl.cpp | 35 +++++++++++++++++++++ mission/controller/acs/control/PtgCtrl.h | 10 ++++-- mission/controller/acs/control/SafeCtrl.cpp | 24 +++++++------- mission/controller/acs/control/SafeCtrl.h | 2 +- 9 files changed, 90 insertions(+), 46 deletions(-) diff --git a/mission/acs/defs.h b/mission/acs/defs.h index baed276d..15c7e5a8 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -22,7 +22,7 @@ enum AcsMode : Mode_t { enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 }; -enum SafeModeStrategy : uint8_t { +enum ControlModeStrategy : uint8_t { SAFECTRL_OFF = 0, SAFECTRL_NO_MAG_FIELD_FOR_CONTROL = 1, SAFECTRL_NO_SENSORS_FOR_CONTROL = 2, @@ -40,6 +40,9 @@ enum SafeModeStrategy : uint8_t { SAFECTRL_ECLIPSE_IDELING = 19, SAFECTRL_DETUMBLE_FULL = 20, SAFECTRL_DETUMBLE_DETERIORATED = 21, + // Added in vNext + PTGCTRL_ACTIVE_MEKF = 100, + PTGCTRL_WITHOUT_MEKF = 101, }; enum GpsSource : uint8_t { diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 89853b9f..ad3053b3 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -240,27 +240,27 @@ void AcsController::performSafe() { guidance.getTargetParamsSafe(sunTargetDir); double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; - acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy( + acs::ControlModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy( mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(), fusedRotRateData.rotRateTotal.isValid(), acsParameters.safeModeControllerParameters.useMekf, acsParameters.safeModeControllerParameters.useGyr, acsParameters.safeModeControllerParameters.dampingDuringEclipse); switch (safeCtrlStrat) { - case (acs::SafeModeStrategy::SAFECTRL_MEKF): + case (acs::ControlModeStrategy::SAFECTRL_MEKF): safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value, susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_GYR): + case (acs::ControlModeStrategy::SAFECTRL_GYR): safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_SUSMGM): + case (acs::ControlModeStrategy::SAFECTRL_SUSMGM): safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value, fusedRotRateData.rotRateParallel.value, fusedRotRateData.rotRateOrthogonal.value, @@ -268,29 +268,29 @@ void AcsController::performSafe() { safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR): + case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR): safeCtrl.safeRateDampingGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM): + case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM): safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING): + case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING): errAng = NAN; safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): + case (acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): safeCtrlFailure(1, 0); break; - case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): + case (acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): safeCtrlFailure(0, 1); break; default: @@ -335,24 +335,24 @@ void AcsController::performSafe() { } void AcsController::performDetumble() { - acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy( + acs::ControlModeStrategy safeCtrlStrat = detumble.detumbleStrategy( mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(), mgmDataProcessed.mgmVecTotDerivative.isValid(), acsParameters.detumbleParameter.useFullDetumbleLaw); double magMomMtq[3] = {0, 0, 0}; switch (safeCtrlStrat) { - case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL): + case (acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL): detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value, magMomMtq, acsParameters.detumbleParameter.gainFull); break; - case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED): + case (acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED): detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value, magMomMtq, acsParameters.detumbleParameter.gainBdot); break; - case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): + case (acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): safeCtrlFailure(1, 0); break; - case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): + case (acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): safeCtrlFailure(0, 1); break; default: @@ -668,7 +668,7 @@ void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQu std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double)); ctrlValData.errAng.value = errAng; std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double)); - ctrlValData.safeStrat.value = acs::SafeModeStrategy::SAFECTRL_OFF; + ctrlValData.safeStrat.value = acs::ControlModeStrategy::SAFECTRL_OFF; ctrlValData.setValidity(true, true); } } diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index bf834d12..0be636ad 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -495,24 +495,24 @@ void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double time ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) { - bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()); - bool rw2valid = (sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid()); - bool rw3valid = (sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid()); - bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid()); + bool rw1valid = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid()); + bool rw2valid = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid()); + bool rw3valid = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid()); + bool rw4valid = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid()); - if (rw1valid && rw2valid && rw3valid && rw4valid) { + if (rw1valid and rw2valid and rw3valid and rw4valid) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double)); return returnvalue::OK; - } else if (!rw1valid && rw2valid && rw3valid && rw4valid) { + } else if (not rw1valid and rw2valid and rw3valid and rw4valid) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without1, 12 * sizeof(double)); return returnvalue::OK; - } else if (rw1valid && !rw2valid && rw3valid && rw4valid) { + } else if (rw1valid and not rw2valid and rw3valid and rw4valid) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without2, 12 * sizeof(double)); return returnvalue::OK; - } else if (rw1valid && rw2valid && !rw3valid && rw4valid) { + } else if (rw1valid and rw2valid and not rw3valid and rw4valid) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without3, 12 * sizeof(double)); return returnvalue::OK; - } else if (rw1valid && rw2valid && rw3valid && !rw4valid) { + } else if (rw1valid and rw2valid and rw3valid and not rw4valid) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double)); return returnvalue::OK; } else { diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 8f422ec1..343308e5 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -7,18 +7,18 @@ Detumble::Detumble() {} Detumble::~Detumble() {} -acs::SafeModeStrategy Detumble::detumbleStrategy(const bool magFieldValid, +acs::ControlModeStrategy Detumble::detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, const bool magFieldRateValid, const bool useFullDetumbleLaw) { if (not magFieldValid) { - return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; + return acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (satRotRateValid and useFullDetumbleLaw) { - return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL; + return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL; } else if (magFieldRateValid) { - return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED; + return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED; } else { - return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; } } diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index 9fca77e6..476766c3 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -11,7 +11,7 @@ class Detumble { Detumble(); virtual ~Detumble(); - acs::SafeModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, + acs::ControlModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, const bool magFieldRateValid, const bool useFullDetumbleLaw); diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 2f5847cc..7cf13ab0 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -10,6 +10,41 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_ PtgCtrl::~PtgCtrl() {} +acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy( + const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, + const bool sunDirValid, const bool fusedRateTotalValid, const uint8_t mekfEnabled, + const uint8_t gyrEnabled, const uint8_t dampingEnabled) { + if (not magFieldValid) { + return acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; + } else if (mekfEnabled and mekfValid) { + return acs::ControlModeStrategy::SAFECTRL_MEKF; + } else if (sunDirValid) { + if (gyrEnabled and satRotRateValid) { + return acs::ControlModeStrategy::SAFECTRL_GYR; + } else if (not gyrEnabled and fusedRateTotalValid) { + return acs::ControlModeStrategy::SAFECTRL_SUSMGM; + } else { + return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + } + } else if (not sunDirValid) { + if (dampingEnabled) { + if (gyrEnabled and satRotRateValid) { + return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR; + } else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) { + return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; + } else { + return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + } + } else if (not dampingEnabled and satRotRateValid) { + return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING; + } else { + return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + } + } else { + return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + } +} + void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *errorQuat, const double *deltaRate, const double *rwPseudoInv, double *torqueRws) { diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 5f731e6b..2872cd5e 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -2,6 +2,7 @@ #define PTGCTRL_H_ #include +#include #include #include #include @@ -9,7 +10,7 @@ class PtgCtrl { /* * @brief: This class handles the pointing control mechanism. Calculation of an commanded - * torque for the reaction wheels, and magnetic Field strength for magnetorques for desaturation + * torque for the reaction wheels, and magnetic Field strength for magnetorquer for desaturation * * @note: A description of the used algorithms can be found in * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110 @@ -21,6 +22,11 @@ class PtgCtrl { PtgCtrl(AcsParameters *acsParameters_); virtual ~PtgCtrl(); + acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy( + const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, + const bool sunDirValid, const bool fusedRateTotalValid, const uint8_t mekfEnabled, + const uint8_t gyrEnabled, const uint8_t dampingEnabled); + /* @brief: Calculates the needed torque for the pointing control mechanism */ void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError, @@ -36,7 +42,7 @@ class PtgCtrl { const int32_t speedRw3, double *mgtDpDes); /* @brief: Commands the stiction torque in case wheel speed is to low - * torqueCommand modified torque after antistiction + * torqueCommand modified torque after anti-stiction */ void rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeed); diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index de0cd197..1a8c1211 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -9,40 +9,40 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter SafeCtrl::~SafeCtrl() {} -acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, +acs::ControlModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, const bool sunDirValid, const bool fusedRateTotalValid, const uint8_t mekfEnabled, const uint8_t gyrEnabled, const uint8_t dampingEnabled) { if (not magFieldValid) { - return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; + return acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (mekfEnabled and mekfValid) { - return acs::SafeModeStrategy::SAFECTRL_MEKF; + return acs::ControlModeStrategy::SAFECTRL_MEKF; } else if (sunDirValid) { if (gyrEnabled and satRotRateValid) { - return acs::SafeModeStrategy::SAFECTRL_GYR; + return acs::ControlModeStrategy::SAFECTRL_GYR; } else if (not gyrEnabled and fusedRateTotalValid) { - return acs::SafeModeStrategy::SAFECTRL_SUSMGM; + return acs::ControlModeStrategy::SAFECTRL_SUSMGM; } else { - return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; } } else if (not sunDirValid) { if (dampingEnabled) { if (gyrEnabled and satRotRateValid) { - return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR; + return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR; } else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) { - return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; + return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; } else { - return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; } } else if (not dampingEnabled and satRotRateValid) { - return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING; + return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING; } else { - return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; } } else { - return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; } } diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index d35d5d04..1c38d81d 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -12,7 +12,7 @@ class SafeCtrl { SafeCtrl(AcsParameters *acsParameters_); virtual ~SafeCtrl(); - acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, + acs::ControlModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, const bool sunDirValid, const bool fusedRateTotalValid, const uint8_t mekfEnabled, const uint8_t gyrEnabled, const uint8_t dampingEnabled); From 463841526489c519838f569b69874a1bf282b614 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 14 Nov 2023 13:21:03 +0100 Subject: [PATCH 14/68] b-side as default --- mission/system/acs/acsModeTree.cpp | 35 ++++++++++++++++-------------- 1 file changed, 19 insertions(+), 16 deletions(-) diff --git a/mission/system/acs/acsModeTree.cpp b/mission/system/acs/acsModeTree.cpp index 185ad9eb..6429c0d4 100644 --- a/mission/system/acs/acsModeTree.cpp +++ b/mission/system/acs/acsModeTree.cpp @@ -106,7 +106,7 @@ Subsystem& satsystem::acs::init() { // Build TARGET PT transition 0 iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TRANS_0.second); iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TRANS_0.second, true); - iht(objects::ACS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TRANS_0.second, true); + iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TRANS_0.second, true); iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TRANS_0.second); iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TRANS_0.second); check(ACS_SUBSYSTEM.addTable( @@ -160,7 +160,8 @@ void buildOffSequence(Subsystem& ss, ModeListEntry& eh) { // Build OFF transition 1 iht(objects::IMTQ_ASSY, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); iht(objects::STR_ASSY, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); - iht(objects::ACS_BOARD_ASS, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); + iht(objects::ACS_BOARD_ASS, OFF, duallane::B_SIDE, ACS_TABLE_OFF_TRANS_1.second); + iht(objects::SUS_BOARD_ASS, OFF, duallane::A_SIDE, ACS_TABLE_OFF_TRANS_1.second); iht(objects::RW_ASSY, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); check(ss.addTable(TableEntry(ACS_TABLE_OFF_TRANS_1.first, &ACS_TABLE_OFF_TRANS_1.second)), ctxc); @@ -199,13 +200,13 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) { iht(objects::ACS_CONTROLLER, acs::AcsMode::SAFE, acs::SafeSubmode::DEFAULT, ACS_TABLE_SAFE_TGT.second, true); iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_SAFE_TGT.second); - iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second, true); - iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second, true); + iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_SAFE_TGT.second, true); + iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_SAFE_TGT.second, true); check(ss.addTable(&ACS_TABLE_SAFE_TGT.second, ACS_TABLE_SAFE_TGT.first, false, true), ctxc); // Build SAFE transition 0 iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_SAFE_TRANS_0.second); - iht(objects::ACS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_SAFE_TRANS_0.second, true); + iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_SAFE_TRANS_0.second, true); iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_SAFE_TRANS_0.second, true); iht(objects::STR_ASSY, OFF, 0, ACS_TABLE_SAFE_TRANS_0.second); iht(objects::RW_ASSY, OFF, 0, ACS_TABLE_SAFE_TRANS_0.second); @@ -256,13 +257,13 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) { iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second); iht(objects::RW_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second); iht(objects::STR_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second); - iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second, true); - iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second, true); + iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_IDLE_TGT.second, true); + iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_IDLE_TGT.second, true); ss.addTable(&ACS_TABLE_IDLE_TGT.second, ACS_TABLE_IDLE_TGT.first, false, true); // Build IDLE transition 0 iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_IDLE_TRANS_0.second); - iht(objects::ACS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_IDLE_TRANS_0.second, true); + iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_IDLE_TRANS_0.second, true); iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_IDLE_TRANS_0.second, true); iht(objects::RW_ASSY, NML, 0, ACS_TABLE_IDLE_TRANS_0.second); iht(objects::STR_ASSY, NML, 0, ACS_TABLE_IDLE_TRANS_0.second); @@ -306,8 +307,8 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) { // Build TARGET PT table iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_TARGET, 0, ACS_TABLE_PTG_TARGET_TGT.second); iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); - iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second, true); - iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second, true); + iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TARGET_TGT.second, true); + iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TARGET_TGT.second, true); iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); check(ss.addTable(&ACS_TABLE_PTG_TARGET_TGT.second, ACS_TABLE_PTG_TARGET_TGT.first, false, true), @@ -355,8 +356,8 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) { // Build TARGET PT table iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_NADIR, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); - iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second, true); - iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second, true); + iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TARGET_NADIR_TGT.second, true); + iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TARGET_NADIR_TGT.second, true); iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_NADIR_TGT.first, @@ -408,8 +409,8 @@ void buildTargetPtGsSequence(Subsystem& ss, ModeListEntry& eh) { // Build TARGET PT table iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_TARGET_GS, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); - iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second, true); - iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second, true); + iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TARGET_GS_TGT.second, true); + iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TARGET_GS_TGT.second, true); iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); check(ss.addTable( @@ -461,8 +462,10 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) { iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_INERTIAL, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); - iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second, true); - iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second, true); + iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second, + true); + iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second, + true); iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_INERTIAL_TGT.first, From c155f399b142bc0c76c3b06ae067ca4053622250 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 14 Nov 2023 13:22:35 +0100 Subject: [PATCH 15/68] . --- mission/acs/defs.h | 6 +-- mission/controller/AcsController.cpp | 10 ++--- .../acs/FusedRotationEstimation.cpp | 41 +++++++++++++++---- .../controller/acs/FusedRotationEstimation.h | 14 ++++++- mission/controller/acs/control/Detumble.cpp | 4 +- mission/controller/acs/control/PtgCtrl.cpp | 12 +++--- mission/controller/acs/control/SafeCtrl.cpp | 10 ++--- 7 files changed, 67 insertions(+), 30 deletions(-) diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 15c7e5a8..253c6aca 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -23,9 +23,9 @@ enum AcsMode : Mode_t { enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 }; enum ControlModeStrategy : uint8_t { - SAFECTRL_OFF = 0, - SAFECTRL_NO_MAG_FIELD_FOR_CONTROL = 1, - SAFECTRL_NO_SENSORS_FOR_CONTROL = 2, + CTRL_OFF = 0, + CTRL_NO_MAG_FIELD_FOR_CONTROL = 1, + CTRL_NO_SENSORS_FOR_CONTROL = 2, // OBSW version <= v6.1.0 LEGACY_SAFECTRL_ACTIVE_MEKF = 10, LEGACY_SAFECTRL_WITHOUT_MEKF = 11, diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index ad3053b3..5a606304 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -287,10 +287,10 @@ void AcsController::performSafe() { safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): + case (acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL): safeCtrlFailure(1, 0); break; - case (acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): + case (acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL): safeCtrlFailure(0, 1); break; default: @@ -349,10 +349,10 @@ void AcsController::performDetumble() { detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value, magMomMtq, acsParameters.detumbleParameter.gainBdot); break; - case (acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): + case (acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL): safeCtrlFailure(1, 0); break; - case (acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): + case (acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL): safeCtrlFailure(0, 1); break; default: @@ -668,7 +668,7 @@ void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQu std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double)); ctrlValData.errAng.value = errAng; std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double)); - ctrlValData.safeStrat.value = acs::ControlModeStrategy::SAFECTRL_OFF; + ctrlValData.safeStrat.value = acs::ControlModeStrategy::CTRL_OFF; ctrlValData.setValidity(true, true); } } diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index 4f1dad45..a68418d2 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -4,6 +4,31 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_) acsParameters = acsParameters_; } +void FusedRotationEstimation::estimateFusedRotationRate( + acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues, + acsctrl::FusedRotRateData *fusedRotRateData) { + if (sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and + sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid()) { + double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, + sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; + if (VectorOperations::norm(quatOld, 4) != 0) { + estimateFusedRotationRateStr(quatNew, fusedRotRateData); + } else { + estimateFusedRotationRateSafe(susDataProcessed, mgmDataProcessed, gyrDataProcessed, + fusedRotRateData); + } + std::memcpy(quatOld, quatNew, sizeof(quatOld)); + } else { + std::memcpy(quatOld, ZERO_VEC4, sizeof(quatOld)); + estimateFusedRotationRateSafe(susDataProcessed, mgmDataProcessed, gyrDataProcessed, + fusedRotRateData); + } +} + +void FusedRotationEstimation::estimateFusedRotationRateStr( + double *quatNew, acsctrl::FusedRotRateData *fusedRotRateData) {} + void FusedRotationEstimation::estimateFusedRotationRateSafe( acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) { @@ -13,9 +38,9 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( not mgmDataProcessed->mgmVecTotDerivative.isValid())) { { PoolReadGuard pg(fusedRotRateData); - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double)); fusedRotRateData->setValidity(false, true); } // store for calculation of angular acceleration @@ -92,9 +117,9 @@ void FusedRotationEstimation::estimateFusedRotationRateEclipse( VectorOperations::norm(fusedRotRateData->rotRateTotal.value, 3) == 0) { { PoolReadGuard pg(fusedRotRateData); - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double)); fusedRotRateData->setValidity(false, true); } return; @@ -106,9 +131,9 @@ void FusedRotationEstimation::estimateFusedRotationRateEclipse( 3); { PoolReadGuard pg(fusedRotRateData); - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); fusedRotRateData->rotRateOrthogonal.setValid(false); - std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); fusedRotRateData->rotRateParallel.setValid(false); std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double)); fusedRotRateData->rotRateTotal.setValid(true); diff --git a/mission/controller/acs/FusedRotationEstimation.h b/mission/controller/acs/FusedRotationEstimation.h index fa4fda1c..989f1f7a 100644 --- a/mission/controller/acs/FusedRotationEstimation.h +++ b/mission/controller/acs/FusedRotationEstimation.h @@ -2,24 +2,36 @@ #define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ #include +#include #include #include +#include #include class FusedRotationEstimation { public: FusedRotationEstimation(AcsParameters *acsParameters_); + void estimateFusedRotationRate(acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::GyrDataProcessed *gyrDataProcessed, + ACS::SensorValues *sensorValues, + acsctrl::FusedRotRateData *fusedRotRateData); + void estimateFusedRotationRateSafe(acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData); + void estimateFusedRotationRateStr(double *quatNew, acsctrl::FusedRotRateData *fusedRotRateData); + protected: private: - static constexpr double ZERO_VEC[3] = {0, 0, 0}; + static constexpr double ZERO_VEC3[3] = {0, 0, 0}; + static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0}; AcsParameters *acsParameters; + double quatOld[3] = {0, 0, 0, 0}; double rotRateOldB[3] = {0, 0, 0}; void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed, diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 343308e5..754843f8 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -12,13 +12,13 @@ acs::ControlModeStrategy Detumble::detumbleStrategy(const bool magFieldValid, const bool magFieldRateValid, const bool useFullDetumbleLaw) { if (not magFieldValid) { - return acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (satRotRateValid and useFullDetumbleLaw) { return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL; } else if (magFieldRateValid) { return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED; } else { - return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } } diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 7cf13ab0..2818d4e7 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -15,16 +15,16 @@ acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy( const bool sunDirValid, const bool fusedRateTotalValid, const uint8_t mekfEnabled, const uint8_t gyrEnabled, const uint8_t dampingEnabled) { if (not magFieldValid) { - return acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (mekfEnabled and mekfValid) { - return acs::ControlModeStrategy::SAFECTRL_MEKF; + return acs::ControlModeStrategy::PTGCTRL_ACTIVE_MEKF; } else if (sunDirValid) { if (gyrEnabled and satRotRateValid) { return acs::ControlModeStrategy::SAFECTRL_GYR; } else if (not gyrEnabled and fusedRateTotalValid) { return acs::ControlModeStrategy::SAFECTRL_SUSMGM; } else { - return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } } else if (not sunDirValid) { if (dampingEnabled) { @@ -33,15 +33,15 @@ acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy( } else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) { return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; } else { - return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } } else if (not dampingEnabled and satRotRateValid) { return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING; } else { - return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } } else { - return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } } diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 1a8c1211..b1699aef 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -16,7 +16,7 @@ acs::ControlModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, co const uint8_t gyrEnabled, const uint8_t dampingEnabled) { if (not magFieldValid) { - return acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (mekfEnabled and mekfValid) { return acs::ControlModeStrategy::SAFECTRL_MEKF; } else if (sunDirValid) { @@ -25,7 +25,7 @@ acs::ControlModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, co } else if (not gyrEnabled and fusedRateTotalValid) { return acs::ControlModeStrategy::SAFECTRL_SUSMGM; } else { - return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } } else if (not sunDirValid) { if (dampingEnabled) { @@ -34,15 +34,15 @@ acs::ControlModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, co } else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) { return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; } else { - return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } } else if (not dampingEnabled and satRotRateValid) { return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING; } else { - return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } } else { - return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } } From 74e7785c6832ee26381492c03faacd9e8303927e Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 14 Nov 2023 16:07:14 +0100 Subject: [PATCH 16/68] calc rot rate from STR --- .../acs/FusedRotationEstimation.cpp | 40 +++++++++++++++++-- .../controller/acs/FusedRotationEstimation.h | 5 ++- 2 files changed, 39 insertions(+), 6 deletions(-) diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index a68418d2..1b2df526 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -7,13 +7,13 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_) void FusedRotationEstimation::estimateFusedRotationRate( acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues, - acsctrl::FusedRotRateData *fusedRotRateData) { + const double timeDelta, acsctrl::FusedRotRateData *fusedRotRateData) { if (sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid()) { double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; - if (VectorOperations::norm(quatOld, 4) != 0) { - estimateFusedRotationRateStr(quatNew, fusedRotRateData); + if (VectorOperations::norm(quatOld, 4) != 0 and timeDelta != 0) { + estimateFusedRotationRateStr(quatNew, timeDelta, fusedRotRateData); } else { estimateFusedRotationRateSafe(susDataProcessed, mgmDataProcessed, gyrDataProcessed, fusedRotRateData); @@ -27,7 +27,39 @@ void FusedRotationEstimation::estimateFusedRotationRate( } void FusedRotationEstimation::estimateFusedRotationRateStr( - double *quatNew, acsctrl::FusedRotRateData *fusedRotRateData) {} + double *quatNew, const double timeDelta, acsctrl::FusedRotRateData *fusedRotRateData) { + double quatOldInv[4] = {0, 0, 0, 0}; + double quatDelta[4] = {0, 0, 0, 0}; + + QuaternionOperations::inverse(quatOld, quatOldInv); + QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta); + QuaternionOperations::normalize(quatDelta); + + double rotVec[3] = {0, 0, 0}; + double angle = QuaternionOperations::getAngle(quatDelta); + if (angle == 0.0) { + { + PoolReadGuard pg(fusedRotRateData); + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->rotRateOrthogonal.setValid(false); + std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->rotRateParallel.setValid(false); + std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->rotRateTotal.setValid(true); + } + } + VectorOperations::normalize(quatDelta, rotVec, 3); + VectorOperations::mulScalar(rotVec, angle / timeDelta, rotVec, 3); + { + PoolReadGuard pg(fusedRotRateData); + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->rotRateOrthogonal.setValid(false); + std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->rotRateParallel.setValid(false); + std::memcpy(fusedRotRateData->rotRateTotal.value, rotVec, 3 * sizeof(double)); + fusedRotRateData->rotRateTotal.setValid(true); + } +} void FusedRotationEstimation::estimateFusedRotationRateSafe( acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, diff --git a/mission/controller/acs/FusedRotationEstimation.h b/mission/controller/acs/FusedRotationEstimation.h index 989f1f7a..490fe126 100644 --- a/mission/controller/acs/FusedRotationEstimation.h +++ b/mission/controller/acs/FusedRotationEstimation.h @@ -15,7 +15,7 @@ class FusedRotationEstimation { void estimateFusedRotationRate(acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, - ACS::SensorValues *sensorValues, + ACS::SensorValues *sensorValues, const double timeDelta, acsctrl::FusedRotRateData *fusedRotRateData); void estimateFusedRotationRateSafe(acsctrl::SusDataProcessed *susDataProcessed, @@ -23,7 +23,8 @@ class FusedRotationEstimation { acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData); - void estimateFusedRotationRateStr(double *quatNew, acsctrl::FusedRotRateData *fusedRotRateData); + void estimateFusedRotationRateStr(double *quatNew, const double timeDelta, + acsctrl::FusedRotRateData *fusedRotRateData); protected: private: From 5512605cd7aedff8b1fd52680d2582c602385de6 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 14 Nov 2023 16:23:12 +0100 Subject: [PATCH 17/68] param to enable/disable STR for rot rate calc --- mission/controller/acs/AcsParameters.h | 1 + mission/controller/acs/FusedRotationEstimation.cpp | 5 +++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index a9c9ab5d..f62928a0 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -910,6 +910,7 @@ class AcsParameters : public HasParametersIF { struct StrParameters { double exclusionAngle = 20 * M_PI / 180; double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // geometry frame + uint8_t useStrForRotRate = true; } strParameters; struct GpsParameters { diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index 1b2df526..cf91b951 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -8,8 +8,9 @@ void FusedRotationEstimation::estimateFusedRotationRate( acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues, const double timeDelta, acsctrl::FusedRotRateData *fusedRotRateData) { - if (sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and - sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid()) { + if (acsParameters->strParameters.useStrForRotRate and + (sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and + sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid())) { double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; if (VectorOperations::norm(quatOld, 4) != 0 and timeDelta != 0) { From 9482f3cae9e3334b15637488a1eadc6ec834dbcd Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 14 Nov 2023 16:57:55 +0100 Subject: [PATCH 18/68] fdir needs to change --- mission/controller/AcsController.cpp | 60 ++++++++-------------------- 1 file changed, 16 insertions(+), 44 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 5a606304..8b484fb4 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -169,23 +169,24 @@ void AcsController::performAttitudeControl() { result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &mekfData, &acsParameters); + if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and + result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { + if (not mekfInvalidFlag) { + triggerEvent(acs::MEKF_INVALID_INFO, static_cast(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 (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(); @@ -200,35 +201,6 @@ void AcsController::performAttitudeControl() { 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; } From 4a86d4ba4b95cd510f30a18531aaa70cfde24a1a Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 15 Nov 2023 16:54:54 +0100 Subject: [PATCH 19/68] changed fdir to new variant --- mission/acs/defs.h | 6 +++--- mission/controller/AcsController.cpp | 15 +++++++++++++++ mission/controller/AcsController.h | 2 +- mission/controller/acs/AcsParameters.cpp | 2 +- mission/controller/acs/AcsParameters.h | 2 +- mission/system/acs/StrFdir.cpp | 2 +- 6 files changed, 22 insertions(+), 7 deletions(-) diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 253c6aca..2f82ca9f 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -67,9 +67,9 @@ static constexpr Event MEKF_INVALID_INFO = MAKE_EVENT(3, severity::INFO); static constexpr Event MEKF_RECOVERY = MAKE_EVENT(4, severity::INFO); //! [EXPORT] : [COMMENT] MEKF performed an automatic reset after detection of nonfinite values. static constexpr Event MEKF_AUTOMATIC_RESET = MAKE_EVENT(5, severity::INFO); -//! [EXPORT] : [COMMENT] MEKF was not able to compute a solution during any pointing ACS mode for a -//! prolonged time. -static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(6, severity::HIGH); +//! [EXPORT] : [COMMENT] For a prolonged time, no attitude information was available for the +//! Pointing Controller. Falling back to Safe Mode. +static constexpr Event PTG_CTRL_NO_ATTITUDE_INFORMATION = MAKE_EVENT(6, severity::HIGH); //! [EXPORT] : [COMMENT] The ACS safe mode controller was not able to compute a solution and has //! failed. //! P1: Missing information about magnetic field, P2: Missing information about rotational rate diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 8b484fb4..47c5d77e 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -369,6 +369,21 @@ void AcsController::performDetumble() { } void AcsController::performPointingCtrl() { + acs::ControlModeStrategy ptgCtrlStrat = + ptgCtrl.pointingCtrlStrategy(magFieldValid, mekfValid, satRotRateValid, sunDirValid, + fusedRateTotalValid, mekfEnabled, gyrEnabled, dampingEnabled); + + if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) { + ptgCtrlLostCounter++; + if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) { + triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION); + ptgCtrlLostCounter = 0; + } + commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], + cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], + acsParameters.rwHandlingParameters.rampTime); + } + uint8_t enableAntiStiction = true; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 1b184b80..f67595ee 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -72,7 +72,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes uint8_t detumbleCounter = 0; uint8_t multipleRwUnavailableCounter = 0; bool mekfInvalidFlag = false; - uint16_t mekfInvalidCounter = 0; + uint16_t ptgCtrlLostCounter = 0; bool safeCtrlFailureFlag = false; uint8_t safeCtrlFailureCounter = 0; uint8_t resetMekfCount = 0; diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 7b867a46..551f7cd3 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -24,7 +24,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(onBoardParams.sampleTime); break; case 0x1: - parameterWrapper->set(onBoardParams.mekfViolationTimer); + parameterWrapper->set(onBoardParams.ptgCtrlLostTimer); break; case 0x2: parameterWrapper->set(onBoardParams.fusedRateSafeDuringEclipse); diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index f62928a0..cbebb52a 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -18,7 +18,7 @@ class AcsParameters : public HasParametersIF { struct OnBoardParams { double sampleTime = 0.4; // [s] - uint16_t mekfViolationTimer = 750; + uint16_t ptgCtrlLostTimer = 750; uint8_t fusedRateSafeDuringEclipse = true; } onBoardParams; diff --git a/mission/system/acs/StrFdir.cpp b/mission/system/acs/StrFdir.cpp index 97a4162a..83bd27fa 100644 --- a/mission/system/acs/StrFdir.cpp +++ b/mission/system/acs/StrFdir.cpp @@ -6,7 +6,7 @@ StrFdir::StrFdir(object_id_t strObject) : DeviceHandlerFailureIsolation(strObject, objects::NO_OBJECT) {} ReturnValue_t StrFdir::eventReceived(EventMessage* event) { - if (event->getEvent() == acs::MEKF_INVALID_MODE_VIOLATION) { + if (event->getEvent() == acs::PTG_CTRL_NO_ATTITUDE_INFORMATION) { setFaulty(event->getEvent()); return returnvalue::OK; } From a45e96b772398e079fd8367260ba968c878792a1 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 22 Nov 2023 13:36:04 +0100 Subject: [PATCH 20/68] quest attitude estimation --- mission/controller/acs/AttitudeEstimation.cpp | 91 +++++++++++++++++++ mission/controller/acs/AttitudeEstimation.h | 28 ++++++ mission/controller/acs/CMakeLists.txt | 1 + 3 files changed, 120 insertions(+) create mode 100644 mission/controller/acs/AttitudeEstimation.cpp create mode 100644 mission/controller/acs/AttitudeEstimation.h diff --git a/mission/controller/acs/AttitudeEstimation.cpp b/mission/controller/acs/AttitudeEstimation.cpp new file mode 100644 index 00000000..0e8be1b1 --- /dev/null +++ b/mission/controller/acs/AttitudeEstimation.cpp @@ -0,0 +1,91 @@ +#include "AttitudeEstimation.h" + +AttitudeEstimation::AttitudeEstimation(AcsParameters *acsParameters_) { + acsParameters = acsParameters_; +} + +AttitudeEstimation::~AttitudeEstimation() {} + +void AttitudeEstimation::Quest(const double susB[3], const bool susBvalid, const double susI[3], + const bool susIvalid, const double mgmB[3], const bool mgmBvalid, + const double mgmI[3], const bool mgmIvalid, double qBI[4]) { + if (susBvalid and susIvalid and mgmBvalid and mgmIvalid) { + // Normalize Data + double normMgmB[3] = {0, 0, 0}, normMgmI[3] = {0, 0, 0}, normSusB[3] = {0, 0, 0}, + normSusI[3] = {0, 0, 0}; + VectorOperations::normalize(mgmB, normMgmB, 3); + VectorOperations::normalize(mgmI, normMgmI, 3); + VectorOperations::normalize(susB, normSusB, 3); + VectorOperations::normalize(susI, normSusI, 3); + + // Create Helper Vectors + double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0}, + helperSum[3] = {0, 0, 0}; + VectorOperations::cross(normSusB, normMgmB, normHelperB); + VectorOperations::cross(normSusI, normMgmI, normHelperI); + VectorOperations::normalize(normHelperB, normHelperB, 3); + VectorOperations::normalize(normHelperI, normHelperI, 3); + VectorOperations::cross(normHelperB, normHelperI, helperCross); + VectorOperations::add(normHelperB, normHelperI, helperSum, 3); + + // Sensor Weights + double kSus = 0, kMgm = 0; + kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSS, -2); + kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMAG, -2); + + // Weighted Vectors + double weightedSusB[3] = {0, 0, 0}, weightedMgmB[3] = {0, 0, 0}, kSusVec[3] = {0, 0, 0}, + kMgmVec[3] = {0, 0, 0}, kSumVec[3] = {0, 0, 0}; + VectorOperations::mulScalar(normSusB, kSus, weightedSusB, 3); + VectorOperations::mulScalar(normMgmB, kMgm, weightedMgmB, 3); + VectorOperations::cross(weightedSusB, normSusI, kSusVec); + VectorOperations::cross(weightedMgmB, normMgmI, kMgmVec); + VectorOperations::add(kSusVec, kMgmVec, kSumVec, 3); + + // Some weird Angles + double alpha = (1 + VectorOperations::dot(normHelperB, normHelperI)) * + (VectorOperations::dot(weightedSusB, normSusI) + + VectorOperations::dot(weightedMgmB, normMgmI)) + + VectorOperations::dot(helperCross, kSumVec); + double beta = VectorOperations::dot(helperSum, kSumVec); + double gamma = std::sqrt(std::pow(alpha, 2) + std::pow(beta, 2)); + + // I don't even know what this is supposed to be + double constPlus = + 1. / (2 * std::sqrt(gamma * (gamma + alpha) * + (1 + VectorOperations::dot(normHelperB, normHelperI)))); + double constMinus = + 1. / (2 * std::sqrt(gamma * (gamma - alpha) * + (1 + VectorOperations::dot(normHelperB, normHelperI)))); + + // Calculate Quaternion + double qRotVecTot[3] = {0, 0, 0}, qRotVecPt0[3] = {0, 0, 0}, qRotVecPt1[3] = {0, 0, 0}; + if (alpha >= 0) { + // Scalar Part + qBI[3] = (gamma + alpha) * (1 + VectorOperations::dot(normHelperB, normHelperI)); + // Rotational Vector Part + VectorOperations::mulScalar(helperCross, gamma + alpha, qRotVecPt0, 3); + VectorOperations::add(normHelperB, normHelperI, qRotVecPt1, 3); + VectorOperations::mulScalar(qRotVecPt1, beta, qRotVecPt1, 3); + VectorOperations::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3); + std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot)); + + VectorOperations::mulScalar(qBI, constPlus, qBI, 3); + QuaternionOperations::normalize(qBI, qBI); + } else { + // Scalar Part + qBI[3] = (beta) * (1 + VectorOperations::dot(normHelperB, normHelperI)); + // Rotational Vector Part + VectorOperations::mulScalar(helperCross, beta, qRotVecPt0, 3); + VectorOperations::add(normHelperB, normHelperI, qRotVecPt1, 3); + VectorOperations::mulScalar(qRotVecPt1, gamma - alpha, qRotVecPt1, 3); + VectorOperations::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3); + std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot)); + + VectorOperations::mulScalar(qBI, constMinus, qBI, 3); + QuaternionOperations::normalize(qBI, qBI); + } + } else { + // ToDo: fill dataset and set to invalid + } +} diff --git a/mission/controller/acs/AttitudeEstimation.h b/mission/controller/acs/AttitudeEstimation.h new file mode 100644 index 00000000..eee58f81 --- /dev/null +++ b/mission/controller/acs/AttitudeEstimation.h @@ -0,0 +1,28 @@ +#ifndef MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_ +#define MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_ + +#include +#include +#include + +#include +#include + +class AttitudeEstimation { + public: + AttitudeEstimation(AcsParameters *acsParameters_); + virtual ~AttitudeEstimation(); + ; + + void Quest(const double susB[3], const bool susBvalid, const double susI[3], const bool susIvalid, + const double mgmB[3], const bool mgmBvalid, const double mgmI[3], const bool mgmIvalid, + double qBI[4]); + + protected: + private: + AcsParameters *acsParameters; + + void Triad(double susB[3], double susI[3], double mgmB[3], double mgmI[3]); +}; + +#endif /* MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_ */ diff --git a/mission/controller/acs/CMakeLists.txt b/mission/controller/acs/CMakeLists.txt index a8b0e9a6..4af05d8b 100644 --- a/mission/controller/acs/CMakeLists.txt +++ b/mission/controller/acs/CMakeLists.txt @@ -2,6 +2,7 @@ target_sources( ${LIB_EIVE_MISSION} PRIVATE AcsParameters.cpp ActuatorCmd.cpp + AttitudeEstimation.cpp FusedRotationEstimation.cpp Guidance.cpp Igrf13Model.cpp From 0aa09bd5169b907861f97dba4d8ae47dc22b45cf Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 22 Nov 2023 13:56:33 +0100 Subject: [PATCH 21/68] this is better --- mission/controller/acs/AttitudeEstimation.cpp | 21 +++++++++++-------- mission/controller/acs/AttitudeEstimation.h | 8 +++---- .../AcsCtrlDefinitions.h | 8 ++++--- 3 files changed, 20 insertions(+), 17 deletions(-) diff --git a/mission/controller/acs/AttitudeEstimation.cpp b/mission/controller/acs/AttitudeEstimation.cpp index 0e8be1b1..f8effbe9 100644 --- a/mission/controller/acs/AttitudeEstimation.cpp +++ b/mission/controller/acs/AttitudeEstimation.cpp @@ -6,17 +6,18 @@ AttitudeEstimation::AttitudeEstimation(AcsParameters *acsParameters_) { AttitudeEstimation::~AttitudeEstimation() {} -void AttitudeEstimation::Quest(const double susB[3], const bool susBvalid, const double susI[3], - const bool susIvalid, const double mgmB[3], const bool mgmBvalid, - const double mgmI[3], const bool mgmIvalid, double qBI[4]) { - if (susBvalid and susIvalid and mgmBvalid and mgmIvalid) { +void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData, + acsctrl::MgmDataProcessed *mgmData, + acsctrl::AttitudeEstimationData *attitudeEstimation) { + if (susData->susVecTot.isValid() and susData->sunIjkModel.isValid() and + mgmData->mgmVecTot.value and mgmData->magIgrfModel.isValid()) { // Normalize Data double normMgmB[3] = {0, 0, 0}, normMgmI[3] = {0, 0, 0}, normSusB[3] = {0, 0, 0}, normSusI[3] = {0, 0, 0}; - VectorOperations::normalize(mgmB, normMgmB, 3); - VectorOperations::normalize(mgmI, normMgmI, 3); - VectorOperations::normalize(susB, normSusB, 3); - VectorOperations::normalize(susI, normSusI, 3); + VectorOperations::normalize(susData->susVecTot.value, normMgmB, 3); + VectorOperations::normalize(susData->sunIjkModel.value, normMgmI, 3); + VectorOperations::normalize(mgmData->mgmVecTot.value, normSusB, 3); + VectorOperations::normalize(mgmData->magIgrfModel.value, normSusI, 3); // Create Helper Vectors double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0}, @@ -59,7 +60,8 @@ void AttitudeEstimation::Quest(const double susB[3], const bool susBvalid, const (1 + VectorOperations::dot(normHelperB, normHelperI)))); // Calculate Quaternion - double qRotVecTot[3] = {0, 0, 0}, qRotVecPt0[3] = {0, 0, 0}, qRotVecPt1[3] = {0, 0, 0}; + double qBI[4] = {0, 0, 0, 0}, qRotVecTot[3] = {0, 0, 0}, qRotVecPt0[3] = {0, 0, 0}, + qRotVecPt1[3] = {0, 0, 0}; if (alpha >= 0) { // Scalar Part qBI[3] = (gamma + alpha) * (1 + VectorOperations::dot(normHelperB, normHelperI)); @@ -85,6 +87,7 @@ void AttitudeEstimation::Quest(const double susB[3], const bool susBvalid, const VectorOperations::mulScalar(qBI, constMinus, qBI, 3); QuaternionOperations::normalize(qBI, qBI); } + // ToDo: fill dataset and set to valid } else { // ToDo: fill dataset and set to invalid } diff --git a/mission/controller/acs/AttitudeEstimation.h b/mission/controller/acs/AttitudeEstimation.h index eee58f81..b485b7b1 100644 --- a/mission/controller/acs/AttitudeEstimation.h +++ b/mission/controller/acs/AttitudeEstimation.h @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -14,15 +15,12 @@ class AttitudeEstimation { virtual ~AttitudeEstimation(); ; - void Quest(const double susB[3], const bool susBvalid, const double susI[3], const bool susIvalid, - const double mgmB[3], const bool mgmBvalid, const double mgmI[3], const bool mgmIvalid, - double qBI[4]); + void quest(acsctrl::SusDataProcessed *susData, acsctrl::MgmDataProcessed *mgmData, + acsctrl::AttitudeEstimationData *attitudeEstimation); protected: private: AcsParameters *acsParameters; - - void Triad(double susB[3], double susI[3], double mgmB[3], double mgmI[3]); }; #endif /* MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_ */ diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index b843ca0c..26edf269 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -96,6 +96,7 @@ enum PoolIds : lp_id_t { SAT_ROT_RATE_MEKF, QUAT_MEKF, MEKF_STATUS, + QUAT_QUEST, // Ctrl Values SAFE_STRAT, TGT_QUAT, @@ -122,7 +123,7 @@ static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15; static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4; static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5; static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 6; -static constexpr uint8_t MEKF_SET_ENTRIES = 3; +static constexpr uint8_t ATTITUDE_ESTIMATION_SET_ENTRIES = 4; static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 3; @@ -250,13 +251,14 @@ class GpsDataProcessed : public StaticLocalDataSet { private: }; -class MekfData : public StaticLocalDataSet { +class AttitudeEstimationData : public StaticLocalDataSet { public: - MekfData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MEKF_DATA) {} + AttitudeEstimationData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MEKF_DATA) {} lp_vec_t quatMekf = lp_vec_t(sid.objectId, QUAT_MEKF, this); lp_vec_t satRotRateMekf = lp_vec_t(sid.objectId, SAT_ROT_RATE_MEKF, this); lp_var_t mekfStatus = lp_var_t(sid.objectId, MEKF_STATUS, this); + lp_vec_t quatQuest = lp_vec_t(sid.objectId, QUAT_MEKF, this); private: }; From 886dd17e4a515e8a3e26e79b82adb8b13b69d5cc Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 23 Nov 2023 11:50:26 +0100 Subject: [PATCH 22/68] dataset output --- mission/controller/acs/AttitudeEstimation.cpp | 16 ++++++++++++++-- mission/controller/acs/AttitudeEstimation.h | 3 +++ 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/AttitudeEstimation.cpp b/mission/controller/acs/AttitudeEstimation.cpp index f8effbe9..45bd9461 100644 --- a/mission/controller/acs/AttitudeEstimation.cpp +++ b/mission/controller/acs/AttitudeEstimation.cpp @@ -87,8 +87,20 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData, VectorOperations::mulScalar(qBI, constMinus, qBI, 3); QuaternionOperations::normalize(qBI, qBI); } - // ToDo: fill dataset and set to valid + { + PoolReadGuard pg{attitudeEstimation}; + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(attitudeEstimation->quatQuest.value, qBI, 4 * sizeof(double)); + attitudeEstimation->quatQuest.setValid(true); + } + } } else { - // ToDo: fill dataset and set to invalid + { + PoolReadGuard pg{attitudeEstimation}; + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(attitudeEstimation->quatQuest.value, ZERO_VEC4, 4 * sizeof(double)); + attitudeEstimation->quatQuest.setValid(false); + } + } } } diff --git a/mission/controller/acs/AttitudeEstimation.h b/mission/controller/acs/AttitudeEstimation.h index b485b7b1..99df8091 100644 --- a/mission/controller/acs/AttitudeEstimation.h +++ b/mission/controller/acs/AttitudeEstimation.h @@ -1,6 +1,7 @@ #ifndef MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_ #define MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_ +#include #include #include #include @@ -21,6 +22,8 @@ class AttitudeEstimation { protected: private: AcsParameters *acsParameters; + + static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0}; }; #endif /* MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_ */ From 4a67f9ffe5a094e4e75d4ca70765b149c37e3f3a Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 23 Nov 2023 16:56:36 +0100 Subject: [PATCH 23/68] this is a mess --- mission/acs/defs.h | 5 +- mission/controller/AcsController.cpp | 112 +++++++++++------- mission/controller/AcsController.h | 16 ++- mission/controller/acs/AcsParameters.cpp | 6 + mission/controller/acs/AcsParameters.h | 3 +- .../acs/FusedRotationEstimation.cpp | 100 +++++++++++----- .../controller/acs/FusedRotationEstimation.h | 25 ++-- .../acs/MultiplicativeKalmanFilter.cpp | 10 +- .../acs/MultiplicativeKalmanFilter.h | 10 +- mission/controller/acs/Navigation.cpp | 4 +- mission/controller/acs/Navigation.h | 4 +- mission/controller/acs/control/PtgCtrl.cpp | 33 ++---- mission/controller/acs/control/PtgCtrl.h | 8 +- .../AcsCtrlDefinitions.h | 30 ++++- 14 files changed, 234 insertions(+), 132 deletions(-) diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 2f82ca9f..d9ce3c6c 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -41,8 +41,9 @@ enum ControlModeStrategy : uint8_t { SAFECTRL_DETUMBLE_FULL = 20, SAFECTRL_DETUMBLE_DETERIORATED = 21, // Added in vNext - PTGCTRL_ACTIVE_MEKF = 100, - PTGCTRL_WITHOUT_MEKF = 101, + PTGCTRL_MEKF = 100, + PTGCTRL_STR = 101, + PTGCTRL_QUEST = 102, }; enum GpsSource : uint8_t { diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 47c5d77e..3990c3f2 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -7,6 +7,7 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets) : ExtendedControllerBase(objectId), enableHkSets(enableHkSets), + attitudeEstimation(&acsParameters), fusedRotationEstimation(&acsParameters), guidance(&acsParameters), safeCtrl(&acsParameters), @@ -19,10 +20,11 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets) gyrDataRaw(this), gyrDataProcessed(this), gpsDataProcessed(this), - mekfData(this), + attitudeEstimationData(this), ctrlValData(this), actuatorCmdData(this), fusedRotRateData(this), + fusedRotRateSourcesData(this), tleData(this) {} ReturnValue_t AcsController::initialize() { @@ -56,7 +58,7 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t return HasActionsIF::EXECUTION_FINISHED; } case RESET_MEKF: { - navigation.resetMekf(&mekfData); + navigation.resetMekf(&attitudeEstimationData); return HasActionsIF::EXECUTION_FINISHED; } case RESTORE_MEKF_NONFINITE_RECOVERY: { @@ -164,20 +166,23 @@ void AcsController::performAttitudeControl() { sensorProcessing.process(timeAbsolute, timeDelta, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed, - &gyrDataProcessed, &fusedRotRateData); + attitudeEstimation.quest(&susDataProcessed, &mgmDataProcessed, &attitudeEstimationData); + fusedRotationEstimation.estimateFusedRotationRate( + &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues, + &attitudeEstimationData, timeDelta, &fusedRotRateData); result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, - &susDataProcessed, &mekfData, &acsParameters); + &susDataProcessed, &attitudeEstimationData, &acsParameters); if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { if (not mekfInvalidFlag) { - triggerEvent(acs::MEKF_INVALID_INFO, static_cast(mekfData.mekfStatus.value)); + triggerEvent(acs::MEKF_INVALID_INFO, + static_cast(attitudeEstimationData.mekfStatus.value)); mekfInvalidFlag = true; } if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE and not mekfLost) { triggerEvent(acs::MEKF_AUTOMATIC_RESET); - navigation.resetMekf(&mekfData); + navigation.resetMekf(&attitudeEstimationData); mekfLost = true; } } else if (mekfInvalidFlag) { @@ -220,9 +225,10 @@ void AcsController::performSafe() { acsParameters.safeModeControllerParameters.dampingDuringEclipse); switch (safeCtrlStrat) { case (acs::ControlModeStrategy::SAFECTRL_MEKF): - safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value, - susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir, - magMomMtq, errAng); + safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, + attitudeEstimationData.satRotRateMekf.value, + susDataProcessed.sunIjkModel.value, attitudeEstimationData.quatMekf.value, + sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; @@ -275,8 +281,8 @@ void AcsController::performSafe() { // detumble check and switch if (acsParameters.safeModeControllerParameters.useMekf) { - if (mekfData.satRotRateMekf.isValid() and - VectorOperations::norm(mekfData.satRotRateMekf.value, 3) > + if (attitudeEstimationData.satRotRateMekf.isValid() and + VectorOperations::norm(attitudeEstimationData.satRotRateMekf.value, 3) > acsParameters.detumbleParameter.omegaDetumbleStart) { detumbleCounter++; } @@ -336,8 +342,8 @@ void AcsController::performDetumble() { acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs); if (acsParameters.safeModeControllerParameters.useMekf) { - if (mekfData.satRotRateMekf.isValid() and - VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < + if (attitudeEstimationData.satRotRateMekf.isValid() and + VectorOperations::norm(attitudeEstimationData.satRotRateMekf.value, 3) < acsParameters.detumbleParameter.omegaDetumbleEnd) { detumbleCounter++; } @@ -369,20 +375,24 @@ void AcsController::performDetumble() { } void AcsController::performPointingCtrl() { + bool strValid = (sensorValues.strSet.caliQw.isValid() and sensorValues.strSet.caliQx.isValid() and + sensorValues.strSet.caliQy.isValid() and sensorValues.strSet.caliQz.isValid()); acs::ControlModeStrategy ptgCtrlStrat = - ptgCtrl.pointingCtrlStrategy(magFieldValid, mekfValid, satRotRateValid, sunDirValid, - fusedRateTotalValid, mekfEnabled, gyrEnabled, dampingEnabled); + ptgCtrl.pointingCtrlStrategy(mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, + strValid, fusedRotRateData.rotRateTotal.isValid(), mekfEnabled, + acsParameters.strParameters.useStrForRotRate); - if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) { - ptgCtrlLostCounter++; - if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) { - triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION); - ptgCtrlLostCounter = 0; - } - commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], - cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], - acsParameters.rwHandlingParameters.rampTime); - } + // if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) { + // ptgCtrlLostCounter++; + // if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) { + // triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION); + // ptgCtrlLostCounter = 0; + // } + // commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, + // cmdSpeedRws[0], + // cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], + // acsParameters.rwHandlingParameters.rampTime); + // } uint8_t enableAntiStiction = true; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -410,8 +420,9 @@ void AcsController::performPointingCtrl() { case acs::PTG_IDLE: guidance.targetQuatPtgSun(timeDelta, susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); - guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, - targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); + guidance.comparePtg(attitudeEstimationData.quatMekf.value, + attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate, + errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters, @@ -422,7 +433,7 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, + mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction; @@ -432,8 +443,9 @@ void AcsController::performPointingCtrl() { guidance.targetQuatPtgThreeAxes(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate); - guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, - targetSatRotRate, acsParameters.targetModeControllerParameters.quatRef, + guidance.comparePtg(attitudeEstimationData.quatMekf.value, + attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate, + acsParameters.targetModeControllerParameters.quatRef, acsParameters.targetModeControllerParameters.refRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate, @@ -446,7 +458,7 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, + mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; @@ -455,8 +467,9 @@ void AcsController::performPointingCtrl() { case acs::PTG_TARGET_GS: 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); + guidance.comparePtg(attitudeEstimationData.quatMekf.value, + attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate, + errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters, @@ -467,7 +480,7 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, + mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction; @@ -477,8 +490,9 @@ void AcsController::performPointingCtrl() { 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, + guidance.comparePtg(attitudeEstimationData.quatMekf.value, + attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate, + acsParameters.nadirModeControllerParameters.quatRef, acsParameters.nadirModeControllerParameters.refRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate, @@ -491,7 +505,7 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, + mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; @@ -500,8 +514,9 @@ void AcsController::performPointingCtrl() { case acs::PTG_INERTIAL: std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, sizeof(targetQuat)); - guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, - targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef, + guidance.comparePtg(attitudeEstimationData.quatMekf.value, + attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate, + acsParameters.inertialModeControllerParameters.quatRef, acsParameters.inertialModeControllerParameters.refRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate, @@ -514,7 +529,7 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, + mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; @@ -732,11 +747,12 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity); localDataPoolMap.emplace(acsctrl::PoolIds::SOURCE, &gpsSource); poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), enableHkSets, 30.0}); - // MEKF + // Attitude Estimation localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf); localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf); localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus); - poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), enableHkSets, 10.0}); + localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_QUEST, &quatQuest); + poolManager.subscribeForDiagPeriodicPacket({attitudeEstimationData.getSid(), enableHkSets, 10.0}); // Ctrl Values localDataPoolMap.emplace(acsctrl::PoolIds::SAFE_STRAT, &safeStrat); localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat); @@ -753,7 +769,15 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL, &rotRateOrthogonal); localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel); localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL, &rotRateTotal); + localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_SOURCE, &rotRateSource); poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0}); + // Fused Rot Rate Sources + localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL_SUSMGM, &rotRateOrthogonalSusMgm); + localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL_SUSMGM, &rotRateParallelSusMgm); + localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_SUSMGM, &rotRateTotalSusMgm); + localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_QUEST, &rotRateTotalQuest); + localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_STR, &rotRateTotalStr); + poolManager.subscribeForRegularPeriodicPacket({fusedRotRateSourcesData.getSid(), false, 10.0}); // TLE Data localDataPoolMap.emplace(acsctrl::PoolIds::TLE_LINE_1, &line1); localDataPoolMap.emplace(acsctrl::PoolIds::TLE_LINE_2, &line2); @@ -777,7 +801,7 @@ LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) { case acsctrl::GPS_PROCESSED_DATA: return &gpsDataProcessed; case acsctrl::MEKF_DATA: - return &mekfData; + return &attitudeEstimationData; case acsctrl::CTRL_VAL_DATA: return &ctrlValData; case acsctrl::ACTUATOR_CMD_DATA: diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index f67595ee..6d30cba6 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -14,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -57,6 +58,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes AcsParameters acsParameters; SensorProcessing sensorProcessing; + AttitudeEstimation attitudeEstimation; FusedRotationEstimation fusedRotationEstimation; Navigation navigation; ActuatorCmd actuatorCmd; @@ -218,11 +220,12 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes PoolEntry gpsVelocity = PoolEntry(3); PoolEntry gpsSource = PoolEntry(); - // MEKF - acsctrl::MekfData mekfData; + // Attitude Estimation + acsctrl::AttitudeEstimationData attitudeEstimationData; PoolEntry quatMekf = PoolEntry(4); PoolEntry satRotRateMekf = PoolEntry(3); PoolEntry mekfStatus = PoolEntry(); + PoolEntry quatQuest = PoolEntry(4); // Ctrl Values acsctrl::CtrlValData ctrlValData; @@ -243,6 +246,15 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes PoolEntry rotRateOrthogonal = PoolEntry(3); PoolEntry rotRateParallel = PoolEntry(3); PoolEntry rotRateTotal = PoolEntry(3); + PoolEntry rotRateSource = PoolEntry(); + + // Fused Rot Rate Sources + acsctrl::FusedRotRateSourcesData fusedRotRateSourcesData; + PoolEntry rotRateOrthogonalSusMgm = PoolEntry(3); + PoolEntry rotRateParallelSusMgm = PoolEntry(3); + PoolEntry rotRateTotalSusMgm = PoolEntry(3); + PoolEntry rotRateTotalQuest = PoolEntry(3); + PoolEntry rotRateTotalStr = PoolEntry(3); // TLE Dataset acsctrl::TleData tleData; diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 551f7cd3..ab5e301e 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -29,6 +29,12 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x2: parameterWrapper->set(onBoardParams.fusedRateSafeDuringEclipse); break; + case 0x3: + parameterWrapper->set(onBoardParams.fusedRateFromStr); + break; + case 0x4: + parameterWrapper->set(onBoardParams.fusedRateFromQuest); + break; default: return INVALID_IDENTIFIER_ID; } diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index cbebb52a..7c1b2de6 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -20,6 +20,8 @@ class AcsParameters : public HasParametersIF { double sampleTime = 0.4; // [s] uint16_t ptgCtrlLostTimer = 750; uint8_t fusedRateSafeDuringEclipse = true; + uint8_t fusedRateFromStr = false; + uint8_t fusedRateFromQuest = false; } onBoardParams; struct InertiaEIVE { @@ -910,7 +912,6 @@ class AcsParameters : public HasParametersIF { struct StrParameters { double exclusionAngle = 20 * M_PI / 180; double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // geometry frame - uint8_t useStrForRotRate = true; } strParameters; struct GpsParameters { diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index cf91b951..ee245519 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -7,32 +7,73 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_) void FusedRotationEstimation::estimateFusedRotationRate( acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues, - const double timeDelta, acsctrl::FusedRotRateData *fusedRotRateData) { - if (acsParameters->strParameters.useStrForRotRate and - (sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and + acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta, + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData, + acsctrl::FusedRotRateData *fusedRotRateData) { + estimateFusedRotationRateStr(sensorValues, timeDelta, fusedRotRateSourcesData); + estimateFusedRotationRateQuest(attitudeEstimationData, timeDelta, fusedRotRateSourcesData); + + // Fused Rotation Estimation of STR + if ((sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid())) { double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; - if (VectorOperations::norm(quatOld, 4) != 0 and timeDelta != 0) { - estimateFusedRotationRateStr(quatNew, timeDelta, fusedRotRateData); + if (VectorOperations::norm(quatOldStr, 4) != 0 and timeDelta != 0) { + estimateFusedRotationRateQuat(quatNew, timeDelta, true, fusedRotRateSourcesData); } else { - estimateFusedRotationRateSafe(susDataProcessed, mgmDataProcessed, gyrDataProcessed, - fusedRotRateData); + { + PoolReadGuard pg(fusedRotRateSourcesData); + std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalStr.setValid(false); + } } - std::memcpy(quatOld, quatNew, sizeof(quatOld)); + std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr)); } else { - std::memcpy(quatOld, ZERO_VEC4, sizeof(quatOld)); - estimateFusedRotationRateSafe(susDataProcessed, mgmDataProcessed, gyrDataProcessed, - fusedRotRateData); + std::memcpy(quatOldStr, ZERO_VEC4, sizeof(quatOldStr)); + { + PoolReadGuard pg(fusedRotRateSourcesData); + std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalStr.setValid(false); + } } + + // Fused Rotation Estimation of QUEST + if ((attitudeEstimationData->quatQuest.isValid())) { + if (VectorOperations::norm(quatOldQuest, 4) != 0 and timeDelta != 0) { + estimateFusedRotationRateQuat(attitudeEstimationData->quatQuest.value, timeDelta, true, + fusedRotRateSourcesData); + } else { + { + PoolReadGuard pg(fusedRotRateSourcesData); + std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalQuest.setValid(false); + } + } + std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest)); + } else { + std::memcpy(quatOldQuest, ZERO_VEC4, sizeof(quatOldQuest)); + { + PoolReadGuard pg(fusedRotRateSourcesData); + std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalQuest.setValid(false); + } + } + + // Fused Rotation Estimation of SUS&MGM } -void FusedRotationEstimation::estimateFusedRotationRateStr( - double *quatNew, const double timeDelta, acsctrl::FusedRotRateData *fusedRotRateData) { +void FusedRotationEstimation::estimateFusedRotationRateQuat( + double *quatNew, const double timeDelta, const bool str, + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) { double quatOldInv[4] = {0, 0, 0, 0}; double quatDelta[4] = {0, 0, 0, 0}; - QuaternionOperations::inverse(quatOld, quatOldInv); + if (str) { + QuaternionOperations::inverse(quatOldStr, quatOldInv); + } else { + QuaternionOperations::inverse(quatOldQuest, quatOldInv); + } QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta); QuaternionOperations::normalize(quatDelta); @@ -40,25 +81,28 @@ void FusedRotationEstimation::estimateFusedRotationRateStr( double angle = QuaternionOperations::getAngle(quatDelta); if (angle == 0.0) { { - PoolReadGuard pg(fusedRotRateData); - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); - fusedRotRateData->rotRateOrthogonal.setValid(false); - std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); - fusedRotRateData->rotRateParallel.setValid(false); - std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double)); - fusedRotRateData->rotRateTotal.setValid(true); + PoolReadGuard pg(fusedRotRateSourcesData); + if (str) { + std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalStr.setValid(true); + } else { + std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalQuest.setValid(true); + } } } VectorOperations::normalize(quatDelta, rotVec, 3); VectorOperations::mulScalar(rotVec, angle / timeDelta, rotVec, 3); { - PoolReadGuard pg(fusedRotRateData); - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); - fusedRotRateData->rotRateOrthogonal.setValid(false); - std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); - fusedRotRateData->rotRateParallel.setValid(false); - std::memcpy(fusedRotRateData->rotRateTotal.value, rotVec, 3 * sizeof(double)); - fusedRotRateData->rotRateTotal.setValid(true); + PoolReadGuard pg(fusedRotRateSourcesData); + if (str) { + std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, rotVec, 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalStr.setValid(true); + } else { + std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, rotVec, 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalQuest.setValid(true); + } } } diff --git a/mission/controller/acs/FusedRotationEstimation.h b/mission/controller/acs/FusedRotationEstimation.h index 490fe126..2c4c1588 100644 --- a/mission/controller/acs/FusedRotationEstimation.h +++ b/mission/controller/acs/FusedRotationEstimation.h @@ -15,28 +15,33 @@ class FusedRotationEstimation { void estimateFusedRotationRate(acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, - ACS::SensorValues *sensorValues, const double timeDelta, + ACS::SensorValues *sensorValues, + acsctrl::AttitudeEstimationData *attitudeEstimationData, + const double timeDelta, + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData, acsctrl::FusedRotRateData *fusedRotRateData); - void estimateFusedRotationRateSafe(acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::MgmDataProcessed *mgmDataProcessed, - acsctrl::GyrDataProcessed *gyrDataProcessed, - acsctrl::FusedRotRateData *fusedRotRateData); - - void estimateFusedRotationRateStr(double *quatNew, const double timeDelta, - acsctrl::FusedRotRateData *fusedRotRateData); - protected: private: static constexpr double ZERO_VEC3[3] = {0, 0, 0}; static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0}; AcsParameters *acsParameters; - double quatOld[3] = {0, 0, 0, 0}; + double quatOldQuest[4] = {0, 0, 0, 0}; + double quatOldStr[4] = {0, 0, 0, 0}; double rotRateOldB[3] = {0, 0, 0}; + void estimateFusedRotationRateSusMgm(acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData); void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData); + void estimateFusedRotationRateQuest(acsctrl::AttitudeEstimationData *attitudeEstimationData, + const double timeDelta, + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData); + void estimateFusedRotationRateStr(ACS::SensorValues *sensorValues, const double timeDelta, + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData); }; #endif /* MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ */ diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 0f297b21..52acc7ee 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -19,7 +19,7 @@ MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {} ReturnValue_t MultiplicativeKalmanFilter::init( const double *magneticField_, const bool validMagField_, const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ, - const bool validMagModel, acsctrl::MekfData *mekfData, + const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters) { // valids for "model measurements"? // check for valid mag/sun if (validMagField_ && validSS && validSSModel && validMagModel) { @@ -191,7 +191,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_, const bool validGYRs_, const double *magneticField_, const bool validMagField_, const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, - const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData, + const double *magFieldJ, const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters) { // Check for GYR Measurements int MDF = 0; // Matrix Dimension Factor @@ -1090,7 +1090,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( return MEKF_RUNNING; } -ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) { +ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::AttitudeEstimationData *mekfData) { double resetQuaternion[4] = {0, 0, 0, 1}; double resetCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}; @@ -1100,7 +1100,7 @@ ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) { return MEKF_UNINITIALIZED; } -void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData, +void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus) { { PoolReadGuard pg(mekfData); @@ -1115,7 +1115,7 @@ void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mek } } -void MultiplicativeKalmanFilter::updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, +void MultiplicativeKalmanFilter::updateDataSet(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus, double quat[4], double satRotRate[3]) { { PoolReadGuard pg(mekfData); diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index 5a878c4b..bd722115 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -21,7 +21,7 @@ class MultiplicativeKalmanFilter { MultiplicativeKalmanFilter(); virtual ~MultiplicativeKalmanFilter(); - ReturnValue_t reset(acsctrl::MekfData *mekfData); + ReturnValue_t reset(acsctrl::AttitudeEstimationData *mekfData); /* @brief: init() - This function initializes the Kalman Filter and will provide the first * quaternion through the QUEST algorithm @@ -32,7 +32,7 @@ class MultiplicativeKalmanFilter { */ ReturnValue_t init(const double *magneticField_, const bool validMagField_, const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, - const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData, + const double *magFieldJ, const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters); /* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter @@ -53,7 +53,7 @@ class MultiplicativeKalmanFilter { const bool validGYRs_, const double *magneticField_, const bool validMagField_, const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ, - const bool validMagModel, acsctrl::MekfData *mekfData, + const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters); enum MekfStatus : uint8_t { @@ -99,8 +99,8 @@ class MultiplicativeKalmanFilter { double biasGYR[3]; /*Between measured and estimated sat Rate*/ /*Parameter INIT*/ /*Functions*/ - void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus); - void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4], + void updateDataSetWithoutData(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus); + void updateDataSet(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus, double quat[4], double satRotRate[3]); }; diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 4726586b..8c3b1dd9 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -16,7 +16,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::MekfData *mekfData, AcsParameters *acsParameters) { + acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters) { double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; bool quatIBValid = sensorValues->strSet.isTrustWorthy.value; @@ -41,7 +41,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, } } -void Navigation::resetMekf(acsctrl::MekfData *mekfData) { +void Navigation::resetMekf(acsctrl::AttitudeEstimationData *mekfData) { mekfStatus = multiplicativeKalmanFilter.reset(mekfData); } diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h index 2785ff2e..4f58b197 100644 --- a/mission/controller/acs/Navigation.h +++ b/mission/controller/acs/Navigation.h @@ -17,9 +17,9 @@ class Navigation { ReturnValue_t useMekf(ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, - acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData, + acsctrl::SusDataProcessed *susDataProcessed, acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters); - void resetMekf(acsctrl::MekfData *mekfData); + void resetMekf(acsctrl::AttitudeEstimationData *mekfData); ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed); ReturnValue_t updateTle(const uint8_t *line1, const uint8_t *line2); diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 2818d4e7..cf722f35 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -10,36 +10,17 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_ PtgCtrl::~PtgCtrl() {} -acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy( - const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, - const bool sunDirValid, const bool fusedRateTotalValid, const uint8_t mekfEnabled, - const uint8_t gyrEnabled, const uint8_t dampingEnabled) { +acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(const bool magFieldValid, + const bool mekfValid, const bool strValid, + const bool fusedRateValid, + const uint8_t mekfEnabled, + const uint8_t strRateEnabled) { if (not magFieldValid) { return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (mekfEnabled and mekfValid) { return acs::ControlModeStrategy::PTGCTRL_ACTIVE_MEKF; - } else if (sunDirValid) { - if (gyrEnabled and satRotRateValid) { - return acs::ControlModeStrategy::SAFECTRL_GYR; - } else if (not gyrEnabled and fusedRateTotalValid) { - return acs::ControlModeStrategy::SAFECTRL_SUSMGM; - } else { - return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; - } - } else if (not sunDirValid) { - if (dampingEnabled) { - if (gyrEnabled and satRotRateValid) { - return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR; - } else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) { - return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; - } else { - return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; - } - } else if (not dampingEnabled and satRotRateValid) { - return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING; - } else { - return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; - } + } else if (strValid and strRateEnabled and fusedRateValid) { + return acs::ControlModeStrategy::PTGCTRL_WITHOUT_MEKF; } else { return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 2872cd5e..b484f3c9 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -22,10 +22,10 @@ class PtgCtrl { PtgCtrl(AcsParameters *acsParameters_); virtual ~PtgCtrl(); - acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy( - const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, - const bool sunDirValid, const bool fusedRateTotalValid, const uint8_t mekfEnabled, - const uint8_t gyrEnabled, const uint8_t dampingEnabled); + acs::ControlModeStrategy pointingCtrlStrategy(const bool magFieldValid, const bool mekfValid, + const bool strValid, const bool fusedRateValid, + const uint8_t mekfEnabled, + const uint8_t strRateEnabled); /* @brief: Calculates the needed torque for the pointing control mechanism */ diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index 26edf269..83b4bb87 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -20,6 +20,7 @@ enum SetIds : uint32_t { CTRL_VAL_DATA, ACTUATOR_CMD_DATA, FUSED_ROTATION_RATE_DATA, + FUSED_ROTATION_RATE_SOURCES_DATA, TLE_SET, }; @@ -111,6 +112,13 @@ enum PoolIds : lp_id_t { ROT_RATE_ORTHOGONAL, ROT_RATE_PARALLEL, ROT_RATE_TOTAL, + ROT_RATE_SOURCE, + // Fused Rotation Rate Sources + ROT_RATE_ORTHOGONAL_SUSMGM, + ROT_RATE_PARALLEL_SUSMGM, + ROT_RATE_TOTAL_SUSMGM, + ROT_RATE_TOTAL_QUEST, + ROT_RATE_TOTAL_STR, // TLE TLE_LINE_1, TLE_LINE_2, @@ -126,7 +134,8 @@ static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 6; static constexpr uint8_t ATTITUDE_ESTIMATION_SET_ENTRIES = 4; static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; -static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 3; +static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 4; +static constexpr uint8_t FUSED_ROT_RATE_SOURCES_SET_ENTRIES = 5; static constexpr uint8_t TLE_SET_ENTRIES = 2; /** @@ -297,6 +306,25 @@ class FusedRotRateData : public StaticLocalDataSet { lp_vec_t(sid.objectId, ROT_RATE_ORTHOGONAL, this); lp_vec_t rotRateParallel = lp_vec_t(sid.objectId, ROT_RATE_PARALLEL, this); lp_vec_t rotRateTotal = lp_vec_t(sid.objectId, ROT_RATE_TOTAL, this); + lp_var_t rotRateSource = lp_var_t(sid.objectId, ROT_RATE_SOURCE, this); + + private: +}; + +class FusedRotRateSourcesData : public StaticLocalDataSet { + public: + FusedRotRateSourcesData(HasLocalDataPoolIF* hkOwner) + : StaticLocalDataSet(hkOwner, FUSED_ROTATION_RATE_SOURCES_DATA) {} + + lp_vec_t rotRateOrthogonalSusMgm = + lp_vec_t(sid.objectId, ROT_RATE_ORTHOGONAL_SUSMGM, this); + lp_vec_t rotRateParallelSusMgm = + lp_vec_t(sid.objectId, ROT_RATE_PARALLEL_SUSMGM, this); + lp_vec_t rotRateTotalSusMgm = + lp_vec_t(sid.objectId, ROT_RATE_TOTAL_SUSMGM, this); + lp_vec_t rotRateTotalQuest = + lp_vec_t(sid.objectId, ROT_RATE_TOTAL_QUEST, this); + lp_vec_t rotRateTotalStr = lp_vec_t(sid.objectId, ROT_RATE_TOTAL_STR, this); private: }; From 647d5fda7c6b78b213b815185032d646de78fc13 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 24 Nov 2023 10:52:36 +0100 Subject: [PATCH 24/68] this is much better --- .../acs/FusedRotationEstimation.cpp | 286 ++++++++++++------ .../controller/acs/FusedRotationEstimation.h | 2 +- 2 files changed, 194 insertions(+), 94 deletions(-) diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index ee245519..a74b6082 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -12,113 +12,194 @@ void FusedRotationEstimation::estimateFusedRotationRate( acsctrl::FusedRotRateData *fusedRotRateData) { estimateFusedRotationRateStr(sensorValues, timeDelta, fusedRotRateSourcesData); estimateFusedRotationRateQuest(attitudeEstimationData, timeDelta, fusedRotRateSourcesData); + estimateFusedRotationRateSusMgm(susDataProcessed, mgmDataProcessed, gyrDataProcessed, + fusedRotRateSourcesData); - // Fused Rotation Estimation of STR + if (fusedRotRateSourcesData->rotRateTotalStr.isValid() and + acsParameters->onBoardParams.fusedRateFromStr) { + PoolReadGuard pg(fusedRotRateData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->rotRateOrthogonal.setValid(false); + std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->rotRateParallel.setValid(false); + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, + fusedRotRateSourcesData->rotRateTotalStr.value, 3 * sizeof(double)); + fusedRotRateData->rotRateOrthogonal.setValid(true); + } + } else if (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and + acsParameters->onBoardParams.fusedRateFromQuest) { + PoolReadGuard pg(fusedRotRateData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->rotRateOrthogonal.setValid(false); + std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->rotRateParallel.setValid(false); + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, + fusedRotRateSourcesData->rotRateTotalQuest.value, 3 * sizeof(double)); + fusedRotRateData->rotRateOrthogonal.setValid(true); + } + } else if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) { + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, + fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, 3 * sizeof(double)); + fusedRotRateData->rotRateOrthogonal.setValid( + fusedRotRateSourcesData->rotRateOrthogonalSusMgm.isValid()); + std::memcpy(fusedRotRateData->rotRateParallel.value, + fusedRotRateSourcesData->rotRateParallelSusMgm.value, 3 * sizeof(double)); + fusedRotRateData->rotRateParallel.setValid( + fusedRotRateSourcesData->rotRateParallelSusMgm.isValid()); + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, + fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double)); + fusedRotRateData->rotRateOrthogonal.setValid(true); + } else { + PoolReadGuard pg(fusedRotRateData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->setValidity(false, true); + } + } +} + +void FusedRotationEstimation::estimateFusedRotationRateStr( + ACS::SensorValues *sensorValues, const double timeDelta, + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) { if ((sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid())) { double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; if (VectorOperations::norm(quatOldStr, 4) != 0 and timeDelta != 0) { - estimateFusedRotationRateQuat(quatNew, timeDelta, true, fusedRotRateSourcesData); - } else { + double quatOldInv[4] = {0, 0, 0, 0}; + double quatDelta[4] = {0, 0, 0, 0}; + + QuaternionOperations::inverse(quatOldStr, quatOldInv); + QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta); + QuaternionOperations::normalize(quatDelta); + + double rotVec[3] = {0, 0, 0}; + double angle = QuaternionOperations::getAngle(quatDelta); + if (angle == 0.0) { + { + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalStr.setValid(true); + } + } + } + VectorOperations::normalize(quatDelta, rotVec, 3); + VectorOperations::mulScalar(rotVec, angle / timeDelta, rotVec, 3); { PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, rotVec, 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalStr.setValid(true); + } + } + std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr)); + return; + } + { + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double)); fusedRotRateSourcesData->rotRateTotalStr.setValid(false); } } std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr)); - } else { - std::memcpy(quatOldStr, ZERO_VEC4, sizeof(quatOldStr)); - { - PoolReadGuard pg(fusedRotRateSourcesData); + return; + } + { + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double)); fusedRotRateSourcesData->rotRateTotalStr.setValid(false); } } + std::memcpy(quatOldStr, ZERO_VEC4, sizeof(quatOldStr)); +} - // Fused Rotation Estimation of QUEST - if ((attitudeEstimationData->quatQuest.isValid())) { +void FusedRotationEstimation::estimateFusedRotationRateQuest( + acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta, + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) { + if (attitudeEstimationData->quatQuest.isValid()) { if (VectorOperations::norm(quatOldQuest, 4) != 0 and timeDelta != 0) { - estimateFusedRotationRateQuat(attitudeEstimationData->quatQuest.value, timeDelta, true, - fusedRotRateSourcesData); - } else { + double quatOldInv[4] = {0, 0, 0, 0}; + double quatDelta[4] = {0, 0, 0, 0}; + + QuaternionOperations::inverse(quatOldQuest, quatOldInv); + QuaternionOperations::multiply(attitudeEstimationData->quatQuest.value, quatOldInv, + quatDelta); + QuaternionOperations::normalize(quatDelta); + + double rotVec[3] = {0, 0, 0}; + double angle = QuaternionOperations::getAngle(quatDelta); + if (angle == 0.0) { + { + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalQuest.setValid(true); + } + } + } + VectorOperations::normalize(quatDelta, rotVec, 3); + VectorOperations::mulScalar(rotVec, angle / timeDelta, rotVec, 3); { PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, rotVec, 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalQuest.setValid(true); + } + } + std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest)); + return; + } + { + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, 3 * sizeof(double)); fusedRotRateSourcesData->rotRateTotalQuest.setValid(false); } } std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest)); - } else { - std::memcpy(quatOldQuest, ZERO_VEC4, sizeof(quatOldQuest)); - { - PoolReadGuard pg(fusedRotRateSourcesData); + return; + } + { + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, 3 * sizeof(double)); fusedRotRateSourcesData->rotRateTotalQuest.setValid(false); } } - - // Fused Rotation Estimation of SUS&MGM + std::memcpy(quatOldQuest, ZERO_VEC4, sizeof(quatOldQuest)); } -void FusedRotationEstimation::estimateFusedRotationRateQuat( - double *quatNew, const double timeDelta, const bool str, - acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) { - double quatOldInv[4] = {0, 0, 0, 0}; - double quatDelta[4] = {0, 0, 0, 0}; - - if (str) { - QuaternionOperations::inverse(quatOldStr, quatOldInv); - } else { - QuaternionOperations::inverse(quatOldQuest, quatOldInv); - } - QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta); - QuaternionOperations::normalize(quatDelta); - - double rotVec[3] = {0, 0, 0}; - double angle = QuaternionOperations::getAngle(quatDelta); - if (angle == 0.0) { - { - PoolReadGuard pg(fusedRotRateSourcesData); - if (str) { - std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double)); - fusedRotRateSourcesData->rotRateTotalStr.setValid(true); - } else { - std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, - 3 * sizeof(double)); - fusedRotRateSourcesData->rotRateTotalQuest.setValid(true); - } - } - } - VectorOperations::normalize(quatDelta, rotVec, 3); - VectorOperations::mulScalar(rotVec, angle / timeDelta, rotVec, 3); - { - PoolReadGuard pg(fusedRotRateSourcesData); - if (str) { - std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, rotVec, 3 * sizeof(double)); - fusedRotRateSourcesData->rotRateTotalStr.setValid(true); - } else { - std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, rotVec, 3 * sizeof(double)); - fusedRotRateSourcesData->rotRateTotalQuest.setValid(true); - } - } -} - -void FusedRotationEstimation::estimateFusedRotationRateSafe( +void FusedRotationEstimation::estimateFusedRotationRateSusMgm( acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, - acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) { + acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) { if ((not mgmDataProcessed->mgmVecTot.isValid() and not susDataProcessed->susVecTot.isValid() and - not fusedRotRateData->rotRateTotal.isValid()) or + not fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) or (not susDataProcessed->susVecTotDerivative.isValid() and not mgmDataProcessed->mgmVecTotDerivative.isValid())) { { - PoolReadGuard pg(fusedRotRateData); - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double)); - fusedRotRateData->setValidity(false, true); + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(false); + std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(false); + std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(false); + } } // store for calculation of angular acceleration if (gyrDataProcessed->gyrVecTot.isValid()) { @@ -127,7 +208,7 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( return; } if (not susDataProcessed->susVecTot.isValid()) { - estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData); + estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateSourcesData); // store for calculation of angular acceleration if (gyrDataProcessed->gyrVecTot.isValid()) { std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double)); @@ -151,7 +232,7 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( VectorOperations::mulScalar(susDataProcessed->susVecTot.value, omegaParallel, fusedRotRateParallel, 3); } else { - estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData); + estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateSourcesData); // store for calculation of angular acceleration if (gyrDataProcessed->gyrVecTot.isValid()) { std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double)); @@ -173,12 +254,18 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( VectorOperations::add(fusedRotRateParallel, fusedRotRateOrthogonal, fusedRotRateTotal); { - PoolReadGuard pg(fusedRotRateData); - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateOrthogonal, - 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateParallel.value, fusedRotRateParallel, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double)); - fusedRotRateData->setValidity(true, true); + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, fusedRotRateOrthogonal, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(true); + std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, fusedRotRateParallel, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(true); + std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, fusedRotRateTotal, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(true); + } } // store for calculation of angular acceleration @@ -188,31 +275,44 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( } void FusedRotationEstimation::estimateFusedRotationRateEclipse( - acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) { + acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) { if (not acsParameters->onBoardParams.fusedRateSafeDuringEclipse or not gyrDataProcessed->gyrVecTot.isValid() or - VectorOperations::norm(fusedRotRateData->rotRateTotal.value, 3) == 0) { + VectorOperations::norm(fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3) == 0) { { - PoolReadGuard pg(fusedRotRateData); - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double)); - fusedRotRateData->setValidity(false, true); + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(false); + std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(false); + std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(false); + } } return; } double angAccelB[3] = {0, 0, 0}; VectorOperations::subtract(gyrDataProcessed->gyrVecTot.value, rotRateOldB, angAccelB, 3); double fusedRotRateTotal[3] = {0, 0, 0}; - VectorOperations::add(fusedRotRateData->rotRateTotal.value, angAccelB, fusedRotRateTotal, - 3); + VectorOperations::add(fusedRotRateSourcesData->rotRateTotalSusMgm.value, angAccelB, + fusedRotRateTotal, 3); { - PoolReadGuard pg(fusedRotRateData); - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); - fusedRotRateData->rotRateOrthogonal.setValid(false); - std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); - fusedRotRateData->rotRateParallel.setValid(false); - std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double)); - fusedRotRateData->rotRateTotal.setValid(true); + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(false); + std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(false); + std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, fusedRotRateTotal, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(true); + } } } diff --git a/mission/controller/acs/FusedRotationEstimation.h b/mission/controller/acs/FusedRotationEstimation.h index 2c4c1588..2fc2ab8f 100644 --- a/mission/controller/acs/FusedRotationEstimation.h +++ b/mission/controller/acs/FusedRotationEstimation.h @@ -36,7 +36,7 @@ class FusedRotationEstimation { acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData); void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed, - acsctrl::FusedRotRateData *fusedRotRateData); + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData); void estimateFusedRotationRateQuest(acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta, acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData); From c7ec9726c40474bb89001c8909e9df7081fd2e7f Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 24 Nov 2023 11:30:27 +0100 Subject: [PATCH 25/68] this should work --- mission/acs/defs.h | 7 +++ mission/controller/AcsController.cpp | 49 +++++++++++------ mission/controller/acs/AcsParameters.cpp | 53 ++++++++++++------- mission/controller/acs/AcsParameters.h | 1 + .../acs/FusedRotationEstimation.cpp | 7 +++ mission/controller/acs/control/PtgCtrl.cpp | 16 +++--- mission/controller/acs/control/PtgCtrl.h | 7 +-- 7 files changed, 94 insertions(+), 46 deletions(-) diff --git a/mission/acs/defs.h b/mission/acs/defs.h index d9ce3c6c..729e29a5 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -53,6 +53,13 @@ enum GpsSource : uint8_t { SPG4, }; +enum RotRateSource : uint8_t { + ALL_INVALID, + SUSMGM, + QUEST, + STR, +}; + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM; //! [EXPORT] : [COMMENT] The limits for the rotation in safe mode were violated. static constexpr Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM); diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 3990c3f2..c1c26299 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -169,7 +169,7 @@ void AcsController::performAttitudeControl() { attitudeEstimation.quest(&susDataProcessed, &mgmDataProcessed, &attitudeEstimationData); fusedRotationEstimation.estimateFusedRotationRate( &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues, - &attitudeEstimationData, timeDelta, &fusedRotRateData); + &attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData); result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &attitudeEstimationData, &acsParameters); @@ -377,22 +377,39 @@ void AcsController::performDetumble() { void AcsController::performPointingCtrl() { bool strValid = (sensorValues.strSet.caliQw.isValid() and sensorValues.strSet.caliQx.isValid() and sensorValues.strSet.caliQy.isValid() and sensorValues.strSet.caliQz.isValid()); - acs::ControlModeStrategy ptgCtrlStrat = - ptgCtrl.pointingCtrlStrategy(mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, - strValid, fusedRotRateData.rotRateTotal.isValid(), mekfEnabled, - acsParameters.strParameters.useStrForRotRate); + uint8_t useMekf = false; + switch (mode) { + case acs::PTG_IDLE: + useMekf = acsParameters.idleModeControllerParameters.useMekf; + break; + case acs::PTG_TARGET: + useMekf = acsParameters.targetModeControllerParameters.useMekf; + break; + case acs::PTG_TARGET_GS: + useMekf = acsParameters.gsTargetModeControllerParameters.useMekf; + break; + case acs::PTG_NADIR: + useMekf = acsParameters.nadirModeControllerParameters.useMekf; + break; + case acs::PTG_INERTIAL: + useMekf = acsParameters.inertialModeControllerParameters.useMekf; + break; + } + acs::ControlModeStrategy ptgCtrlStrat = ptgCtrl.pointingCtrlStrategy( + mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, strValid, + attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotal.isValid(), + fusedRotRateData.rotRateSource.isValid(), useMekf); - // if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) { - // ptgCtrlLostCounter++; - // if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) { - // triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION); - // ptgCtrlLostCounter = 0; - // } - // commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, - // cmdSpeedRws[0], - // cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], - // acsParameters.rwHandlingParameters.rampTime); - // } + if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) { + ptgCtrlLostCounter++; + if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) { + triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION); + ptgCtrlLostCounter = 0; + } + commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], + cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], + acsParameters.rwHandlingParameters.rampTime); + } uint8_t enableAntiStiction = true; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index ab5e301e..370d6edb 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -431,6 +431,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x9: parameterWrapper->set(idleModeControllerParameters.enableAntiStiction); break; + case 0xA: + parameterWrapper->set(idleModeControllerParameters.useMekf); + break; default: return INVALID_IDENTIFIER_ID; } @@ -468,36 +471,39 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->setVector(targetModeControllerParameters.refDirection); + parameterWrapper->set(targetModeControllerParameters.useMekf); break; case 0xB: - parameterWrapper->setVector(targetModeControllerParameters.refRotRate); + parameterWrapper->setVector(targetModeControllerParameters.refDirection); break; case 0xC: - parameterWrapper->setVector(targetModeControllerParameters.quatRef); + parameterWrapper->setVector(targetModeControllerParameters.refRotRate); break; case 0xD: - parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); + parameterWrapper->setVector(targetModeControllerParameters.quatRef); break; case 0xE: - parameterWrapper->set(targetModeControllerParameters.latitudeTgt); + parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); break; case 0xF: - parameterWrapper->set(targetModeControllerParameters.longitudeTgt); + parameterWrapper->set(targetModeControllerParameters.latitudeTgt); break; case 0x10: - parameterWrapper->set(targetModeControllerParameters.altitudeTgt); + parameterWrapper->set(targetModeControllerParameters.longitudeTgt); break; case 0x11: - parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); + parameterWrapper->set(targetModeControllerParameters.altitudeTgt); break; case 0x12: - parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); + parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); break; case 0x13: - parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); + parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); break; case 0x14: + parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); + break; + case 0x15: parameterWrapper->set(targetModeControllerParameters.blindRotRate); break; default: @@ -537,18 +543,21 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection); + parameterWrapper->set(gsTargetModeControllerParameters.useMekf); break; case 0xB: - parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax); + parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection); break; case 0xC: - parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); + parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax); break; case 0xD: - parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); + parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); break; case 0xE: + parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); + break; + case 0xF: parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt); break; default: @@ -588,15 +597,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->setVector(nadirModeControllerParameters.refDirection); + parameterWrapper->set(nadirModeControllerParameters.useMekf); break; case 0xB: - parameterWrapper->setVector(nadirModeControllerParameters.quatRef); + parameterWrapper->setVector(nadirModeControllerParameters.refDirection); break; case 0xC: - parameterWrapper->setVector(nadirModeControllerParameters.refRotRate); + parameterWrapper->setVector(nadirModeControllerParameters.quatRef); break; case 0xD: + parameterWrapper->setVector(nadirModeControllerParameters.refRotRate); + break; + case 0xE: parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax); break; default: @@ -636,12 +648,15 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat); + parameterWrapper->set(inertialModeControllerParameters.useMekf); break; case 0xB: - parameterWrapper->setVector(inertialModeControllerParameters.refRotRate); + parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat); break; case 0xC: + parameterWrapper->setVector(inertialModeControllerParameters.refRotRate); + break; + case 0xD: parameterWrapper->setVector(inertialModeControllerParameters.quatRef); break; default: diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 7c1b2de6..ba3efedd 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -863,6 +863,7 @@ class AcsParameters : public HasParametersIF { double deSatGainFactor = 1000; uint8_t desatOn = true; uint8_t enableAntiStiction = true; + uint8_t useMekf = false; } pointingLawParameters; struct IdleModeControllerParameters : PointingLawParameters { diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index a74b6082..403639aa 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -26,6 +26,8 @@ void FusedRotationEstimation::estimateFusedRotationRate( std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateSourcesData->rotRateTotalStr.value, 3 * sizeof(double)); fusedRotRateData->rotRateOrthogonal.setValid(true); + fusedRotRateData->rotRateSource.value = acs::RotRateSource::STR; + fusedRotRateData->rotRateSource.setValid(true); } } else if (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and acsParameters->onBoardParams.fusedRateFromQuest) { @@ -38,6 +40,8 @@ void FusedRotationEstimation::estimateFusedRotationRate( std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateSourcesData->rotRateTotalQuest.value, 3 * sizeof(double)); fusedRotRateData->rotRateOrthogonal.setValid(true); + fusedRotRateData->rotRateSource.value = acs::RotRateSource::QUEST; + fusedRotRateData->rotRateSource.setValid(true); } } else if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) { std::memcpy(fusedRotRateData->rotRateOrthogonal.value, @@ -51,12 +55,15 @@ void FusedRotationEstimation::estimateFusedRotationRate( std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double)); fusedRotRateData->rotRateOrthogonal.setValid(true); + fusedRotRateData->rotRateSource.value = acs::RotRateSource::SUSMGM; + fusedRotRateData->rotRateSource.setValid(true); } else { PoolReadGuard pg(fusedRotRateData); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->rotRateSource.value = acs::RotRateSource::ALL_INVALID; fusedRotRateData->setValidity(false, true); } } diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index cf722f35..55127b1d 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -10,17 +10,17 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_ PtgCtrl::~PtgCtrl() {} -acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(const bool magFieldValid, - const bool mekfValid, const bool strValid, - const bool fusedRateValid, - const uint8_t mekfEnabled, - const uint8_t strRateEnabled) { +acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy( + const bool magFieldValid, const bool mekfValid, const bool strValid, const bool questValid, + const bool fusedRateValid, const uint8_t rotRateSource, const uint8_t mekfEnabled) { if (not magFieldValid) { return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (mekfEnabled and mekfValid) { - return acs::ControlModeStrategy::PTGCTRL_ACTIVE_MEKF; - } else if (strValid and strRateEnabled and fusedRateValid) { - return acs::ControlModeStrategy::PTGCTRL_WITHOUT_MEKF; + return acs::ControlModeStrategy::PTGCTRL_MEKF; + } else if (strValid and fusedRateValid and rotRateSource > acs::RotRateSource::SUSMGM) { + return acs::ControlModeStrategy::PTGCTRL_STR; + } else if (questValid and fusedRateValid and rotRateSource > acs::RotRateSource::SUSMGM) { + return acs::ControlModeStrategy::PTGCTRL_QUEST; } else { return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index b484f3c9..a07849ec 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -23,9 +23,10 @@ class PtgCtrl { virtual ~PtgCtrl(); acs::ControlModeStrategy pointingCtrlStrategy(const bool magFieldValid, const bool mekfValid, - const bool strValid, const bool fusedRateValid, - const uint8_t mekfEnabled, - const uint8_t strRateEnabled); + const bool strValid, const bool questValid, + const bool fusedRateValid, + const uint8_t rotRateSource, + const uint8_t mekfEnabled); /* @brief: Calculates the needed torque for the pointing control mechanism */ From 7ef55dcab158a5be2077289b2c6a31ef75c1d627 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 24 Nov 2023 11:32:15 +0100 Subject: [PATCH 26/68] small fix --- mission/controller/AcsController.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index c1c26299..1a47076f 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -409,6 +409,8 @@ void AcsController::performPointingCtrl() { commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], acsParameters.rwHandlingParameters.rampTime); + } else { + ptgCtrlLostCounter = 0; } uint8_t enableAntiStiction = true; From f0247a9ab31b0cd887ec24f27d2165b0c61ba79f Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 24 Nov 2023 11:45:10 +0100 Subject: [PATCH 27/68] select the according quaternion and rotational rate --- mission/controller/AcsController.cpp | 69 +++++++++++++++++----------- 1 file changed, 42 insertions(+), 27 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 1a47076f..d75a7b96 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -409,10 +409,30 @@ void AcsController::performPointingCtrl() { commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], acsParameters.rwHandlingParameters.rampTime); + return; } else { ptgCtrlLostCounter = 0; } + double quatBI[4] = {0, 0, 0, 0}, rotRateB[3] = {0, 0, 0}; + switch (ptgCtrlStrat) { + case acs::ControlModeStrategy::PTGCTRL_MEKF: + std::memcpy(quatBI, attitudeEstimationData.quatMekf.value, sizeof(quatBI)); + std::memcpy(rotRateB, attitudeEstimationData.satRotRateMekf.value, sizeof(rotRateB)); + break; + case acs::ControlModeStrategy::PTGCTRL_STR: + quatBI[0] = sensorValues.strSet.caliQx.value; + quatBI[1] = sensorValues.strSet.caliQy.value; + quatBI[2] = sensorValues.strSet.caliQz.value; + quatBI[3] = sensorValues.strSet.caliQw.value; + std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB)); + break; + case acs::ControlModeStrategy::PTGCTRL_QUEST: + std::memcpy(quatBI, attitudeEstimationData.quatQuest.value, sizeof(quatBI)); + std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB)); + break; + } + uint8_t enableAntiStiction = true; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); @@ -439,9 +459,8 @@ void AcsController::performPointingCtrl() { case acs::PTG_IDLE: guidance.targetQuatPtgSun(timeDelta, susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); - guidance.comparePtg(attitudeEstimationData.quatMekf.value, - attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate, - errorQuat, errorSatRotRate, errorAngle); + guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat, + errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters, @@ -452,9 +471,9 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, - sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); + mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, + sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, + sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction; break; @@ -462,8 +481,7 @@ void AcsController::performPointingCtrl() { guidance.targetQuatPtgThreeAxes(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate); - guidance.comparePtg(attitudeEstimationData.quatMekf.value, - attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate, + guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, acsParameters.targetModeControllerParameters.quatRef, acsParameters.targetModeControllerParameters.refRotRate, errorQuat, errorSatRotRate, errorAngle); @@ -477,18 +495,17 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, - sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); + mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, + sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, + sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; break; case acs::PTG_TARGET_GS: guidance.targetQuatPtgGs(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); - guidance.comparePtg(attitudeEstimationData.quatMekf.value, - attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate, - errorQuat, errorSatRotRate, errorAngle); + guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat, + errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters, @@ -499,9 +516,9 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, - sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); + mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, + sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, + sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction; break; @@ -509,8 +526,7 @@ void AcsController::performPointingCtrl() { guidance.targetQuatPtgNadirThreeAxes( timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate); - guidance.comparePtg(attitudeEstimationData.quatMekf.value, - attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate, + guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef, acsParameters.nadirModeControllerParameters.refRotRate, errorQuat, errorSatRotRate, errorAngle); @@ -524,17 +540,16 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, - sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); + mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, + sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, + sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; break; case acs::PTG_INERTIAL: std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, sizeof(targetQuat)); - guidance.comparePtg(attitudeEstimationData.quatMekf.value, - attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate, + guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef, acsParameters.inertialModeControllerParameters.refRotRate, errorQuat, errorSatRotRate, errorAngle); @@ -548,9 +563,9 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, - sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); + mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, + sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, + sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; break; default: From 2f3335403b2d2d60a52dd58ff417a54a7988eb83 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 27 Nov 2023 10:37:40 +0100 Subject: [PATCH 28/68] oh my poggers --- mission/acs/defs.h | 11 +++++++---- mission/controller/AcsController.cpp | 4 ++++ mission/controller/acs/FusedRotationEstimation.cpp | 8 ++++---- mission/controller/acs/Navigation.cpp | 7 ++++--- mission/controller/acs/SensorProcessing.cpp | 8 ++++---- mission/controller/acs/control/PtgCtrl.cpp | 4 ++-- 6 files changed, 25 insertions(+), 17 deletions(-) diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 729e29a5..1cd55dda 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -45,20 +45,23 @@ enum ControlModeStrategy : uint8_t { PTGCTRL_STR = 101, PTGCTRL_QUEST = 102, }; - -enum GpsSource : uint8_t { +namespace gps { +enum Source : uint8_t { NONE, GPS, GPS_EXTRAPOLATED, SPG4, }; +} -enum RotRateSource : uint8_t { - ALL_INVALID, +namespace rotrate { +enum Source : uint8_t { + NONE, SUSMGM, QUEST, STR, }; +} static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM; //! [EXPORT] : [COMMENT] The limits for the rotation in safe mode were violated. diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index d75a7b96..8aa7b41c 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -431,6 +431,10 @@ void AcsController::performPointingCtrl() { std::memcpy(quatBI, attitudeEstimationData.quatQuest.value, sizeof(quatBI)); std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB)); break; + default: + sif::error << "AcsController: Invalid pointing mode strategy for performDetumble" + << std::endl; + break; } uint8_t enableAntiStiction = true; diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index 403639aa..c936bc16 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -26,7 +26,7 @@ void FusedRotationEstimation::estimateFusedRotationRate( std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateSourcesData->rotRateTotalStr.value, 3 * sizeof(double)); fusedRotRateData->rotRateOrthogonal.setValid(true); - fusedRotRateData->rotRateSource.value = acs::RotRateSource::STR; + fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR; fusedRotRateData->rotRateSource.setValid(true); } } else if (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and @@ -40,7 +40,7 @@ void FusedRotationEstimation::estimateFusedRotationRate( std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateSourcesData->rotRateTotalQuest.value, 3 * sizeof(double)); fusedRotRateData->rotRateOrthogonal.setValid(true); - fusedRotRateData->rotRateSource.value = acs::RotRateSource::QUEST; + fusedRotRateData->rotRateSource.value = acs::rotrate::Source::QUEST; fusedRotRateData->rotRateSource.setValid(true); } } else if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) { @@ -55,7 +55,7 @@ void FusedRotationEstimation::estimateFusedRotationRate( std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double)); fusedRotRateData->rotRateOrthogonal.setValid(true); - fusedRotRateData->rotRateSource.value = acs::RotRateSource::SUSMGM; + fusedRotRateData->rotRateSource.value = acs::rotrate::Source::SUSMGM; fusedRotRateData->rotRateSource.setValid(true); } else { PoolReadGuard pg(fusedRotRateData); @@ -63,7 +63,7 @@ void FusedRotationEstimation::estimateFusedRotationRate( std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); - fusedRotRateData->rotRateSource.value = acs::RotRateSource::ALL_INVALID; + fusedRotRateData->rotRateSource.value = acs::rotrate::Source::NONE; fusedRotRateData->setValidity(false, true); } } diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 8c3b1dd9..624f4cea 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -16,7 +16,8 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters) { + acsctrl::AttitudeEstimationData *mekfData, + AcsParameters *acsParameters) { double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; bool quatIBValid = sensorValues->strSet.isTrustWorthy.value; @@ -54,7 +55,7 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat { PoolReadGuard pg(gpsDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - gpsDataProcessed->source = acs::GpsSource::SPG4; + gpsDataProcessed->source = acs::gps::Source::SPG4; gpsDataProcessed->source.setValid(true); std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double)); gpsDataProcessed->gpsPosition.setValid(true); @@ -66,7 +67,7 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat { PoolReadGuard pg(gpsDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - gpsDataProcessed->source = acs::GpsSource::NONE; + gpsDataProcessed->source = acs::gps::Source::NONE; gpsDataProcessed->source.setValid(true); std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double)); gpsDataProcessed->gpsPosition.setValid(false); diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 56217a8f..0390d7f1 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -15,7 +15,7 @@ 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) { + if (gpsDataProcessed->source.value != acs::gps::Source::NONE) { Igrf13Model igrf13; igrf13.schmidtNormalization(); igrf13.updateCoeffGH(timeAbsolute); @@ -526,9 +526,9 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong // init variables double gdLongitude = 0, gdLatitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0}; - uint8_t gpsSource = acs::GpsSource::NONE; + uint8_t gpsSource = acs::gps::Source::NONE; // We do not trust the GPS and therefore it shall die here if SPG4 is running - if (gpsDataProcessed->source.value == acs::GpsSource::SPG4 and gpsParameters->useSpg4) { + if (gpsDataProcessed->source.value == acs::gps::Source::SPG4 and gpsParameters->useSpg4) { MathOperations::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value, gdLatitude, gdLongitude, altitude); double factor = 1 - pow(ECCENTRICITY_WGS84, 2); @@ -571,7 +571,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong validSavedPosSatE = true; - gpsSource = acs::GpsSource::GPS; + gpsSource = acs::gps::Source::GPS; } { PoolReadGuard pg(gpsDataProcessed); diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 55127b1d..eb510c53 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -17,9 +17,9 @@ acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy( return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (mekfEnabled and mekfValid) { return acs::ControlModeStrategy::PTGCTRL_MEKF; - } else if (strValid and fusedRateValid and rotRateSource > acs::RotRateSource::SUSMGM) { + } else if (strValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) { return acs::ControlModeStrategy::PTGCTRL_STR; - } else if (questValid and fusedRateValid and rotRateSource > acs::RotRateSource::SUSMGM) { + } else if (questValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) { return acs::ControlModeStrategy::PTGCTRL_QUEST; } else { return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; From 70be396b622c51583c65b0e112f4694c2f404fa1 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 27 Nov 2023 10:41:21 +0100 Subject: [PATCH 29/68] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index cc3e64e7..74775bb5 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit cc3e64e70d90f6a2b5c59215b2569c1771e890f0 +Subproject commit 74775bb59d76ac631b9649bd069389ab38213df4 From 14e56aa5d0fa268d84cbb01c9779185bfc22a105 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 27 Nov 2023 10:52:30 +0100 Subject: [PATCH 30/68] low pass for quest --- mission/controller/acs/AcsParameters.cpp | 3 +++ mission/controller/acs/AcsParameters.h | 1 + mission/controller/acs/AttitudeEstimation.cpp | 5 +++++ mission/controller/acs/AttitudeEstimation.h | 2 ++ 4 files changed, 11 insertions(+) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 370d6edb..a6ce6c5d 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -35,6 +35,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x4: parameterWrapper->set(onBoardParams.fusedRateFromQuest); break; + case 0x5: + parameterWrapper->set(onBoardParams.questFilterWeight); + break; default: return INVALID_IDENTIFIER_ID; } diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index ba3efedd..598fecb3 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -22,6 +22,7 @@ class AcsParameters : public HasParametersIF { uint8_t fusedRateSafeDuringEclipse = true; uint8_t fusedRateFromStr = false; uint8_t fusedRateFromQuest = false; + double questFilterWeight = 0.0; } onBoardParams; struct InertiaEIVE { diff --git a/mission/controller/acs/AttitudeEstimation.cpp b/mission/controller/acs/AttitudeEstimation.cpp index 45bd9461..02e122cf 100644 --- a/mission/controller/acs/AttitudeEstimation.cpp +++ b/mission/controller/acs/AttitudeEstimation.cpp @@ -86,6 +86,11 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData, VectorOperations::mulScalar(qBI, constMinus, qBI, 3); QuaternionOperations::normalize(qBI, qBI); + + // Low Pass + if (VectorOperations::norm(qOld, 4) != 0.0) { + QuaternionOperations::slerp(qBI, qOld, acsParameters->onBoardParams.questFilterWeight, qBI); + } } { PoolReadGuard pg{attitudeEstimation}; diff --git a/mission/controller/acs/AttitudeEstimation.h b/mission/controller/acs/AttitudeEstimation.h index 99df8091..8f307cc4 100644 --- a/mission/controller/acs/AttitudeEstimation.h +++ b/mission/controller/acs/AttitudeEstimation.h @@ -23,6 +23,8 @@ class AttitudeEstimation { private: AcsParameters *acsParameters; + double qOld[4] = {0, 0, 0, 0}; + static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0}; }; From e3271b6b4daf8f66de21c09812f20953016ffe46 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 27 Nov 2023 11:15:39 +0100 Subject: [PATCH 31/68] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 74775bb5..8dfc84c0 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 74775bb59d76ac631b9649bd069389ab38213df4 +Subproject commit 8dfc84c0b57aabc94e9819bdeeb69cd864ced093 From bd71446e051ea84550e187ee07fdae4763c1022e Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 29 Nov 2023 10:42:36 +0100 Subject: [PATCH 32/68] changelog --- CHANGELOG.md | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 74bd77e3..2b7e6eef 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,7 +14,11 @@ will consitute of a breaking change warranting a new major release: - The TMTC interface changes in any shape of form. - The behaviour of the OBSW changes in a major shape or form relevant for operations -# [unreleased] +# [v7.4.0] 2023-12-XX + +## Changed + +- ACS-Board default side changed to B-Side # [v7.3.0] 2023-11-07 From 6ed3dc1b50f8e2d42008197397122a837b6eab16 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 29 Nov 2023 10:43:10 +0100 Subject: [PATCH 33/68] one of those days ... --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 2b7e6eef..2c7e549c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,7 +14,7 @@ will consitute of a breaking change warranting a new major release: - The TMTC interface changes in any shape of form. - The behaviour of the OBSW changes in a major shape or form relevant for operations -# [v7.4.0] 2023-12-XX +# [v7.5.0] 2023-12-XX ## Changed From 3c8b0d1a7139b141ba190ad1ab840bf48694112c Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 29 Nov 2023 16:58:43 +0100 Subject: [PATCH 34/68] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index cc3e64e7..c906acd6 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit cc3e64e70d90f6a2b5c59215b2569c1771e890f0 +Subproject commit c906acd65960d400e7766fc12947815551d10a1a From 370eff5204fda3cae2088ced269569b3eaa8df16 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 29 Nov 2023 16:59:03 +0100 Subject: [PATCH 35/68] get correct sd card --- bsp_q7s/em/emObjectFactory.cpp | 2 +- bsp_q7s/fmObjectFactory.cpp | 2 +- linux/ObjectFactory.cpp | 5 +-- linux/ObjectFactory.h | 3 +- mission/acs/defs.h | 2 ++ mission/controller/AcsController.cpp | 49 ++++++++++++++++------------ mission/controller/AcsController.h | 16 ++++++--- 7 files changed, 49 insertions(+), 30 deletions(-) diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 41dab3e6..bcba1331 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -175,7 +175,7 @@ void ObjectFactory::produce(void* args) { createScexComponents(q7s::UART_SCEX_DEV, pwrSwitcher, *SdCardManager::instance(), false, power::Switches::PDU1_CH5_SOLAR_CELL_EXP_5V); #endif - createAcsController(true, enableHkSets); + createAcsController(true, enableHkSets, *SdCardManager::instance()); HeaterHandler* heaterHandler; createHeaterComponents(gpioComIF, pwrSwitcher, healthTable, heaterHandler); createThermalController(*heaterHandler, true); diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index b154ac52..118b583e 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -130,6 +130,6 @@ void ObjectFactory::produce(void* args) { createMiscComponents(); createThermalController(*heaterHandler, false); - createAcsController(true, enableHkSets); + createAcsController(true, enableHkSets, *SdCardManager::instance()); satsystem::init(false); } diff --git a/linux/ObjectFactory.cpp b/linux/ObjectFactory.cpp index 756dc0af..4956c1d6 100644 --- a/linux/ObjectFactory.cpp +++ b/linux/ObjectFactory.cpp @@ -335,8 +335,9 @@ void ObjectFactory::createScexComponents(std::string uartDev, PowerSwitchIF* pwr scexHandler->connectModeTreeParent(satsystem::payload::SUBSYSTEM); } -AcsController* ObjectFactory::createAcsController(bool connectSubsystem, bool enableHkSets) { - auto acsCtrl = new AcsController(objects::ACS_CONTROLLER, enableHkSets); +AcsController* ObjectFactory::createAcsController(bool connectSubsystem, bool enableHkSets, + SdCardMountedIF& mountedIF) { + auto acsCtrl = new AcsController(objects::ACS_CONTROLLER, enableHkSets, mountedIF); if (connectSubsystem) { acsCtrl->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM); } diff --git a/linux/ObjectFactory.h b/linux/ObjectFactory.h index 46baa685..b813d823 100644 --- a/linux/ObjectFactory.h +++ b/linux/ObjectFactory.h @@ -31,7 +31,8 @@ void createScexComponents(std::string uartDev, PowerSwitchIF* pwrSwitcher, void gpioChecker(ReturnValue_t result, std::string output); -AcsController* createAcsController(bool connectSubsystem, bool enableHkSets); +AcsController* createAcsController(bool connectSubsystem, bool enableHkSets, + SdCardMountedIF& mountedIF); PowerController* createPowerController(bool connectSubsystem, bool enableHkSets); } // namespace ObjectFactory diff --git a/mission/acs/defs.h b/mission/acs/defs.h index baed276d..ce308f7c 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -73,6 +73,8 @@ static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(6, severity::HIG static constexpr Event SAFE_MODE_CONTROLLER_FAILURE = MAKE_EVENT(7, severity::HIGH); //! [EXPORT] : [COMMENT] The TLE for the SGP4 Propagator has become too old. static constexpr Event TLE_TOO_OLD = MAKE_EVENT(8, severity::INFO); +//! [EXPORT] : [COMMENT] The TLE for the SGP4 Propagator has become too old. +static constexpr Event TLE_FILE_READ_FAILED = MAKE_EVENT(9, severity::LOW); extern const char* getModeStr(AcsMode mode); diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index de226515..7dce604e 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -1,12 +1,9 @@ #include "AcsController.h" -#include -#include -#include - -AcsController::AcsController(object_id_t objectId, bool enableHkSets) +AcsController::AcsController(object_id_t objectId, bool enableHkSets, SdCardMountedIF &sdcMan) : ExtendedControllerBase(objectId), enableHkSets(enableHkSets), + sdcMan(sdcMan), fusedRotationEstimation(&acsParameters), guidance(&acsParameters), safeCtrl(&acsParameters), @@ -1040,8 +1037,11 @@ ReturnValue_t AcsController::updateTle(const uint8_t *line1, const uint8_t *line } ReturnValue_t AcsController::writeTleToFs(const uint8_t *tle) { - // ToDo: check which SD card is active via sdcMan - std::string path = SD_0 + TLE_FILE; + auto mntPrefix = sdcMan.getCurrentMountPrefix(); + if (mntPrefix == nullptr or !sdcMan.isSdCardUsable(std::nullopt)) { + return returnvalue::FAILED; + } + std::string path = mntPrefix + TLE_FILE; // Clear existing TLE from file std::ofstream tleFile(path.c_str(), std::ofstream::out | std::ofstream::trunc); if (tleFile.is_open()) { @@ -1049,27 +1049,34 @@ ReturnValue_t AcsController::writeTleToFs(const uint8_t *tle) { tleFile << "\n"; tleFile.write(reinterpret_cast(tle + 69), 69); } else { - // return error + return WRITE_FILE_FAILED; } tleFile.close(); return returnvalue::OK; } ReturnValue_t AcsController::readTleFromFs(uint8_t *line1, uint8_t *line2) { - // ToDo: check which SD card is active via sdcMan - std::string path = SD_0 + TLE_FILE; - // Read existing TLE from file - std::fstream tleFile = std::fstream(path.c_str(), std::fstream::in); - if (tleFile.is_open()) { - std::string tleLine1, tleLine2; - getline(tleFile, tleLine1); - std::memcpy(line1, tleLine1.c_str(), 69); - getline(tleFile, tleLine2); - std::memcpy(line2, tleLine2.c_str(), 69); - } else { - // return error + auto mntPrefix = sdcMan.getCurrentMountPrefix(); + if (mntPrefix == nullptr or !sdcMan.isSdCardUsable(std::nullopt)) { + return returnvalue::FAILED; + } + std::string path = mntPrefix + TLE_FILE; + std::error_code e; + if (std::filesystem::exists(path, e)) { + // Read existing TLE from file + std::fstream tleFile = std::fstream(path.c_str(), std::fstream::in); + if (tleFile.is_open()) { + std::string tleLine1, tleLine2; + getline(tleFile, tleLine1); + std::memcpy(line1, tleLine1.c_str(), 69); + getline(tleFile, tleLine2); + std::memcpy(line2, tleLine2.c_str(), 69); + } else { + triggerEvent(acs::TLE_FILE_READ_FAILED); + return returnvalue::FAILED; + } + tleFile.close(); } - tleFile.close(); return returnvalue::OK; } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 9d91ae10..c2e4790f 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -4,15 +4,18 @@ #include #include #include +#include #include #include #include #include #include #include +#include #include #include #include +#include #include #include #include @@ -23,15 +26,18 @@ #include #include #include +#include #include +#include #include +#include class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF { public: static constexpr dur_millis_t INIT_DELAY = 500; - AcsController(object_id_t objectId, bool enableHkSets); + AcsController(object_id_t objectId, bool enableHkSets, SdCardMountedIF& sdcMan); MessageQueueId_t getCommandQueue() const; ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId, @@ -51,6 +57,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes bool enableHkSets = false; + SdCardMountedIF& sdcMan; + AcsParameters acsParameters; SensorProcessing sensorProcessing; FusedRotationEstimation fusedRotationEstimation; @@ -93,6 +101,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL; //! [EXPORT] : [COMMENT] File deletion failed and at least one file is still existent. static constexpr ReturnValue_t FILE_DELETION_FAILED = MAKE_RETURN_CODE(0); + //! [EXPORT] : [COMMENT] Writing the TLE to the file has failed. + static constexpr ReturnValue_t WRITE_FILE_FAILED = MAKE_RETURN_CODE(1); ReturnValue_t initialize() override; ReturnValue_t handleCommandMessage(CommandMessage* message) override; @@ -131,9 +141,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes ReturnValue_t writeTleToFs(const uint8_t* tle); ReturnValue_t readTleFromFs(uint8_t* line1, uint8_t* line2); - const std::string SD_0 = "/mnt/sd0/"; - const std::string SD_1 = "/mnt/sd1/"; - const std::string TLE_FILE = "conf/tle.txt"; + const std::string TLE_FILE = "/conf/tle.txt"; /* ACS Sensor Values */ ACS::SensorValues sensorValues; From 882c5ce5980c7246f7929efb399e2dd26827c580 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 29 Nov 2023 17:03:34 +0100 Subject: [PATCH 36/68] changelog --- CHANGELOG.md | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 74bd77e3..a3d12faa 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,7 +14,15 @@ will consitute of a breaking change warranting a new major release: - The TMTC interface changes in any shape of form. - The behaviour of the OBSW changes in a major shape or form relevant for operations -# [unreleased] +# [v7.5.0] 2023-12-XX + +## Changed + +- The TLE uploaded now gets stored in a file on the filesystem. It will always be stored on + the current active SD Card. After a reboot, the TLE will be read from the filesystem. + A filesystem change via `prefSD` on bootup, can lead to the TLE not being read, even + though it is there. +- Bumped `eive-fsfw` # [v7.3.0] 2023-11-07 From a481fc23f22e8a119f3880ab2efaf3a4d95445b0 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 29 Nov 2023 17:11:46 +0100 Subject: [PATCH 37/68] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index c906acd6..7187f2b5 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit c906acd65960d400e7766fc12947815551d10a1a +Subproject commit 7187f2b5cdfe163bf7ed1a8fab48900d69f4c8bf From f395c71fd08640807718e891291f4a407023305e Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 29 Nov 2023 17:12:10 +0100 Subject: [PATCH 38/68] changelog --- CHANGELOG.md | 1 - 1 file changed, 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index a3d12faa..a71a5863 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -22,7 +22,6 @@ will consitute of a breaking change warranting a new major release: the current active SD Card. After a reboot, the TLE will be read from the filesystem. A filesystem change via `prefSD` on bootup, can lead to the TLE not being read, even though it is there. -- Bumped `eive-fsfw` # [v7.3.0] 2023-11-07 From 7239b2e26d5ea68bd78aa636da318b677c69d6c1 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 30 Nov 2023 11:43:12 +0100 Subject: [PATCH 39/68] small fixes --- CHANGELOG.md | 2 ++ mission/system/acs/acsModeTree.cpp | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index ae47a8f7..cd298df1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,8 @@ will consitute of a breaking change warranting a new major release: - ACS-Board default side changed to B-Side +# [v7.4.0] 2023-12-XX + ## Changed - Rewrote the PLOC Supervisor Handler, which is now based on a new device handler base class. diff --git a/mission/system/acs/acsModeTree.cpp b/mission/system/acs/acsModeTree.cpp index 6429c0d4..018418a4 100644 --- a/mission/system/acs/acsModeTree.cpp +++ b/mission/system/acs/acsModeTree.cpp @@ -160,8 +160,8 @@ void buildOffSequence(Subsystem& ss, ModeListEntry& eh) { // Build OFF transition 1 iht(objects::IMTQ_ASSY, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); iht(objects::STR_ASSY, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); - iht(objects::ACS_BOARD_ASS, OFF, duallane::B_SIDE, ACS_TABLE_OFF_TRANS_1.second); - iht(objects::SUS_BOARD_ASS, OFF, duallane::A_SIDE, ACS_TABLE_OFF_TRANS_1.second); + iht(objects::ACS_BOARD_ASS, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); + iht(objects::SUS_BOARD_ASS, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); iht(objects::RW_ASSY, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); check(ss.addTable(TableEntry(ACS_TABLE_OFF_TRANS_1.first, &ACS_TABLE_OFF_TRANS_1.second)), ctxc); From c6d0357ac9732e3808a821c5a606b618565b03ea Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 1 Dec 2023 10:22:52 +0100 Subject: [PATCH 40/68] changelog fix --- CHANGELOG.md | 2 -- 1 file changed, 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index e1f06f95..a7dd8a6f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,8 +20,6 @@ will consitute of a breaking change warranting a new major release: - ACS-Board default side changed to B-Side -# [v7.4.0] 2023-12-XX - # [v7.4.0] 2023-11-30 - `eive-tmtc` v5.11.0 From c28ff551dbdf7061109512b14bf8b07b11459456 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 1 Dec 2023 11:41:35 +0100 Subject: [PATCH 41/68] small fix --- mission/acs/defs.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/acs/defs.h b/mission/acs/defs.h index ce308f7c..91030839 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -73,7 +73,7 @@ static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(6, severity::HIG static constexpr Event SAFE_MODE_CONTROLLER_FAILURE = MAKE_EVENT(7, severity::HIGH); //! [EXPORT] : [COMMENT] The TLE for the SGP4 Propagator has become too old. static constexpr Event TLE_TOO_OLD = MAKE_EVENT(8, severity::INFO); -//! [EXPORT] : [COMMENT] The TLE for the SGP4 Propagator has become too old. +//! [EXPORT] : [COMMENT] The TLE could not be read from the filesystem. static constexpr Event TLE_FILE_READ_FAILED = MAKE_EVENT(9, severity::LOW); extern const char* getModeStr(AcsMode mode); From d22a2abf64f6ae1edc61f3852ab56f2d3aa98f0a Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 1 Dec 2023 11:42:27 +0100 Subject: [PATCH 42/68] added action cmd to read tle from file --- mission/controller/AcsController.cpp | 11 +++++++++++ mission/controller/AcsController.h | 1 + 2 files changed, 12 insertions(+) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 7dce604e..2b6a1ffc 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -73,6 +73,17 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t } return HasActionsIF::EXECUTION_FINISHED; } + case (READ_TLE): { + uint8_t tle[69 * 2] = {}; + uint8_t line2[69] = {}; + ReturnValue_t result = readTleFromFs(tle, line2); + if (result != returnvalue::OK) { + return result; + } + std::memcpy(tle + 69, line2, 69); + actionHelper.reportData(commandedBy, actionId, tle, 69 * 2); + return EXECUTION_FINISHED; + } default: { return HasActionsIF::INVALID_ACTION_ID; } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index c2e4790f..4c0480bd 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -97,6 +97,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes static const DeviceCommandId_t RESET_MEKF = 0x1; static const DeviceCommandId_t RESTORE_MEKF_NONFINITE_RECOVERY = 0x2; static const DeviceCommandId_t UPDATE_TLE = 0x3; + static const DeviceCommandId_t READ_TLE = 0x4; static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL; //! [EXPORT] : [COMMENT] File deletion failed and at least one file is still existent. From b86ee21da0ab2171aaf9802e804f1239abbe2136 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 1 Dec 2023 12:56:31 +0100 Subject: [PATCH 43/68] fix for failed handling --- CHANGELOG.md | 1 + mission/controller/AcsController.cpp | 5 ++++- mission/controller/AcsController.h | 2 ++ 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5dd6ef7c..e0926cc4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -23,6 +23,7 @@ will consitute of a breaking change warranting a new major release: the current active SD Card. After a reboot, the TLE will be read from the filesystem. 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. # [v7.4.0] 2023-11-30 diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 2b6a1ffc..8bdc7d15 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -1084,9 +1084,12 @@ ReturnValue_t AcsController::readTleFromFs(uint8_t *line1, uint8_t *line2) { std::memcpy(line2, tleLine2.c_str(), 69); } else { triggerEvent(acs::TLE_FILE_READ_FAILED); - return returnvalue::FAILED; + return READ_FILE_FAILED; } tleFile.close(); + } else { + triggerEvent(acs::TLE_FILE_READ_FAILED); + return READ_FILE_FAILED; } return returnvalue::OK; } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 4c0480bd..4d80da3e 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -104,6 +104,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes static constexpr ReturnValue_t FILE_DELETION_FAILED = MAKE_RETURN_CODE(0); //! [EXPORT] : [COMMENT] Writing the TLE to the file has failed. static constexpr ReturnValue_t WRITE_FILE_FAILED = MAKE_RETURN_CODE(1); + //! [EXPORT] : [COMMENT] Reading the TLE to the file has failed. + static constexpr ReturnValue_t READ_FILE_FAILED = MAKE_RETURN_CODE(2); ReturnValue_t initialize() override; ReturnValue_t handleCommandMessage(CommandMessage* message) override; From 6d2bfbcfe6448af279c5a8e9a837500659d9e7e1 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 1 Dec 2023 14:20:11 +0100 Subject: [PATCH 44/68] this is cleaner --- mission/controller/PowerController.cpp | 31 ++++++++++---------------- mission/controller/PowerController.h | 2 +- 2 files changed, 13 insertions(+), 20 deletions(-) diff --git a/mission/controller/PowerController.cpp b/mission/controller/PowerController.cpp index f8a02c09..719ef507 100644 --- a/mission/controller/PowerController.cpp +++ b/mission/controller/PowerController.cpp @@ -158,6 +158,11 @@ ReturnValue_t PowerController::checkModeCommand(Mode_t mode, Submode_t submode, void PowerController::calculateStateOfCharge() { // get time 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,17 +281,14 @@ ReturnValue_t PowerController::calculateOpenCircuitVoltageCharge() { return returnvalue::OK; } -ReturnValue_t PowerController::calculateCoulombCounterCharge() { - double timeDiff = 0.0; - if (oldTime.tv_sec != 0) { - timeDiff = timevalOperations::toDouble(now - oldTime); - } else { +ReturnValue_t PowerController::calculateCoulombCounterCharge(double timeDelta) { + if (timeDelta == 0.0) { return returnvalue::FAILED; } - if (timeDiff > maxAllowedTimeDiff) { + if (timeDelta > maxAllowedTimeDiff) { // should not be a permanent state so no spam protection required - triggerEvent(power::TIMEDELTA_OUT_OF_BOUNDS, static_cast(timeDiff * 10)); - sif::error << "Power Controller::Time delta too large for Coulomb Counter: " << timeDiff + triggerEvent(power::TIMEDELTA_OUT_OF_BOUNDS, static_cast(timeDelta * 10)); + sif::error << "Power Controller::Time delta too large for Coulomb Counter: " << timeDelta << std::endl; return returnvalue::FAILED; } @@ -303,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; } diff --git a/mission/controller/PowerController.h b/mission/controller/PowerController.h index b7c3d1a0..6df60732 100644 --- a/mission/controller/PowerController.h +++ b/mission/controller/PowerController.h @@ -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); From aab705ca040baf148d853ca99045be640935955b Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 1 Dec 2023 14:22:10 +0100 Subject: [PATCH 45/68] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index e0926cc4..f763c678 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -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 From 08d0619b1112e5ec9006322568aa2880652c199c Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 1 Dec 2023 15:20:30 +0100 Subject: [PATCH 46/68] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 8dfc84c0..39785241 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 8dfc84c0b57aabc94e9819bdeeb69cd864ced093 +Subproject commit 39785241810e1b3d48ae139c941ef1eebe915883 From 064200e7300142fe096bf9daa719abf4be0f5c9f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 4 Dec 2023 09:58:37 +0100 Subject: [PATCH 47/68] yesyes clangd shush now --- mission/controller/acs/AcsParameters.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index a9c9ab5d..130d9a91 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -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; From 4ea1e16880ed4c75c0846d0f29ee71926444f735 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 4 Dec 2023 10:59:44 +0100 Subject: [PATCH 48/68] cpu goes brrrr --- mission/controller/acs/SensorProcessing.cpp | 6 ++---- mission/controller/acs/SensorProcessing.h | 2 ++ 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 56217a8f..975b38b1 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -1,5 +1,7 @@ #include "SensorProcessing.h" +#include + SensorProcessing::SensorProcessing() {} SensorProcessing::~SensorProcessing() {} @@ -16,12 +18,8 @@ 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) { - Igrf13Model igrf13; igrf13.schmidtNormalization(); igrf13.updateCoeffGH(timeAbsolute); - // ToDo - // 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, gpsDataProcessed->altitude.value, timeAbsolute, magIgrfModel); gpsValid = true; diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index fea0fd01..7caa29f8 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -35,6 +35,8 @@ class SensorProcessing { static constexpr double ZERO_VEC_D[3] = {0, 0, 0}; static constexpr double ECCENTRICITY_WGS84 = 0.0818195; + Igrf13Model igrf13; + protected: void processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid, const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid, From 41c8d7e0dd884f7a73983f4d4b6c3c2748e8137c Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 4 Dec 2023 11:15:08 +0100 Subject: [PATCH 49/68] replaced one ToDo with another one --- mission/controller/acs/SensorProcessing.cpp | 3 +++ mission/controller/acs/SensorProcessing.h | 2 -- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 975b38b1..23388e5f 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -18,6 +18,9 @@ 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(timeAbsolute); igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value, diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 7caa29f8..fea0fd01 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -35,8 +35,6 @@ class SensorProcessing { static constexpr double ZERO_VEC_D[3] = {0, 0, 0}; static constexpr double ECCENTRICITY_WGS84 = 0.0818195; - Igrf13Model igrf13; - protected: void processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid, const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid, From 9f5a198c5d99904743c9a25efb7cf8e9d0d33b17 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 4 Dec 2023 17:13:41 +0100 Subject: [PATCH 50/68] small fix --- mission/controller/acs/SensorProcessing.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index c454440a..43582f18 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -15,7 +15,7 @@ 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) { + if (gpsDataProcessed->source.value != acs::gps::Source::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; From fec4f64a07644b70b93318156b2161c5e0bf8dea Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 4 Dec 2023 17:54:21 +0100 Subject: [PATCH 51/68] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 39785241..5b661551 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 39785241810e1b3d48ae139c941ef1eebe915883 +Subproject commit 5b661551a813fa6a1d5f0c78889250965fc514fc From f22236b419e48755667d23af83f55c6fe8f01b0a Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 4 Dec 2023 18:07:22 +0100 Subject: [PATCH 52/68] fixed datasets --- mission/controller/AcsController.cpp | 2 ++ mission/controller/controllerdefinitions/AcsCtrlDefinitions.h | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 1c89de49..6adb4a0b 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -848,6 +848,8 @@ LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) { return &actuatorCmdData; case acsctrl::FUSED_ROTATION_RATE_DATA: return &fusedRotRateData; + case acsctrl::FUSED_ROTATION_RATE_SOURCES_DATA: + return &fusedRotRateSourcesData; default: return nullptr; } diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index 67041a3d..83a74552 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -307,7 +307,7 @@ class FusedRotRateData : public StaticLocalDataSet { private: }; -class FusedRotRateSourcesData : public StaticLocalDataSet { +class FusedRotRateSourcesData : public StaticLocalDataSet { public: FusedRotRateSourcesData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, FUSED_ROTATION_RATE_SOURCES_DATA) {} From a05fd758285e1ad2af4c4e05630a72f4a0e3c064 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 5 Dec 2023 13:12:42 +0100 Subject: [PATCH 53/68] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 5b661551..c648229c 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 5b661551a813fa6a1d5f0c78889250965fc514fc +Subproject commit c648229c999df81cb673d73e478a00dd5419f7d8 From dab10596f6a9935d07b1fd030514344c59419fc5 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 5 Dec 2023 15:02:17 +0100 Subject: [PATCH 54/68] fixes --- .../acs/FusedRotationEstimation.cpp | 30 ++++++++++++------- 1 file changed, 19 insertions(+), 11 deletions(-) diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index c936bc16..c2b69746 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -23,9 +23,9 @@ void FusedRotationEstimation::estimateFusedRotationRate( fusedRotRateData->rotRateOrthogonal.setValid(false); std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); fusedRotRateData->rotRateParallel.setValid(false); - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, + std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateSourcesData->rotRateTotalStr.value, 3 * sizeof(double)); - fusedRotRateData->rotRateOrthogonal.setValid(true); + fusedRotRateData->rotRateTotal.setValid(true); fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR; fusedRotRateData->rotRateSource.setValid(true); } @@ -37,9 +37,9 @@ void FusedRotationEstimation::estimateFusedRotationRate( fusedRotRateData->rotRateOrthogonal.setValid(false); std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); fusedRotRateData->rotRateParallel.setValid(false); - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, + std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateSourcesData->rotRateTotalQuest.value, 3 * sizeof(double)); - fusedRotRateData->rotRateOrthogonal.setValid(true); + fusedRotRateData->rotRateTotal.setValid(true); fusedRotRateData->rotRateSource.value = acs::rotrate::Source::QUEST; fusedRotRateData->rotRateSource.setValid(true); } @@ -52,9 +52,9 @@ void FusedRotationEstimation::estimateFusedRotationRate( fusedRotRateSourcesData->rotRateParallelSusMgm.value, 3 * sizeof(double)); fusedRotRateData->rotRateParallel.setValid( fusedRotRateSourcesData->rotRateParallelSusMgm.isValid()); - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, + std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double)); - fusedRotRateData->rotRateOrthogonal.setValid(true); + fusedRotRateData->rotRateTotal.setValid(true); fusedRotRateData->rotRateSource.value = acs::rotrate::Source::SUSMGM; fusedRotRateData->rotRateSource.setValid(true); } else { @@ -62,7 +62,7 @@ void FusedRotationEstimation::estimateFusedRotationRate( if (pg.getReadResult() == returnvalue::OK) { std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double)); fusedRotRateData->rotRateSource.value = acs::rotrate::Source::NONE; fusedRotRateData->setValidity(false, true); } @@ -82,11 +82,13 @@ void FusedRotationEstimation::estimateFusedRotationRateStr( QuaternionOperations::inverse(quatOldStr, quatOldInv); QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta); - QuaternionOperations::normalize(quatDelta); + if (VectorOperations::norm(quatDelta, 4) != 0.0) { + QuaternionOperations::normalize(quatDelta); + } double rotVec[3] = {0, 0, 0}; double angle = QuaternionOperations::getAngle(quatDelta); - if (angle == 0.0) { + if (VectorOperations::norm(quatDelta, 3) == 0.0) { { PoolReadGuard pg(fusedRotRateSourcesData); if (pg.getReadResult() == returnvalue::OK) { @@ -95,6 +97,8 @@ void FusedRotationEstimation::estimateFusedRotationRateStr( fusedRotRateSourcesData->rotRateTotalStr.setValid(true); } } + std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr)); + return; } VectorOperations::normalize(quatDelta, rotVec, 3); VectorOperations::mulScalar(rotVec, angle / timeDelta, rotVec, 3); @@ -139,11 +143,13 @@ void FusedRotationEstimation::estimateFusedRotationRateQuest( QuaternionOperations::inverse(quatOldQuest, quatOldInv); QuaternionOperations::multiply(attitudeEstimationData->quatQuest.value, quatOldInv, quatDelta); - QuaternionOperations::normalize(quatDelta); + if (VectorOperations::norm(quatDelta, 4) != 0.0) { + QuaternionOperations::normalize(quatDelta); + } double rotVec[3] = {0, 0, 0}; double angle = QuaternionOperations::getAngle(quatDelta); - if (angle == 0.0) { + if (VectorOperations::norm(quatDelta, 3) == 0.0) { { PoolReadGuard pg(fusedRotRateSourcesData); if (pg.getReadResult() == returnvalue::OK) { @@ -152,6 +158,8 @@ void FusedRotationEstimation::estimateFusedRotationRateQuest( fusedRotRateSourcesData->rotRateTotalQuest.setValid(true); } } + std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest)); + return; } VectorOperations::normalize(quatDelta, rotVec, 3); VectorOperations::mulScalar(rotVec, angle / timeDelta, rotVec, 3); From 6c6b7ff53ff2b643fbfc65cf54c739b9f5cb8acf Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 5 Dec 2023 15:11:22 +0100 Subject: [PATCH 55/68] this is less confusing --- mission/controller/acs/FusedRotationEstimation.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index c2b69746..d9fb3575 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -63,8 +63,9 @@ void FusedRotationEstimation::estimateFusedRotationRate( std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double)); - fusedRotRateData->rotRateSource.value = acs::rotrate::Source::NONE; fusedRotRateData->setValidity(false, true); + fusedRotRateData->rotRateSource.value = acs::rotrate::Source::NONE; + fusedRotRateData->rotRateSource.setValid(true); } } } From c6a0518515f2f9bd274b8eca3424ba8141e3df1e Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 6 Dec 2023 10:41:51 +0100 Subject: [PATCH 56/68] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index c648229c..7105e199 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit c648229c999df81cb673d73e478a00dd5419f7d8 +Subproject commit 7105e199c650303ac1a48e75aebc44182630931e From 71193495f30e207463e32b2e7922b4cd78d964fa Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 6 Dec 2023 14:36:23 +0100 Subject: [PATCH 57/68] welp i guess i also deleted stuff i still needed --- mission/controller/PowerController.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/controller/PowerController.cpp b/mission/controller/PowerController.cpp index 719ef507..8fb774dc 100644 --- a/mission/controller/PowerController.cpp +++ b/mission/controller/PowerController.cpp @@ -216,6 +216,7 @@ void PowerController::calculateStateOfCharge() { pwrCtrlCoreHk.coulombCounterCharge.setValid(false); } } + return; } // commit to dataset From 4cad1176d075d848bac64028422d1642b399ce36 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 6 Dec 2023 14:45:45 +0100 Subject: [PATCH 58/68] changelog --- CHANGELOG.md | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 9d4cb940..34a7850c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,7 +14,7 @@ will consitute of a breaking change warranting a new major release: - The TMTC interface changes in any shape of form. - The behaviour of the OBSW changes in a major shape or form relevant for operations -# [v7.5.0] 2023-12-XX +# [v7.5.0] 2023-12-06 ## Changed @@ -26,6 +26,13 @@ will consitute of a breaking change warranting a new major release: - 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. + +## Added + +- Higher ACS modes can now be entered without a running `MEKF`. Higher modes will collect their + quaternion and rotational rate depending on the available sources. +- `QUEST` attitude estimation was added to the `AcsController`. +- The fused rotational rate can now be estimated from `QUEST` and the `STR`. # [v7.4.1] 2023-12-06 From 8cd773d18b885feb54c7335b209f2b1befe35df1 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 6 Dec 2023 15:52:20 +0100 Subject: [PATCH 59/68] maybe this makes him happy --- mission/controller/acs/AttitudeEstimation.cpp | 183 +++++++++--------- .../acs/FusedRotationEstimation.cpp | 144 +++++++------- 2 files changed, 165 insertions(+), 162 deletions(-) diff --git a/mission/controller/acs/AttitudeEstimation.cpp b/mission/controller/acs/AttitudeEstimation.cpp index 02e122cf..d8f0bfe6 100644 --- a/mission/controller/acs/AttitudeEstimation.cpp +++ b/mission/controller/acs/AttitudeEstimation.cpp @@ -9,97 +9,8 @@ AttitudeEstimation::~AttitudeEstimation() {} void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData, acsctrl::MgmDataProcessed *mgmData, acsctrl::AttitudeEstimationData *attitudeEstimation) { - if (susData->susVecTot.isValid() and susData->sunIjkModel.isValid() and - mgmData->mgmVecTot.value and mgmData->magIgrfModel.isValid()) { - // Normalize Data - double normMgmB[3] = {0, 0, 0}, normMgmI[3] = {0, 0, 0}, normSusB[3] = {0, 0, 0}, - normSusI[3] = {0, 0, 0}; - VectorOperations::normalize(susData->susVecTot.value, normMgmB, 3); - VectorOperations::normalize(susData->sunIjkModel.value, normMgmI, 3); - VectorOperations::normalize(mgmData->mgmVecTot.value, normSusB, 3); - VectorOperations::normalize(mgmData->magIgrfModel.value, normSusI, 3); - - // Create Helper Vectors - double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0}, - helperSum[3] = {0, 0, 0}; - VectorOperations::cross(normSusB, normMgmB, normHelperB); - VectorOperations::cross(normSusI, normMgmI, normHelperI); - VectorOperations::normalize(normHelperB, normHelperB, 3); - VectorOperations::normalize(normHelperI, normHelperI, 3); - VectorOperations::cross(normHelperB, normHelperI, helperCross); - VectorOperations::add(normHelperB, normHelperI, helperSum, 3); - - // Sensor Weights - double kSus = 0, kMgm = 0; - kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSS, -2); - kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMAG, -2); - - // Weighted Vectors - double weightedSusB[3] = {0, 0, 0}, weightedMgmB[3] = {0, 0, 0}, kSusVec[3] = {0, 0, 0}, - kMgmVec[3] = {0, 0, 0}, kSumVec[3] = {0, 0, 0}; - VectorOperations::mulScalar(normSusB, kSus, weightedSusB, 3); - VectorOperations::mulScalar(normMgmB, kMgm, weightedMgmB, 3); - VectorOperations::cross(weightedSusB, normSusI, kSusVec); - VectorOperations::cross(weightedMgmB, normMgmI, kMgmVec); - VectorOperations::add(kSusVec, kMgmVec, kSumVec, 3); - - // Some weird Angles - double alpha = (1 + VectorOperations::dot(normHelperB, normHelperI)) * - (VectorOperations::dot(weightedSusB, normSusI) + - VectorOperations::dot(weightedMgmB, normMgmI)) + - VectorOperations::dot(helperCross, kSumVec); - double beta = VectorOperations::dot(helperSum, kSumVec); - double gamma = std::sqrt(std::pow(alpha, 2) + std::pow(beta, 2)); - - // I don't even know what this is supposed to be - double constPlus = - 1. / (2 * std::sqrt(gamma * (gamma + alpha) * - (1 + VectorOperations::dot(normHelperB, normHelperI)))); - double constMinus = - 1. / (2 * std::sqrt(gamma * (gamma - alpha) * - (1 + VectorOperations::dot(normHelperB, normHelperI)))); - - // Calculate Quaternion - double qBI[4] = {0, 0, 0, 0}, qRotVecTot[3] = {0, 0, 0}, qRotVecPt0[3] = {0, 0, 0}, - qRotVecPt1[3] = {0, 0, 0}; - if (alpha >= 0) { - // Scalar Part - qBI[3] = (gamma + alpha) * (1 + VectorOperations::dot(normHelperB, normHelperI)); - // Rotational Vector Part - VectorOperations::mulScalar(helperCross, gamma + alpha, qRotVecPt0, 3); - VectorOperations::add(normHelperB, normHelperI, qRotVecPt1, 3); - VectorOperations::mulScalar(qRotVecPt1, beta, qRotVecPt1, 3); - VectorOperations::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3); - std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot)); - - VectorOperations::mulScalar(qBI, constPlus, qBI, 3); - QuaternionOperations::normalize(qBI, qBI); - } else { - // Scalar Part - qBI[3] = (beta) * (1 + VectorOperations::dot(normHelperB, normHelperI)); - // Rotational Vector Part - VectorOperations::mulScalar(helperCross, beta, qRotVecPt0, 3); - VectorOperations::add(normHelperB, normHelperI, qRotVecPt1, 3); - VectorOperations::mulScalar(qRotVecPt1, gamma - alpha, qRotVecPt1, 3); - VectorOperations::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3); - std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot)); - - VectorOperations::mulScalar(qBI, constMinus, qBI, 3); - QuaternionOperations::normalize(qBI, qBI); - - // Low Pass - if (VectorOperations::norm(qOld, 4) != 0.0) { - QuaternionOperations::slerp(qBI, qOld, acsParameters->onBoardParams.questFilterWeight, qBI); - } - } - { - PoolReadGuard pg{attitudeEstimation}; - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(attitudeEstimation->quatQuest.value, qBI, 4 * sizeof(double)); - attitudeEstimation->quatQuest.setValid(true); - } - } - } else { + if (not(susData->susVecTot.isValid() and susData->sunIjkModel.isValid() and + mgmData->mgmVecTot.value and mgmData->magIgrfModel.isValid())) { { PoolReadGuard pg{attitudeEstimation}; if (pg.getReadResult() == returnvalue::OK) { @@ -107,5 +18,95 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData, attitudeEstimation->quatQuest.setValid(false); } } + return; + } + + // Normalize Data + double normMgmB[3] = {0, 0, 0}, normMgmI[3] = {0, 0, 0}, normSusB[3] = {0, 0, 0}, + normSusI[3] = {0, 0, 0}; + VectorOperations::normalize(susData->susVecTot.value, normMgmB, 3); + VectorOperations::normalize(susData->sunIjkModel.value, normMgmI, 3); + VectorOperations::normalize(mgmData->mgmVecTot.value, normSusB, 3); + VectorOperations::normalize(mgmData->magIgrfModel.value, normSusI, 3); + + // Create Helper Vectors + double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0}, + helperSum[3] = {0, 0, 0}; + VectorOperations::cross(normSusB, normMgmB, normHelperB); + VectorOperations::cross(normSusI, normMgmI, normHelperI); + VectorOperations::normalize(normHelperB, normHelperB, 3); + VectorOperations::normalize(normHelperI, normHelperI, 3); + VectorOperations::cross(normHelperB, normHelperI, helperCross); + VectorOperations::add(normHelperB, normHelperI, helperSum, 3); + + // Sensor Weights + double kSus = 0, kMgm = 0; + kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSS, -2); + kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMAG, -2); + + // Weighted Vectors + double weightedSusB[3] = {0, 0, 0}, weightedMgmB[3] = {0, 0, 0}, kSusVec[3] = {0, 0, 0}, + kMgmVec[3] = {0, 0, 0}, kSumVec[3] = {0, 0, 0}; + VectorOperations::mulScalar(normSusB, kSus, weightedSusB, 3); + VectorOperations::mulScalar(normMgmB, kMgm, weightedMgmB, 3); + VectorOperations::cross(weightedSusB, normSusI, kSusVec); + VectorOperations::cross(weightedMgmB, normMgmI, kMgmVec); + VectorOperations::add(kSusVec, kMgmVec, kSumVec, 3); + + // Some weird Angles + double alpha = (1 + VectorOperations::dot(normHelperB, normHelperI)) * + (VectorOperations::dot(weightedSusB, normSusI) + + VectorOperations::dot(weightedMgmB, normMgmI)) + + VectorOperations::dot(helperCross, kSumVec); + double beta = VectorOperations::dot(helperSum, kSumVec); + double gamma = std::sqrt(std::pow(alpha, 2) + std::pow(beta, 2)); + + // I don't even know what this is supposed to be + double constPlus = + 1. / (2 * std::sqrt(gamma * (gamma + alpha) * + (1 + VectorOperations::dot(normHelperB, normHelperI)))); + double constMinus = + 1. / (2 * std::sqrt(gamma * (gamma - alpha) * + (1 + VectorOperations::dot(normHelperB, normHelperI)))); + + // Calculate Quaternion + double qBI[4] = {0, 0, 0, 0}, qRotVecTot[3] = {0, 0, 0}, qRotVecPt0[3] = {0, 0, 0}, + qRotVecPt1[3] = {0, 0, 0}; + if (alpha >= 0) { + // Scalar Part + qBI[3] = (gamma + alpha) * (1 + VectorOperations::dot(normHelperB, normHelperI)); + // Rotational Vector Part + VectorOperations::mulScalar(helperCross, gamma + alpha, qRotVecPt0, 3); + VectorOperations::add(normHelperB, normHelperI, qRotVecPt1, 3); + VectorOperations::mulScalar(qRotVecPt1, beta, qRotVecPt1, 3); + VectorOperations::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3); + std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot)); + + VectorOperations::mulScalar(qBI, constPlus, qBI, 3); + QuaternionOperations::normalize(qBI, qBI); + } else { + // Scalar Part + qBI[3] = (beta) * (1 + VectorOperations::dot(normHelperB, normHelperI)); + // Rotational Vector Part + VectorOperations::mulScalar(helperCross, beta, qRotVecPt0, 3); + VectorOperations::add(normHelperB, normHelperI, qRotVecPt1, 3); + VectorOperations::mulScalar(qRotVecPt1, gamma - alpha, qRotVecPt1, 3); + VectorOperations::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3); + std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot)); + + VectorOperations::mulScalar(qBI, constMinus, qBI, 3); + QuaternionOperations::normalize(qBI, qBI); + + // Low Pass + if (VectorOperations::norm(qOld, 4) != 0.0) { + QuaternionOperations::slerp(qBI, qOld, acsParameters->onBoardParams.questFilterWeight, qBI); + } + } + { + PoolReadGuard pg{attitudeEstimation}; + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(attitudeEstimation->quatQuest.value, qBI, 4 * sizeof(double)); + attitudeEstimation->quatQuest.setValid(true); + } } } diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index d9fb3575..b4d0f0c4 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -73,51 +73,52 @@ void FusedRotationEstimation::estimateFusedRotationRate( void FusedRotationEstimation::estimateFusedRotationRateStr( ACS::SensorValues *sensorValues, const double timeDelta, acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) { - if ((sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and - sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid())) { - double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, - sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; - if (VectorOperations::norm(quatOldStr, 4) != 0 and timeDelta != 0) { - double quatOldInv[4] = {0, 0, 0, 0}; - double quatDelta[4] = {0, 0, 0, 0}; - - QuaternionOperations::inverse(quatOldStr, quatOldInv); - QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta); - if (VectorOperations::norm(quatDelta, 4) != 0.0) { - QuaternionOperations::normalize(quatDelta); + if (not(sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and + sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid())) { + { + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalStr.setValid(false); } + } + std::memcpy(quatOldStr, ZERO_VEC4, sizeof(quatOldStr)); + return; + } - double rotVec[3] = {0, 0, 0}; - double angle = QuaternionOperations::getAngle(quatDelta); - if (VectorOperations::norm(quatDelta, 3) == 0.0) { - { - PoolReadGuard pg(fusedRotRateSourcesData); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, - 3 * sizeof(double)); - fusedRotRateSourcesData->rotRateTotalStr.setValid(true); - } - } - std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr)); - return; - } - VectorOperations::normalize(quatDelta, rotVec, 3); - VectorOperations::mulScalar(rotVec, angle / timeDelta, rotVec, 3); + double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, + sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; + if (VectorOperations::norm(quatOldStr, 4) != 0 and timeDelta != 0) { + double quatOldInv[4] = {0, 0, 0, 0}; + double quatDelta[4] = {0, 0, 0, 0}; + + QuaternionOperations::inverse(quatOldStr, quatOldInv); + QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta); + if (VectorOperations::norm(quatDelta, 4) != 0.0) { + QuaternionOperations::normalize(quatDelta); + } + + double rotVec[3] = {0, 0, 0}; + double angle = QuaternionOperations::getAngle(quatDelta); + if (VectorOperations::norm(quatDelta, 3) == 0.0) { { PoolReadGuard pg(fusedRotRateSourcesData); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, rotVec, 3 * sizeof(double)); + std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, + 3 * sizeof(double)); fusedRotRateSourcesData->rotRateTotalStr.setValid(true); } } std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr)); return; } + VectorOperations::normalize(quatDelta, rotVec, 3); + VectorOperations::mulScalar(rotVec, angle / timeDelta, rotVec, 3); { PoolReadGuard pg(fusedRotRateSourcesData); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double)); - fusedRotRateSourcesData->rotRateTotalStr.setValid(false); + std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, rotVec, 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalStr.setValid(true); } } std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr)); @@ -130,50 +131,14 @@ void FusedRotationEstimation::estimateFusedRotationRateStr( fusedRotRateSourcesData->rotRateTotalStr.setValid(false); } } - std::memcpy(quatOldStr, ZERO_VEC4, sizeof(quatOldStr)); + std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr)); + return; } void FusedRotationEstimation::estimateFusedRotationRateQuest( acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta, acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) { - if (attitudeEstimationData->quatQuest.isValid()) { - if (VectorOperations::norm(quatOldQuest, 4) != 0 and timeDelta != 0) { - double quatOldInv[4] = {0, 0, 0, 0}; - double quatDelta[4] = {0, 0, 0, 0}; - - QuaternionOperations::inverse(quatOldQuest, quatOldInv); - QuaternionOperations::multiply(attitudeEstimationData->quatQuest.value, quatOldInv, - quatDelta); - if (VectorOperations::norm(quatDelta, 4) != 0.0) { - QuaternionOperations::normalize(quatDelta); - } - - double rotVec[3] = {0, 0, 0}; - double angle = QuaternionOperations::getAngle(quatDelta); - if (VectorOperations::norm(quatDelta, 3) == 0.0) { - { - PoolReadGuard pg(fusedRotRateSourcesData); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, - 3 * sizeof(double)); - fusedRotRateSourcesData->rotRateTotalQuest.setValid(true); - } - } - std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest)); - return; - } - VectorOperations::normalize(quatDelta, rotVec, 3); - VectorOperations::mulScalar(rotVec, angle / timeDelta, rotVec, 3); - { - PoolReadGuard pg(fusedRotRateSourcesData); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, rotVec, 3 * sizeof(double)); - fusedRotRateSourcesData->rotRateTotalQuest.setValid(true); - } - } - std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest)); - return; - } + if (not attitudeEstimationData->quatQuest.isValid()) { { PoolReadGuard pg(fusedRotRateSourcesData); if (pg.getReadResult() == returnvalue::OK) { @@ -182,6 +147,42 @@ void FusedRotationEstimation::estimateFusedRotationRateQuest( fusedRotRateSourcesData->rotRateTotalQuest.setValid(false); } } + std::memcpy(quatOldQuest, ZERO_VEC4, sizeof(quatOldQuest)); + } + + if (VectorOperations::norm(quatOldQuest, 4) != 0 and timeDelta != 0) { + double quatOldInv[4] = {0, 0, 0, 0}; + double quatDelta[4] = {0, 0, 0, 0}; + + QuaternionOperations::inverse(quatOldQuest, quatOldInv); + QuaternionOperations::multiply(attitudeEstimationData->quatQuest.value, quatOldInv, quatDelta); + if (VectorOperations::norm(quatDelta, 4) != 0.0) { + QuaternionOperations::normalize(quatDelta); + } + + double rotVec[3] = {0, 0, 0}; + double angle = QuaternionOperations::getAngle(quatDelta); + if (VectorOperations::norm(quatDelta, 3) == 0.0) { + { + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalQuest.setValid(true); + } + } + std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest)); + return; + } + VectorOperations::normalize(quatDelta, rotVec, 3); + VectorOperations::mulScalar(rotVec, angle / timeDelta, rotVec, 3); + { + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, rotVec, 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalQuest.setValid(true); + } + } std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest)); return; } @@ -192,7 +193,8 @@ void FusedRotationEstimation::estimateFusedRotationRateQuest( fusedRotRateSourcesData->rotRateTotalQuest.setValid(false); } } - std::memcpy(quatOldQuest, ZERO_VEC4, sizeof(quatOldQuest)); + std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest)); + return; } void FusedRotationEstimation::estimateFusedRotationRateSusMgm( From d762a2b703a65b370c87ac312a4e3670b4e2241a Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 6 Dec 2023 16:02:08 +0100 Subject: [PATCH 60/68] fix --- mission/controller/acs/AttitudeEstimation.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/mission/controller/acs/AttitudeEstimation.cpp b/mission/controller/acs/AttitudeEstimation.cpp index d8f0bfe6..e287b22f 100644 --- a/mission/controller/acs/AttitudeEstimation.cpp +++ b/mission/controller/acs/AttitudeEstimation.cpp @@ -96,11 +96,10 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData, VectorOperations::mulScalar(qBI, constMinus, qBI, 3); QuaternionOperations::normalize(qBI, qBI); - - // Low Pass - if (VectorOperations::norm(qOld, 4) != 0.0) { - QuaternionOperations::slerp(qBI, qOld, acsParameters->onBoardParams.questFilterWeight, qBI); - } + } + // Low Pass + if (VectorOperations::norm(qOld, 4) != 0.0) { + QuaternionOperations::slerp(qBI, qOld, acsParameters->onBoardParams.questFilterWeight, qBI); } { PoolReadGuard pg{attitudeEstimation}; From b4886822ebf0e62abfdf4bd5519b8fd53c5b9250 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 6 Dec 2023 17:20:20 +0100 Subject: [PATCH 61/68] re-run generators --- bsp_hosted/fsfwconfig/events/translateEvents.cpp | 11 +++++++---- bsp_hosted/fsfwconfig/objects/translateObjects.cpp | 2 +- generators/bsp_hosted_events.csv | 3 ++- generators/bsp_hosted_returnvalues.csv | 2 ++ generators/bsp_q7s_events.csv | 3 ++- generators/bsp_q7s_returnvalues.csv | 2 ++ generators/events/translateEvents.cpp | 11 +++++++---- generators/objects/translateObjects.cpp | 2 +- linux/fsfwconfig/events/translateEvents.cpp | 11 +++++++---- linux/fsfwconfig/objects/translateObjects.cpp | 2 +- tmtc | 2 +- 11 files changed, 33 insertions(+), 18 deletions(-) diff --git a/bsp_hosted/fsfwconfig/events/translateEvents.cpp b/bsp_hosted/fsfwconfig/events/translateEvents.cpp index 92401ab1..18184b50 100644 --- a/bsp_hosted/fsfwconfig/events/translateEvents.cpp +++ b/bsp_hosted/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 317 translations. + * @brief Auto-generated event translation file. Contains 318 translations. * @details - * Generated on: 2023-11-29 15:14:46 + * Generated on: 2023-12-06 17:19:38 */ #include "translateEvents.h" @@ -99,9 +99,10 @@ const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID"; const char *MEKF_INVALID_INFO_STRING = "MEKF_INVALID_INFO"; const char *MEKF_RECOVERY_STRING = "MEKF_RECOVERY"; const char *MEKF_AUTOMATIC_RESET_STRING = "MEKF_AUTOMATIC_RESET"; -const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION"; +const char *PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING = "PTG_CTRL_NO_ATTITUDE_INFORMATION"; const char *SAFE_MODE_CONTROLLER_FAILURE_STRING = "SAFE_MODE_CONTROLLER_FAILURE"; const char *TLE_TOO_OLD_STRING = "TLE_TOO_OLD"; +const char *TLE_FILE_READ_FAILED_STRING = "TLE_FILE_READ_FAILED"; const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT"; const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED"; const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED"; @@ -514,11 +515,13 @@ const char *translateEvents(Event event) { case (11205): return MEKF_AUTOMATIC_RESET_STRING; case (11206): - return MEKF_INVALID_MODE_VIOLATION_STRING; + return PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING; case (11207): return SAFE_MODE_CONTROLLER_FAILURE_STRING; case (11208): return TLE_TOO_OLD_STRING; + case (11209): + return TLE_FILE_READ_FAILED_STRING; case (11300): return SWITCH_CMD_SENT_STRING; case (11301): diff --git a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp index 834bdf41..f8058834 100644 --- a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp +++ b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 175 translations. - * Generated on: 2023-11-29 15:14:46 + * Generated on: 2023-12-06 17:19:38 */ #include "translateObjects.h" diff --git a/generators/bsp_hosted_events.csv b/generators/bsp_hosted_events.csv index 656843f1..97eeaf37 100644 --- a/generators/bsp_hosted_events.csv +++ b/generators/bsp_hosted_events.csv @@ -93,9 +93,10 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 11203;0x2bc3;MEKF_INVALID_INFO;INFO;MEKF was not able to compute a solution. P1: MEKF state on exit;mission/acs/defs.h 11204;0x2bc4;MEKF_RECOVERY;INFO;MEKF is able to compute a solution again.;mission/acs/defs.h 11205;0x2bc5;MEKF_AUTOMATIC_RESET;INFO;MEKF performed an automatic reset after detection of nonfinite values.;mission/acs/defs.h -11206;0x2bc6;MEKF_INVALID_MODE_VIOLATION;HIGH;MEKF was not able to compute a solution during any pointing ACS mode for a prolonged time.;mission/acs/defs.h +11206;0x2bc6;PTG_CTRL_NO_ATTITUDE_INFORMATION;HIGH;For a prolonged time, no attitude information was available for the Pointing Controller. Falling back to Safe Mode.;mission/acs/defs.h 11207;0x2bc7;SAFE_MODE_CONTROLLER_FAILURE;HIGH;The ACS safe mode controller was not able to compute a solution and has failed. P1: Missing information about magnetic field, P2: Missing information about rotational rate;mission/acs/defs.h 11208;0x2bc8;TLE_TOO_OLD;INFO;The TLE for the SGP4 Propagator has become too old.;mission/acs/defs.h +11209;0x2bc9;TLE_FILE_READ_FAILED;LOW;The TLE could not be read from the filesystem.;mission/acs/defs.h 11300;0x2c24;SWITCH_CMD_SENT;INFO;Indicates that a FSFW object requested setting a switch P1: 1 if on was requested, 0 for off | P2: Switch Index;mission/power/defs.h 11301;0x2c25;SWITCH_HAS_CHANGED;INFO;Indicated that a switch state has changed P1: New switch state, 1 for on, 0 for off | P2: Switch Index;mission/power/defs.h 11302;0x2c26;SWITCHING_Q7S_DENIED;MEDIUM;No description;mission/power/defs.h diff --git a/generators/bsp_hosted_returnvalues.csv b/generators/bsp_hosted_returnvalues.csv index abc43314..8bbb79f4 100644 --- a/generators/bsp_hosted_returnvalues.csv +++ b/generators/bsp_hosted_returnvalues.csv @@ -509,6 +509,8 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x67a3;SADPL_SwitchingDeplSa1Failed;No description;163;SA_DEPL_HANDLER;mission/SolarArrayDeploymentHandler.h 0x67a4;SADPL_SwitchingDeplSa2Failed;No description;164;SA_DEPL_HANDLER;mission/SolarArrayDeploymentHandler.h 0x6a00;ACSCTRL_FileDeletionFailed;File deletion failed and at least one file is still existent.;0;ACS_CTRL;mission/controller/AcsController.h +0x6a01;ACSCTRL_WriteFileFailed;Writing the TLE to the file has failed.;1;ACS_CTRL;mission/controller/AcsController.h +0x6a02;ACSCTRL_ReadFileFailed;Reading the TLE to the file has failed.;2;ACS_CTRL;mission/controller/AcsController.h 0x6b02;ACSMEKF_MekfUninitialized;No description;2;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6b03;ACSMEKF_MekfNoGyrData;No description;3;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6b04;ACSMEKF_MekfNoModelVectors;No description;4;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h diff --git a/generators/bsp_q7s_events.csv b/generators/bsp_q7s_events.csv index 656843f1..97eeaf37 100644 --- a/generators/bsp_q7s_events.csv +++ b/generators/bsp_q7s_events.csv @@ -93,9 +93,10 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 11203;0x2bc3;MEKF_INVALID_INFO;INFO;MEKF was not able to compute a solution. P1: MEKF state on exit;mission/acs/defs.h 11204;0x2bc4;MEKF_RECOVERY;INFO;MEKF is able to compute a solution again.;mission/acs/defs.h 11205;0x2bc5;MEKF_AUTOMATIC_RESET;INFO;MEKF performed an automatic reset after detection of nonfinite values.;mission/acs/defs.h -11206;0x2bc6;MEKF_INVALID_MODE_VIOLATION;HIGH;MEKF was not able to compute a solution during any pointing ACS mode for a prolonged time.;mission/acs/defs.h +11206;0x2bc6;PTG_CTRL_NO_ATTITUDE_INFORMATION;HIGH;For a prolonged time, no attitude information was available for the Pointing Controller. Falling back to Safe Mode.;mission/acs/defs.h 11207;0x2bc7;SAFE_MODE_CONTROLLER_FAILURE;HIGH;The ACS safe mode controller was not able to compute a solution and has failed. P1: Missing information about magnetic field, P2: Missing information about rotational rate;mission/acs/defs.h 11208;0x2bc8;TLE_TOO_OLD;INFO;The TLE for the SGP4 Propagator has become too old.;mission/acs/defs.h +11209;0x2bc9;TLE_FILE_READ_FAILED;LOW;The TLE could not be read from the filesystem.;mission/acs/defs.h 11300;0x2c24;SWITCH_CMD_SENT;INFO;Indicates that a FSFW object requested setting a switch P1: 1 if on was requested, 0 for off | P2: Switch Index;mission/power/defs.h 11301;0x2c25;SWITCH_HAS_CHANGED;INFO;Indicated that a switch state has changed P1: New switch state, 1 for on, 0 for off | P2: Switch Index;mission/power/defs.h 11302;0x2c26;SWITCHING_Q7S_DENIED;MEDIUM;No description;mission/power/defs.h diff --git a/generators/bsp_q7s_returnvalues.csv b/generators/bsp_q7s_returnvalues.csv index d2922a1c..e194ac67 100644 --- a/generators/bsp_q7s_returnvalues.csv +++ b/generators/bsp_q7s_returnvalues.csv @@ -593,6 +593,8 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x69c0;SPVRTVIF_BufTooSmall;No description;192;SUPV_RETURN_VALUES_IF;linux/payload/plocSupvDefs.h 0x69c1;SPVRTVIF_NoReplyTimeout;No description;193;SUPV_RETURN_VALUES_IF;linux/payload/plocSupvDefs.h 0x6a00;ACSCTRL_FileDeletionFailed;File deletion failed and at least one file is still existent.;0;ACS_CTRL;mission/controller/AcsController.h +0x6a01;ACSCTRL_WriteFileFailed;Writing the TLE to the file has failed.;1;ACS_CTRL;mission/controller/AcsController.h +0x6a02;ACSCTRL_ReadFileFailed;Reading the TLE to the file has failed.;2;ACS_CTRL;mission/controller/AcsController.h 0x6b02;ACSMEKF_MekfUninitialized;No description;2;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6b03;ACSMEKF_MekfNoGyrData;No description;3;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6b04;ACSMEKF_MekfNoModelVectors;No description;4;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index 92401ab1..18184b50 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 317 translations. + * @brief Auto-generated event translation file. Contains 318 translations. * @details - * Generated on: 2023-11-29 15:14:46 + * Generated on: 2023-12-06 17:19:38 */ #include "translateEvents.h" @@ -99,9 +99,10 @@ const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID"; const char *MEKF_INVALID_INFO_STRING = "MEKF_INVALID_INFO"; const char *MEKF_RECOVERY_STRING = "MEKF_RECOVERY"; const char *MEKF_AUTOMATIC_RESET_STRING = "MEKF_AUTOMATIC_RESET"; -const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION"; +const char *PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING = "PTG_CTRL_NO_ATTITUDE_INFORMATION"; const char *SAFE_MODE_CONTROLLER_FAILURE_STRING = "SAFE_MODE_CONTROLLER_FAILURE"; const char *TLE_TOO_OLD_STRING = "TLE_TOO_OLD"; +const char *TLE_FILE_READ_FAILED_STRING = "TLE_FILE_READ_FAILED"; const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT"; const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED"; const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED"; @@ -514,11 +515,13 @@ const char *translateEvents(Event event) { case (11205): return MEKF_AUTOMATIC_RESET_STRING; case (11206): - return MEKF_INVALID_MODE_VIOLATION_STRING; + return PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING; case (11207): return SAFE_MODE_CONTROLLER_FAILURE_STRING; case (11208): return TLE_TOO_OLD_STRING; + case (11209): + return TLE_FILE_READ_FAILED_STRING; case (11300): return SWITCH_CMD_SENT_STRING; case (11301): diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp index 2af38f77..279261c2 100644 --- a/generators/objects/translateObjects.cpp +++ b/generators/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 179 translations. - * Generated on: 2023-11-29 15:14:46 + * Generated on: 2023-12-06 17:19:38 */ #include "translateObjects.h" diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index 92401ab1..18184b50 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 317 translations. + * @brief Auto-generated event translation file. Contains 318 translations. * @details - * Generated on: 2023-11-29 15:14:46 + * Generated on: 2023-12-06 17:19:38 */ #include "translateEvents.h" @@ -99,9 +99,10 @@ const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID"; const char *MEKF_INVALID_INFO_STRING = "MEKF_INVALID_INFO"; const char *MEKF_RECOVERY_STRING = "MEKF_RECOVERY"; const char *MEKF_AUTOMATIC_RESET_STRING = "MEKF_AUTOMATIC_RESET"; -const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION"; +const char *PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING = "PTG_CTRL_NO_ATTITUDE_INFORMATION"; const char *SAFE_MODE_CONTROLLER_FAILURE_STRING = "SAFE_MODE_CONTROLLER_FAILURE"; const char *TLE_TOO_OLD_STRING = "TLE_TOO_OLD"; +const char *TLE_FILE_READ_FAILED_STRING = "TLE_FILE_READ_FAILED"; const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT"; const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED"; const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED"; @@ -514,11 +515,13 @@ const char *translateEvents(Event event) { case (11205): return MEKF_AUTOMATIC_RESET_STRING; case (11206): - return MEKF_INVALID_MODE_VIOLATION_STRING; + return PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING; case (11207): return SAFE_MODE_CONTROLLER_FAILURE_STRING; case (11208): return TLE_TOO_OLD_STRING; + case (11209): + return TLE_FILE_READ_FAILED_STRING; case (11300): return SWITCH_CMD_SENT_STRING; case (11301): diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp index 2af38f77..279261c2 100644 --- a/linux/fsfwconfig/objects/translateObjects.cpp +++ b/linux/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 179 translations. - * Generated on: 2023-11-29 15:14:46 + * Generated on: 2023-12-06 17:19:38 */ #include "translateObjects.h" diff --git a/tmtc b/tmtc index 74e55b16..58def422 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 74e55b16dcca73023254493d911be3debc36adb2 +Subproject commit 58def42281529f9694f5d997f2314af5bac19864 From 160159ff8df286184e9457034aa860b4cb55020e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 6 Dec 2023 17:21:52 +0100 Subject: [PATCH 62/68] bump eive-tmtc version --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 34a7850c..1ad30898 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,8 @@ will consitute of a breaking change warranting a new major release: # [v7.5.0] 2023-12-06 +- `eive-tmtc` v5.12.0 + ## Changed - ACS-Board default side changed to B-Side From 30f8c31ad0a9349f5c2b64e8e942ed48bf00a727 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 6 Dec 2023 17:23:25 +0100 Subject: [PATCH 63/68] repoint tmtc submodule --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 58def422..f63a834d 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 58def42281529f9694f5d997f2314af5bac19864 +Subproject commit f63a834d9acd4506f0dbd7f8e63007f17c063c1d From 87798f9e520712a551b8409f15faad3147f88213 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 6 Dec 2023 17:27:23 +0100 Subject: [PATCH 64/68] bump minor version --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 29d1ba23..e172ee9c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,8 +10,8 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR 7) -set(OBSW_VERSION_MINOR 4) -set(OBSW_VERSION_REVISION 1) +set(OBSW_VERSION_MINOR 5) +set(OBSW_VERSION_REVISION 0) # set(CMAKE_VERBOSE TRUE) From acf693636a4e1e85e9266564c884d2b010af369e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 6 Dec 2023 17:27:43 +0100 Subject: [PATCH 65/68] auto-formatting --- mission/controller/acs/AcsParameters.h | 4 ++-- mission/controller/acs/MultiplicativeKalmanFilter.cpp | 5 +++-- mission/controller/acs/MultiplicativeKalmanFilter.h | 8 ++++---- mission/controller/acs/Navigation.h | 4 ++-- mission/controller/acs/control/Detumble.cpp | 6 +++--- mission/controller/acs/control/Detumble.h | 4 ++-- mission/controller/acs/control/SafeCtrl.cpp | 10 ++++------ mission/controller/acs/control/SafeCtrl.h | 7 ++++--- 8 files changed, 24 insertions(+), 24 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 80269d55..f11a673b 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -791,8 +791,8 @@ 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 */ 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 + 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 double gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)}; uint8_t preferAdis = false; float gyrFilterWeight = 0.6; diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 52acc7ee..ba878034 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -1115,8 +1115,9 @@ void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::AttitudeEstim } } -void MultiplicativeKalmanFilter::updateDataSet(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus, - double quat[4], double satRotRate[3]) { +void MultiplicativeKalmanFilter::updateDataSet(acsctrl::AttitudeEstimationData *mekfData, + MekfStatus mekfStatus, double quat[4], + double satRotRate[3]) { { PoolReadGuard pg(mekfData); if (pg.getReadResult() == returnvalue::OK) { diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index bd722115..09fad744 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -32,8 +32,8 @@ class MultiplicativeKalmanFilter { */ ReturnValue_t init(const double *magneticField_, const bool validMagField_, const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, - const double *magFieldJ, const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData, - AcsParameters *acsParameters); + const double *magFieldJ, const bool validMagModel, + acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters); /* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter * for the current step after the initalization @@ -100,8 +100,8 @@ class MultiplicativeKalmanFilter { /*Parameter INIT*/ /*Functions*/ void updateDataSetWithoutData(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus); - void updateDataSet(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus, double quat[4], - double satRotRate[3]); + void updateDataSet(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus, + double quat[4], double satRotRate[3]); }; #endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */ diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h index 4f58b197..9f29c4f2 100644 --- a/mission/controller/acs/Navigation.h +++ b/mission/controller/acs/Navigation.h @@ -17,8 +17,8 @@ class Navigation { ReturnValue_t useMekf(ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, - acsctrl::SusDataProcessed *susDataProcessed, acsctrl::AttitudeEstimationData *mekfData, - AcsParameters *acsParameters); + acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters); void resetMekf(acsctrl::AttitudeEstimationData *mekfData); ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed); diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 754843f8..408c30e1 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -8,9 +8,9 @@ Detumble::Detumble() {} Detumble::~Detumble() {} acs::ControlModeStrategy Detumble::detumbleStrategy(const bool magFieldValid, - const bool satRotRateValid, - const bool magFieldRateValid, - const bool useFullDetumbleLaw) { + const bool satRotRateValid, + const bool magFieldRateValid, + const bool useFullDetumbleLaw) { if (not magFieldValid) { return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (satRotRateValid and useFullDetumbleLaw) { diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index 476766c3..851e4541 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -12,8 +12,8 @@ class Detumble { virtual ~Detumble(); acs::ControlModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, - const bool magFieldRateValid, - const bool useFullDetumbleLaw); + const bool magFieldRateValid, + const bool useFullDetumbleLaw); void bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB, double gain); diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index b1699aef..972b7b92 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -9,12 +9,10 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter SafeCtrl::~SafeCtrl() {} -acs::ControlModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, - const bool satRotRateValid, const bool sunDirValid, - const bool fusedRateTotalValid, - const uint8_t mekfEnabled, - const uint8_t gyrEnabled, - const uint8_t dampingEnabled) { +acs::ControlModeStrategy SafeCtrl::safeCtrlStrategy( + const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, + const bool sunDirValid, const bool fusedRateTotalValid, const uint8_t mekfEnabled, + const uint8_t gyrEnabled, const uint8_t dampingEnabled) { if (not magFieldValid) { return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (mekfEnabled and mekfValid) { diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index 1c38d81d..e303f302 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -13,9 +13,10 @@ class SafeCtrl { virtual ~SafeCtrl(); acs::ControlModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, - const bool satRotRateValid, const bool sunDirValid, - const bool fusedRateTotalValid, const uint8_t mekfEnabled, - const uint8_t gyrEnabled, const uint8_t dampingEnabled); + const bool satRotRateValid, const bool sunDirValid, + const bool fusedRateTotalValid, + const uint8_t mekfEnabled, const uint8_t gyrEnabled, + const uint8_t dampingEnabled); void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI, const double *quatBI, const double *sunDirRefB, double *magMomB, From fe0ceac9b40045e731c7d78dbc8b6e16501b457e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 6 Dec 2023 17:42:30 +0100 Subject: [PATCH 66/68] something went wrong here --- CHANGELOG.md | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 1ad30898..2f12ce24 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -28,6 +28,9 @@ will consitute of a breaking change warranting a new major release: - 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. +- `ACS Controller` now has the function `performAttitudeControl` which is called prior to passing + on to the relevant mode functions. It handles all telemetry relevant functions, which were + always called, regardless of the mode. ## Added @@ -132,16 +135,6 @@ will consitute of a breaking change warranting a new major release: during which the SUS was not working as well as the maximum amount of invalid messages. - Updated battery internal resistance to new value -## Changed - -- `Power Controller` now uses monotonic clock for calculating time difference -- `ACS Controller` now uses monotonic clock for calculating time difference and the normal clock - for model calculations. The `timeDelta` is now calculated in the controller instead of - everywhere where it is needed. -- `ACS Controller` now has the function `performAttitudeControl` which is called prior to passing - on to the relevant mode functions. It handles all telemetry relevant functions, which were - always called, regardless of the mode. - # [v7.1.0] 2023-10-11 - Bumped `eive-tmtc` to v5.8.0. From 91a12a7b72cc77166f1aea8f6b680aa0b5dabdc7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 6 Dec 2023 17:43:00 +0100 Subject: [PATCH 67/68] changelog shenanigans --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 2f12ce24..dd0eaddb 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,6 +14,8 @@ will consitute of a breaking change warranting a new major release: - The TMTC interface changes in any shape of form. - The behaviour of the OBSW changes in a major shape or form relevant for operations +# unreleased + # [v7.5.0] 2023-12-06 - `eive-tmtc` v5.12.0 From 30fb60bd2b31adc261d3a17d86428d5a8ffedd8c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 6 Dec 2023 17:43:57 +0100 Subject: [PATCH 68/68] OH MY GOD --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index dd0eaddb..b89cacbe 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,7 +14,7 @@ will consitute of a breaking change warranting a new major release: - The TMTC interface changes in any shape of form. - The behaviour of the OBSW changes in a major shape or form relevant for operations -# unreleased +# [unreleased] # [v7.5.0] 2023-12-06