eive/eive-obsw
eive/eive-obsw
12
4
Fork 0
Code Issues 4 Pull Requests 2 Releases 119 Wiki Activity

changed satRotRateRef to scalar
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-04-05 16:46:57 +02:00
parent ce7da9f513
commit 39b2a3420c
7 changed files with 22 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
mission/controller/AcsController.cpp
View File

@ -165,7 +165,7 @@ void AcsController::performSafe() {
}
// get desired satellite rate, sun direction to align to and inertia
double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0},
double satRateSafe = 0, sunTargetDir[3] = {0, 0, 0},
inertiaEive[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe, inertiaEive);

2
mission/controller/acs/AcsParameters.cpp
View File

@ -351,7 +351,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir);
break;
case 0xA:
parameterWrapper->setVector(safeModeControllerParameters.satRateRef);
parameterWrapper->set(safeModeControllerParameters.satRateRef);
break;
case 0xB:
parameterWrapper->set(safeModeControllerParameters.angleStartSpin);

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

@ -828,7 +828,7 @@ class AcsParameters : public HasParametersIF {
double sunTargetDirLeop[3] = {0, sqrt(.5), sqrt(.5)};
double sunTargetDir[3] = {0, 0, 1};
double satRateRef[3] = {0, 0, 0};
double satRateRef = 0;
float angleStartSpin = 20 * M_PI / 180;
} safeModeControllerParameters;

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

@ -539,7 +539,7 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
}
}
void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3],
void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double &satRateSafe,
double inertiaEive[3][3]) {
std::error_code e;
if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or
@ -554,8 +554,7 @@ void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3
std::memcpy(inertiaEive, acsParameters->inertiaEIVE.inertiaMatrixUndeployed,
sizeof(&inertiaEive));
}
std::memcpy(satRateSafe, acsParameters->safeModeControllerParameters.satRateRef,
sizeof(&satRateSafe));
satRateSafe = acsParameters->safeModeControllerParameters.satRateRef;
}
ReturnValue_t Guidance::solarArrayDeploymentComplete() {

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

@ -12,7 +12,7 @@ class Guidance {
Guidance(AcsParameters *acsParameters_);
virtual ~Guidance();
void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3], double inertiaEive[3][3]);
void getTargetParamsSafe(double sunTargetSafe[3], double &satRateRef, double inertiaEive[3][3]);
ReturnValue_t solarArrayDeploymentComplete();
// Function to get the target quaternion and refence rotation rate from gps position and

21
mission/controller/acs/control/SafeCtrl.cpp
View File

@ -30,7 +30,7 @@ ReturnValue_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const ReturnV
void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
const double *sunDirModelI, const double *quatBI, const double *sunDirRefB,
const double *satRotRateRefB, const double inertiaMatrix[3][3],
const double satRotRateRef, const double inertiaMatrix[3][3],
double *magMomB, double &errorAngle) {
// convert magFieldB from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
@ -44,7 +44,7 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
errorAngle = acos(dotSun);
splitRotationalRate(satRotRateB, sunDirB);
calculateRotationalRateTorque(satRotRateRefB, sunDirB, sunDirRefB, errorAngle,
calculateRotationalRateTorque(satRotRateRef, sunDirB, sunDirRefB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelMekf,
acsParameters->safeModeControllerParameters.k_orthoMekf);
calculateAngleErrorTorque(sunDirB, sunDirRefB,
@ -60,7 +60,7 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
const double *sunDirB, const double *sunDirRefB,
const double *satRotRateRefB, const double inertiaMatrix[3][3],
const double satRotRateRef, const double inertiaMatrix[3][3],
double *magMomB, double &errorAngle) {
// convert magFieldB from uT to T
double magFieldBT[3] = {0, 0, 0};
@ -71,7 +71,7 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
errorAngle = acos(dotSun);
splitRotationalRate(satRotRateB, sunDirB);
calculateRotationalRateTorque(satRotRateRefB, sunDirB, sunDirRefB, errorAngle,
calculateRotationalRateTorque(satRotRateRef, sunDirB, sunDirRefB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelNonMekf,
acsParameters->safeModeControllerParameters.k_orthoNonMekf);
calculateAngleErrorTorque(sunDirB, sunDirRefB,
@ -87,7 +87,7 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
}
void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB,
const double *satRotRateRefB, const double *sunDirRefB,
const double satRotRateRef, const double *sunDirRefB,
double *magMomB, double &errorAngle) {
// convert magFieldB from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
@ -96,7 +96,7 @@ void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRate
errorAngle = NAN;
splitRotationalRate(satRotRateB, sunDirRefB);
calculateRotationalRateTorque(satRotRateRefB, sunDirRefB, sunDirRefB, errorAngle,
calculateRotationalRateTorque(satRotRateRef, sunDirRefB, sunDirRefB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelNonMekf,
acsParameters->safeModeControllerParameters.k_orthoNonMekf);
@ -116,14 +116,17 @@ void SafeCtrl::splitRotationalRate(const double *satRotRateB, const double *sunD
VectorOperations<double>::subtract(satRotRateB, satRotRateParallelB, satRotRateOrthogonalB, 3);
}
void SafeCtrl::calculateRotationalRateTorque(const double *satRotRateRefB, const double *sunDirB,
void SafeCtrl::calculateRotationalRateTorque(const double satRotRateRef, const double *sunDirB,
const double *sunDirRefB, double &errorAngle,
const double gainParallel, const double gainOrtho) {
// calculate torque for parallel rotational rate
if ((isfinite(errorAngle)) and
(errorAngle < (double)acsParameters->safeModeControllerParameters.angleStartSpin)) {
VectorOperations<double>::subtract(satRotRateRefB, satRotRateParallelB, cmdParallel, 3);
VectorOperations<double>::mulScalar(cmdParallel, gainParallel, cmdParallel, 3);
double satRotRateNorm = VectorOperations<double>::norm(satRotRateParallelB, 3);
double satRotRateUnitVec[3] = {0, 0, 0};
VectorOperations<double>::normalize(satRotRateParallelB, satRotRateUnitVec, 3);
VectorOperations<double>::mulScalar(satRotRateUnitVec, satRotRateRef - satRotRateNorm,
cmdParallel, 3);
} else {
VectorOperations<double>::mulScalar(cmdParallel, -gainParallel, cmdParallel, 3);
}

8
mission/controller/acs/control/SafeCtrl.h
View File

@ -26,20 +26,20 @@ class SafeCtrl {
const bool satRotRateValid, const bool sunDirValid);
void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI,
const double *quatBI, const double *sunDirRefB, const double *satRotRateRefB,
const double *quatBI, const double *sunDirRefB, const double satRotRateRef,
const double inertiaMatrix[3][3], double *magMomB, double &errorAngle);
void safeNonMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
const double *sunDirRefB, const double *satRotRateRefB,
const double *sunDirRefB, const double satRotRateRef,
const double inertiaMatrix[3][3], double *magMomB, double &errorAngle);
void safeRateDamping(const double *magFieldB, const double *satRotRateB,
const double *satRotRateRefB, const double *sunDirRefB, double *magMomB,
const double satRotRateRef, const double *sunDirRefB, double *magMomB,
double &errorAngle);
void splitRotationalRate(const double *satRotRateB, const double *sunDirB);
void calculateRotationalRateTorque(const double *satRotRateRefB, const double *sunDirB,
void calculateRotationalRateTorque(const double satRotRateRef, const double *sunDirB,
const double *sunDirRefB, double &errorAngle,
const double gainParallel, const double gainOrtho);