meggert
3750f1ac57
All checks were successful
EIVE/eive-obsw/pipeline/pr-v3.0.0-dev This commit looks good
78 lines
3.1 KiB
C++
78 lines
3.1 KiB
C++
#include "ActuatorCmd.h"
|
|
|
|
#include <fsfw/globalfunctions/constants.h>
|
|
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
|
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
|
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
|
|
|
ActuatorCmd::ActuatorCmd() {}
|
|
|
|
ActuatorCmd::~ActuatorCmd() {}
|
|
|
|
void ActuatorCmd::scalingTorqueRws(double *rwTrq, double maxTorque) {
|
|
uint8_t maxIdx = 0;
|
|
VectorOperations<double>::maxAbsValue(rwTrq, 4, &maxIdx);
|
|
double maxValue = rwTrq[maxIdx];
|
|
|
|
if (maxValue > maxTorque) {
|
|
double scalingFactor = maxTorque / maxValue;
|
|
VectorOperations<double>::mulScalar(rwTrq, scalingFactor, rwTrq, 4);
|
|
}
|
|
}
|
|
|
|
void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1,
|
|
const int32_t speedRw2, const int32_t speedRw3,
|
|
const double sampleTime, const double inertiaWheel,
|
|
const int32_t maxRwSpeed, const double *rwTorque,
|
|
int32_t *rwCmdSpeed) {
|
|
// concentrate RW speed values (in 0.1 [RPM]) in vector
|
|
int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3};
|
|
|
|
// calculate required RW speed as sum of current RW speed and RW speed delta
|
|
// delta w_rw = delta t / I_RW * torque_RW [rad/s]
|
|
double deltaSpeed[4] = {0, 0, 0, 0};
|
|
const double factor = sampleTime / inertiaWheel * RAD_PER_SEC_TO_RPM * 10;
|
|
VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4);
|
|
|
|
// convert double to int32
|
|
int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
|
|
for (int i = 0; i < 4; i++) {
|
|
deltaSpeedInt[i] = std::round(deltaSpeed[i]);
|
|
}
|
|
|
|
// sum of current RW speed and RW speed delta
|
|
VectorOperations<int32_t>::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4);
|
|
|
|
// crop values which would exceed the maximum possible RPM
|
|
for (uint8_t i = 0; i < 4; i++) {
|
|
if (rwCmdSpeed[i] > maxRwSpeed) {
|
|
rwCmdSpeed[i] = maxRwSpeed;
|
|
} else if (rwCmdSpeed[i] < -maxRwSpeed) {
|
|
rwCmdSpeed[i] = -maxRwSpeed;
|
|
}
|
|
}
|
|
}
|
|
|
|
void ActuatorCmd::cmdDipoleMtq(const double *inverseAlignment, const double maxDipole,
|
|
const double *dipoleMoment, int16_t *dipoleMomentActuator) {
|
|
// convert to actuator frame
|
|
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(dipoleMomentActuatorDouble, 3, &maxIdx);
|
|
double maxAbsValue = std::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(dipoleMomentActuatorDouble, 1e4, dipoleMomentActuatorDouble,
|
|
3);
|
|
for (int i = 0; i < 3; i++) {
|
|
dipoleMomentActuator[i] = std::round(dipoleMomentActuatorDouble[i]);
|
|
}
|
|
}
|