84 lines
2.6 KiB
C++
84 lines
2.6 KiB
C++
|
/*
|
||
|
|
* ActuatorCmd.cpp
|
||
|
|
*
|
||
|
|
* Created on: 4 Aug 2022
|
||
|
|
* Author: Robin Marquardt
|
||
|
|
*/
|
||
|
|
|
||
|
|
|
||
|
|
#include "ActuatorCmd.h"
|
||
|
|
#include <math.h>
|
||
|
|
#include <acs/util/MathOperations.h>
|
||
|
|
#include <fsfw/src/fsfw/globalfunctions/constants.h>
|
||
|
|
#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
|
||
|
|
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
|
||
|
|
#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
|
||
|
|
#include <acs/util/CholeskyDecomposition.h>
|
||
|
|
|
||
|
|
ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) {
|
||
|
|
acsParameters = *acsParameters_;
|
||
|
|
}
|
||
|
|
|
||
|
|
ActuatorCmd::~ActuatorCmd(){
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void ActuatorCmd::cmdSpeedToRws(const double *speedRw0, const double *speedRw1, const double *speedRw2,
|
||
|
|
const double *speedRw3, const double *rwTrqIn, const double *rwTrqNS, double *rwCmdSpeed){
|
||
|
|
|
||
|
|
using namespace Math;
|
||
|
|
// Scaling the commanded torque to a maximum value
|
||
|
|
double torque[4] = {0, 0, 0, 0};
|
||
|
|
double maxTrq = acsParameters.rwHandlingParameters.maxTrq;
|
||
|
|
VectorOperations<double>::add(rwTrqIn, rwTrqNS, torque, 4);
|
||
|
|
|
||
|
|
double maxValue = 0;
|
||
|
|
for (int i = 0; i < 4; i++) { //size of torque, always 4 ?
|
||
|
|
if (abs(torque[i]) > maxValue) {
|
||
|
|
maxValue = abs(torque[i]);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (maxValue > maxTrq) {
|
||
|
|
|
||
|
|
double scalingFactor = maxTrq / maxValue;
|
||
|
|
VectorOperations<double>::mulScalar(torque, scalingFactor, torque, 4);
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
// Calculating the commanded speed in RPM for every reaction wheel
|
||
|
|
double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3};
|
||
|
|
double deltaSpeed[4] = {0, 0, 0, 0};
|
||
|
|
double commandTime = acsParameters.onBoardParams.sampleTime,
|
||
|
|
inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel;
|
||
|
|
double radToRpm = 60 / (2 * PI); // factor for conversion to RPM
|
||
|
|
// W_RW = Torque_RW / I_RW * delta t [rad/s]
|
||
|
|
double factor = commandTime / inertiaWheel * radToRpm;
|
||
|
|
VectorOperations<double>::mulScalar(torque, factor, deltaSpeed, 4);
|
||
|
|
VectorOperations<double>::add(speedRws, deltaSpeed, rwCmdSpeed, 4);
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
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]);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (maxValue > maxDipol) {
|
||
|
|
|
||
|
|
double scalingFactor = maxDipol / maxValue;
|
||
|
|
VectorOperations<double>::mulScalar(dipolMomentUnits, scalingFactor, dipolMomentUnits, 3);
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|