72 lines
2.8 KiB
C++
72 lines
2.8 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>
|
|
|
|
#include <cmath>
|
|
|
|
#include "util/CholeskyDecomposition.h"
|
|
#include "util/MathOperations.h"
|
|
|
|
ActuatorCmd::ActuatorCmd() {}
|
|
|
|
ActuatorCmd::~ActuatorCmd() {}
|
|
|
|
void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled, double maxTorque) {
|
|
double maxValue = 0;
|
|
for (int i = 0; i < 4; i++) { // size of torque, always 4 ?
|
|
if (abs(rwTrq[i]) > maxValue) {
|
|
maxValue = abs(rwTrq[i]);
|
|
}
|
|
}
|
|
|
|
if (maxValue > maxTorque) {
|
|
double scalingFactor = maxTorque / maxValue;
|
|
VectorOperations<double>::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4);
|
|
}
|
|
}
|
|
|
|
void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2,
|
|
int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed,
|
|
double sampleTime, double inertiaWheel) {
|
|
using namespace Math;
|
|
|
|
// Calculating the commanded speed in RPM for every reaction wheel
|
|
int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3};
|
|
double deltaSpeed[4] = {0, 0, 0, 0};
|
|
double radToRpm = 60 / (2 * PI); // factor for conversion to RPM
|
|
// W_RW = Torque_RW / I_RW * delta t [rad/s]
|
|
double factor = sampleTime / inertiaWheel * radToRpm;
|
|
int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
|
|
VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4);
|
|
for (int i = 0; i < 4; i++) {
|
|
deltaSpeedInt[i] = std::round(deltaSpeed[i]);
|
|
}
|
|
VectorOperations<int32_t>::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4);
|
|
VectorOperations<int32_t>::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4);
|
|
}
|
|
|
|
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator,
|
|
const double *inverseAlignment, double maxDipol) {
|
|
// Convert to actuator frame
|
|
double dipolMomentActuatorDouble[3] = {0, 0, 0};
|
|
MatrixOperations<double>::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3,
|
|
1);
|
|
// Scaling along largest element if dipol 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);
|
|
}
|
|
// scale dipole from 1 Am^2 to 1e^-4 Am^2
|
|
VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, 1e4, dipolMomentActuatorDouble, 3);
|
|
for (int i = 0; i < 3; i++) {
|
|
dipolMomentActuator[i] = std::round(dipolMomentActuatorDouble[i]);
|
|
}
|
|
}
|