eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 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 // 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}}; inertiaEive[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe, inertiaEive); 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); parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir);
break; break;
case 0xA: case 0xA:
parameterWrapper->setVector(safeModeControllerParameters.satRateRef); parameterWrapper->set(safeModeControllerParameters.satRateRef);
break; break;
case 0xB: case 0xB:
parameterWrapper->set(safeModeControllerParameters.angleStartSpin); 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 sunTargetDirLeop[3] = {0, sqrt(.5), sqrt(.5)};
double sunTargetDir[3] = {0, 0, 1}; double sunTargetDir[3] = {0, 0, 1};
double satRateRef[3] = {0, 0, 0}; double satRateRef = 0;
float angleStartSpin = 20 * M_PI / 180; float angleStartSpin = 20 * M_PI / 180;
} safeModeControllerParameters; } 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]) { double inertiaEive[3][3]) {
std::error_code e; std::error_code e;
if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or 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, std::memcpy(inertiaEive, acsParameters->inertiaEIVE.inertiaMatrixUndeployed,
sizeof(&inertiaEive)); sizeof(&inertiaEive));
} }
std::memcpy(satRateSafe, acsParameters->safeModeControllerParameters.satRateRef, satRateSafe = acsParameters->safeModeControllerParameters.satRateRef;
sizeof(&satRateSafe));
} }
ReturnValue_t Guidance::solarArrayDeploymentComplete() { ReturnValue_t Guidance::solarArrayDeploymentComplete() {

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

@ -12,7 +12,7 @@ class Guidance {
Guidance(AcsParameters *acsParameters_); Guidance(AcsParameters *acsParameters_);
virtual ~Guidance(); 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(); ReturnValue_t solarArrayDeploymentComplete();
// Function to get the target quaternion and refence rotation rate from gps position and // 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, void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
const double *sunDirModelI, const double *quatBI, const double *sunDirRefB, 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) { double *magMomB, double &errorAngle) {
// convert magFieldB from uT to T // convert magFieldB from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3); VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
@ -44,7 +44,7 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
errorAngle = acos(dotSun); errorAngle = acos(dotSun);
splitRotationalRate(satRotRateB, sunDirB); splitRotationalRate(satRotRateB, sunDirB);
calculateRotationalRateTorque(satRotRateRefB, sunDirB, sunDirRefB, errorAngle, calculateRotationalRateTorque(satRotRateRef, sunDirB, sunDirRefB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelMekf, acsParameters->safeModeControllerParameters.k_parallelMekf,
acsParameters->safeModeControllerParameters.k_orthoMekf); acsParameters->safeModeControllerParameters.k_orthoMekf);
calculateAngleErrorTorque(sunDirB, sunDirRefB, 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, void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
const double *sunDirB, const double *sunDirRefB, 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) { double *magMomB, double &errorAngle) {
// convert magFieldB from uT to T // convert magFieldB from uT to T
double magFieldBT[3] = {0, 0, 0}; double magFieldBT[3] = {0, 0, 0};
@ -71,7 +71,7 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
errorAngle = acos(dotSun); errorAngle = acos(dotSun);
splitRotationalRate(satRotRateB, sunDirB); splitRotationalRate(satRotRateB, sunDirB);
calculateRotationalRateTorque(satRotRateRefB, sunDirB, sunDirRefB, errorAngle, calculateRotationalRateTorque(satRotRateRef, sunDirB, sunDirRefB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelNonMekf, acsParameters->safeModeControllerParameters.k_parallelNonMekf,
acsParameters->safeModeControllerParameters.k_orthoNonMekf); acsParameters->safeModeControllerParameters.k_orthoNonMekf);
calculateAngleErrorTorque(sunDirB, sunDirRefB, 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, 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) { double *magMomB, double &errorAngle) {
// convert magFieldB from uT to T // convert magFieldB from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3); VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
@ -96,7 +96,7 @@ void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRate
errorAngle = NAN; errorAngle = NAN;
splitRotationalRate(satRotRateB, sunDirRefB); splitRotationalRate(satRotRateB, sunDirRefB);
calculateRotationalRateTorque(satRotRateRefB, sunDirRefB, sunDirRefB, errorAngle, calculateRotationalRateTorque(satRotRateRef, sunDirRefB, sunDirRefB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelNonMekf, acsParameters->safeModeControllerParameters.k_parallelNonMekf,
acsParameters->safeModeControllerParameters.k_orthoNonMekf); acsParameters->safeModeControllerParameters.k_orthoNonMekf);
@ -116,14 +116,17 @@ void SafeCtrl::splitRotationalRate(const double *satRotRateB, const double *sunD
VectorOperations<double>::subtract(satRotRateB, satRotRateParallelB, satRotRateOrthogonalB, 3); 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 *sunDirRefB, double &errorAngle,
const double gainParallel, const double gainOrtho) { const double gainParallel, const double gainOrtho) {
// calculate torque for parallel rotational rate // calculate torque for parallel rotational rate
if ((isfinite(errorAngle)) and if ((isfinite(errorAngle)) and
(errorAngle < (double)acsParameters->safeModeControllerParameters.angleStartSpin)) { (errorAngle < (double)acsParameters->safeModeControllerParameters.angleStartSpin)) {
VectorOperations<double>::subtract(satRotRateRefB, satRotRateParallelB, cmdParallel, 3); double satRotRateNorm = VectorOperations<double>::norm(satRotRateParallelB, 3);
VectorOperations<double>::mulScalar(cmdParallel, gainParallel, cmdParallel, 3); double satRotRateUnitVec[3] = {0, 0, 0};
VectorOperations<double>::normalize(satRotRateParallelB, satRotRateUnitVec, 3);
VectorOperations<double>::mulScalar(satRotRateUnitVec, satRotRateRef - satRotRateNorm,
cmdParallel, 3);
} else { } else {
VectorOperations<double>::mulScalar(cmdParallel, -gainParallel, cmdParallel, 3); 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); const bool satRotRateValid, const bool sunDirValid);
void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI, 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); const double inertiaMatrix[3][3], double *magMomB, double &errorAngle);
void safeNonMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB, 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); const double inertiaMatrix[3][3], double *magMomB, double &errorAngle);
void safeRateDamping(const double *magFieldB, const double *satRotRateB, 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); double &errorAngle);
void splitRotationalRate(const double *satRotRateB, const double *sunDirB); 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 *sunDirRefB, double &errorAngle,
const double gainParallel, const double gainOrtho); const double gainParallel, const double gainOrtho);