diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index c0853695..22d29294 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -157,20 +157,23 @@ void AcsController::performSafe() { guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); // if MEKF is working double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; - bool magMomMtqValid = false; if (result == MultiplicativeKalmanFilter::MEKF_RUNNING) { - safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(), - mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(), - susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(), - mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(), - sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); + result = safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(), + mgmDataProcessed.magIgrfModel.value, + mgmDataProcessed.magIgrfModel.isValid(), + susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(), + mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(), + sunTargetDir, satRateSafe, &errAng, magMomMtq); } else { - safeCtrl.safeNoMekf( + result = safeCtrl.safeNoMekf( now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(), susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), - sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); + sunTargetDir, satRateSafe, &errAng, magMomMtq); + } + if (result == returnvalue::FAILED) { + // ToDo: this should never ever happen or we are dead. prob add an event at least } actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs); diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 705bf599..0e131942 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -1,11 +1,3 @@ - -/* - * Detumble.cpp - * - * Created on: 17 Aug 2022 - * Author: Robin Marquardt - */ - #include "Detumble.h" #include @@ -31,6 +23,12 @@ ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid, if (!magRateValid || !magFieldValid) { return DETUMBLE_NO_SENSORDATA; } + + // change unit from uT to T + double magFieldT[3] = {0, 0, 0}, magRateT[3] = {0, 0, 0}; + VectorOperations::mulScalar(magField, 1e-6, magFieldT, 3); + VectorOperations::mulScalar(magRate, 1e-6, magRateT, 3); + double gain = detumbleParameter->gainD; double factor = -gain / pow(VectorOperations::norm(magField, 3), 2); VectorOperations::mulScalar(magRate, factor, magMom, 3); diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index aa04cbb6..3a69e343 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -32,15 +32,13 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid, double *magFieldModel, bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid, double *satRateMekf, bool rateMekfValid, double *sunDirRef, double *satRatRef, - double *outputAngle, double *outputMagMomB, bool *outputValid) { + double *outputAngle, double *outputMagMomB) { if (!quatBJValid || !magFieldModelValid || !sunDirModelValid || !rateMekfValid) { - *outputValid = false; return SAFECTRL_MEKF_INPUT_INVALID; } - double kRate = 0, kAlign = 0; - kRate = safeModeControllerParameters->k_rate_mekf; - kAlign = safeModeControllerParameters->k_align_mekf; + double kRate = safeModeControllerParameters->k_rate_mekf; + double kAlign = safeModeControllerParameters->k_align_mekf; // Calc sunDirB ,magFieldB with mekf output and model double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -49,22 +47,22 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid, MatrixOperations::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1); MatrixOperations::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1); - double crossSun[3] = {0, 0, 0}; + // change unit from uT to T + VectorOperations::mulScalar(magFieldB, 1e-6, magFieldB, 3); + double crossSun[3] = {0, 0, 0}; VectorOperations::cross(sunDirRef, sunDirB, crossSun); double normCrossSun = VectorOperations::norm(crossSun, 3); // calc angle alpha between sunDirRef and sunDIr - double alpha = 0, dotSun = 0; - dotSun = VectorOperations::dot(sunDirRef, sunDirB); - alpha = acos(dotSun); + double dotSun = VectorOperations::dot(sunDirRef, sunDirB); + double alpha = acos(dotSun); // Law Torque calculations double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0}, torqueRate[3] = {0, 0, 0}, torqueAll[3] = {0, 0, 0}; - double scalarFac = 0; - scalarFac = kAlign * alpha / normCrossSun; + double scalarFac = kAlign * alpha / normCrossSun; VectorOperations::mulScalar(crossSun, scalarFac, torqueAlign, 3); double rateSafeMode[3] = {0, 0, 0}; @@ -82,23 +80,22 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid, VectorOperations::mulScalar(torqueMgt, 1 / pow(normMag, 2), outputMagMomB, 3); *outputAngle = alpha; - *outputValid = true; - return returnvalue::OK; } // Will be the version in worst case scenario in event of no working MEKF (nor GYRs) -void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB, +ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB, bool sunRateBValid, double *magFieldB, bool magFieldBValid, double *magRateB, bool magRateBValid, double *sunDirRef, - double *satRateRef, double *outputAngle, double *outputMagMomB, - bool *outputValid) { + double *satRateRef, double *outputAngle, double *outputMagMomB) { // Check for invalid Inputs if (!susDirBValid || !magFieldBValid || !magRateBValid) { - *outputValid = false; - return; + return returnvalue::FAILED; } + // change unit from uT to T + VectorOperations::mulScalar(magFieldB, 1e-6, magFieldB, 3); + // normalize sunDir and magDir double magDirB[3] = {0, 0, 0}; VectorOperations::normalize(magFieldB, magDirB, 3); @@ -108,13 +105,11 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl double cosAngleSunMag = VectorOperations::dot(magDirB, susDirB); // Rate parallel to sun direction and magnetic field direction - double rateParaSun = 0, rateParaMag = 0; - double dotSunRateMag = 0, dotmagRateSun = 0, rateFactor = 0; - dotSunRateMag = VectorOperations::dot(sunRateB, magDirB); - dotmagRateSun = VectorOperations::dot(magRateB, susDirB); - rateFactor = 1 - pow(cosAngleSunMag, 2); - rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor; - rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor; + double dotSunRateMag = VectorOperations::dot(sunRateB, magDirB); + double dotmagRateSun = VectorOperations::dot(magRateB, susDirB); + double rateFactor = 1 - pow(cosAngleSunMag, 2); + double rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor; + double rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor; // Full rate or estimate double estSatRate[3] = {0, 0, 0}; @@ -130,7 +125,7 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl * is sufficiently large */ double angleSunMag = acos(cosAngleSunMag); if (angleSunMag < safeModeControllerParameters->sunMagAngleMin) { - return; + return returnvalue::FAILED; } // Rate for Torque Calculation @@ -138,9 +133,8 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl VectorOperations::subtract(estSatRate, satRateRef, diffRate, 3); // Torque Align calculation - double kRateNoMekf = 0, kAlignNoMekf = 0; - kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf; - kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf; + double kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf; + double kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf; double cosAngleAlignErr = VectorOperations::dot(sunDirRef, susDirB); double crossSusSunRef[3] = {0, 0, 0}; @@ -171,5 +165,5 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double)); *outputAngle = angleAlignErr; - *outputValid = true; + return returnvalue::OK; } diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index 1784f9ca..92d20313 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -23,14 +23,12 @@ class SafeCtrl { bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid, double *satRateMekf, bool rateMekfValid, double *sunDirRef, double *satRatRef, // From Guidance (!) - double *outputAngle, double *outputMagMomB, bool *outputValid); + double *outputAngle, double *outputMagMomB); - void safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB, - bool sunRateBValid, double *magFieldB, bool magFieldBValid, double *magRateB, - bool magRateBValid, double *sunDirRef, double *satRateRef, double *outputAngle, - double *outputMagMomB, bool *outputValid); - - void idleSunPointing(); // with reaction wheels + ReturnValue_t safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB, + bool sunRateBValid, double *magFieldB, bool magFieldBValid, + double *magRateB, bool magRateBValid, double *sunDirRef, + double *satRateRef, double *outputAngle, double *outputMagMomB); protected: private: