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

englando too hard

Browse Source
This commit is contained in:
Marius Eggert
2023-04-27 10:33:43 +02:00
committed by Robin Mueller
parent 0ff7e0f97a
commit 6705ede2fc
6 changed files with 33 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -5,9 +5,6 @@
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
ActuatorCmd::ActuatorCmd() {}
ActuatorCmd::~ActuatorCmd() {}
@ -56,24 +53,24 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1,
}
}
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator,
const double *inverseAlignment, double maxDipol) {
void ActuatorCmd::cmdDipoleMtq(const double *inverseAlignment, const double maxDipole,
const double *dipoleMoment, int16_t *dipoleMomentActuator) {
// convert to actuator frame
double dipolMomentActuatorDouble[3] = {0, 0, 0};
MatrixOperations<double>::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3,
double dipoleMomentActuatorDouble[3] = {0, 0, 0};
MatrixOperations<double>::multiply(inverseAlignment, dipoleMoment, dipoleMomentActuatorDouble, 3, 3,
1);
// scaling along largest element if dipole exceeds maximum
uint8_t maxIdx = 0;
VectorOperations<double>::maxAbsValue(dipolMomentActuatorDouble, 3, &maxIdx);
double maxAbsValue = abs(dipolMomentActuatorDouble[maxIdx]);
if (maxAbsValue > maxDipol) {
double scalingFactor = maxDipol / maxAbsValue;
VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, scalingFactor,
dipolMomentActuatorDouble, 3);
VectorOperations<double>::maxAbsValue(dipoleMomentActuatorDouble, 3, &maxIdx);
double maxAbsValue = abs(dipoleMomentActuatorDouble[maxIdx]);
if (maxAbsValue > maxDipole) {
double scalingFactor = maxDipole / maxAbsValue;
VectorOperations<double>::mulScalar(dipoleMomentActuatorDouble, scalingFactor,
dipoleMomentActuatorDouble, 3);
}
// scale dipole from 1 Am^2 to 1e^-4 Am^2
VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, 1e4, dipolMomentActuatorDouble, 3);
VectorOperations<double>::mulScalar(dipoleMomentActuatorDouble, 1e4, dipoleMomentActuatorDouble, 3);
for (int i = 0; i < 3; i++) {
dipolMomentActuator[i] = std::round(dipolMomentActuatorDouble[i]);
dipoleMomentActuator[i] = std::round(dipoleMomentActuatorDouble[i]);
}
}