eive-obsw/mission/controller/acs/ActuatorCmd.cpp

76 lines
3.0 KiB
C++
Raw Normal View History

2022-09-27 11:06:11 +02:00
#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>
2022-09-19 15:03:25 +02:00
#include <cmath>
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
2022-09-19 15:03:25 +02:00
ActuatorCmd::ActuatorCmd() {}
2022-09-19 15:03:25 +02:00
ActuatorCmd::~ActuatorCmd() {}
2022-09-19 15:03:25 +02:00
2023-03-10 17:31:12 +01:00
void ActuatorCmd::scalingTorqueRws(double *rwTrq, double maxTorque) {
uint8_t maxIdx = 0;
VectorOperations<double>::maxAbsValue(rwTrq, 4, &maxIdx);
double maxValue = rwTrq[maxIdx];
2022-10-12 15:06:24 +02:00
2023-02-27 17:36:33 +01:00
if (maxValue > maxTorque) {
double scalingFactor = maxTorque / maxValue;
2023-03-10 17:31:12 +01:00
VectorOperations<double>::mulScalar(rwTrq, scalingFactor, rwTrq, 4);
2022-10-12 15:06:24 +02:00
}
}
2023-02-27 17:36:33 +01:00
void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2,
int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed,
double sampleTime, int32_t maxRwSpeed, double inertiaWheel) {
using namespace Math;
2022-10-12 15:06:24 +02:00
// Calculating the commanded speed in RPM for every reaction wheel
int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3};
2022-10-12 15:06:24 +02:00
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]
2023-02-27 17:36:33 +01:00
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);
for (uint8_t i = 0; i < 4; i++) {
if (rwCmdSpeed[i] > maxRwSpeed) {
rwCmdSpeed[i] = maxRwSpeed;
} else if (rwCmdSpeed[i] < -maxRwSpeed) {
rwCmdSpeed[i] = -maxRwSpeed;
}
2023-03-13 11:05:21 +01:00
}
2022-09-19 15:03:25 +02:00
}
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator,
2023-02-27 17:36:33 +01:00
const double *inverseAlignment, double maxDipol) {
// Convert to actuator frame
double dipolMomentActuatorDouble[3] = {0, 0, 0};
2023-02-27 17:36:33 +01:00
MatrixOperations<double>::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3,
1);
// Scaling along largest element if dipol exceeds maximum
2023-03-07 13:53:46 +01:00
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]);
}
2022-09-19 15:03:25 +02:00
}