eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 Wiki Activity

Merge branch 'eggert/acs' into marquardt/ptgCtrl
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Details

Browse Source 
# Conflicts:
#	mission/controller/AcsController.cpp
#	mission/controller/AcsController.h
#	mission/controller/acs/AcsParameters.h
#	mission/controller/acs/ActuatorCmd.h
#	mission/controller/acs/Guidance.cpp
#	mission/controller/acs/Guidance.h
#	mission/controller/acs/MultiplicativeKalmanFilter.cpp
#	mission/controller/acs/OutputValues.h
#	mission/controller/acs/SensorProcessing.cpp
#	mission/controller/acs/SensorProcessing.h
#	mission/controller/acs/control/Detumble.cpp
#	mission/controller/acs/control/Detumble.h
#	mission/controller/acs/control/PtgCtrl.cpp
#	mission/controller/acs/util/MathOperations.h
This commit is contained in:
Marius Eggert
2022-12-13 11:26:23 +01:00
parent 2b445369fd cf1a1185fc
commit d33ae9ede7
322 changed files with 17249 additions and 9124 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
mission/controller/acs/ActuatorCmd.cpp
View File

@ -5,23 +5,21 @@
* Author: Robin Marquardt
*/
#include "ActuatorCmd.h"
#include "util/MathOperations.h"
#include "util/CholeskyDecomposition.h"
#include <cmath>
#include <fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) {
acsParameters = *acsParameters_;
}
#include <cmath>
ActuatorCmd::~ActuatorCmd(){
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
}
ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; }
ActuatorCmd::~ActuatorCmd() {}
void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) {
// Scaling the commanded torque to a maximum value
@ -46,7 +44,7 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1
using namespace Math;
// Calculating the commanded speed in RPM for every reaction wheel
double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3};
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
double deltaSpeed[4] = {0, 0, 0, 0};
double commandTime = acsParameters.onBoardParams.sampleTime,
inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel;
@ -58,22 +56,20 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1
}
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits) {
// Convert to Unit frame
MatrixOperations<double>::multiply(*acsParameters.magnetorquesParameter.inverseAlignment,
dipolMoment, dipolMomentUnits, 3, 3, 1);
// Scaling along largest element if dipol exceeds maximum
double maxDipol = acsParameters.magnetorquesParameter.DipolMax;
double maxValue = 0;
for (int i = 0; i < 3; i++) {
if (abs(dipolMomentUnits[i]) > maxDipol) {
maxValue = abs(dipolMomentUnits[i]);
}
}
// Convert to Unit frame
MatrixOperations<double>::multiply(*acsParameters.magnetorquesParameter.inverseAlignment, dipolMoment, dipolMomentUnits, 3, 3, 1);
// Scaling along largest element if dipol exceeds maximum
double maxDipol = acsParameters.magnetorquesParameter.DipolMax;
double maxValue = 0;
for (int i = 0; i < 3; i++) {
if (abs(dipolMomentUnits[i]) > maxDipol) {
maxValue = abs(dipolMomentUnits[i]);
}
}
if (maxValue > maxDipol) {
double scalingFactor = maxDipol / maxValue;
VectorOperations<double>::mulScalar(dipolMomentUnits, scalingFactor, dipolMomentUnits, 3);
}
if (maxValue > maxDipol) {
double scalingFactor = maxDipol / maxValue;
VectorOperations<double>::mulScalar(dipolMomentUnits, scalingFactor, dipolMomentUnits, 3);
}
}