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

better solution for actuatorCmd
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert
2023-02-27 17:36:33 +01:00
parent 5c30adb9bb
commit 26369039da
3 changed files with 37 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -14,11 +14,7 @@ ActuatorCmd::ActuatorCmd() {}
ActuatorCmd::~ActuatorCmd() {}
void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled,
AcsParameters::RwHandlingParameters *rwHandlingParameters) {
// Scaling the commanded torque to a maximum value
double maxTrq = rwHandlingParameters->maxTrq;
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) {
@ -26,16 +22,15 @@ void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled,
}
}
if (maxValue > maxTrq) {
double scalingFactor = maxTrq / maxValue;
if (maxValue > maxTorque) {
double scalingFactor = maxTorque / maxValue;
VectorOperations<double>::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4);
}
}
void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1,
const int32_t speedRw2, const int32_t speedRw3,
const double *rwTorque, int32_t *rwCmdSpeed,
AcsParameters *acsParameters) {
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
@ -43,8 +38,7 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1,
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 = acsParameters->onBoardParams.sampleTime /
acsParameters->rwHandlingParameters.inertiaWheel * radToRpm;
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++) {
@ -55,13 +49,12 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1,
}
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator,
AcsParameters::MagnetorquerParameter *magnetorquerParameter) {
const double *inverseAlignment, double maxDipol) {
// Convert to actuator frame
double dipolMomentActuatorDouble[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*magnetorquerParameter->inverseAlignment, dipolMoment,
dipolMomentActuatorDouble, 3, 3, 1);
MatrixOperations<double>::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3,
1);
// Scaling along largest element if dipol exceeds maximum
double maxDipol = magnetorquerParameter->dipolMax;
double maxValue = 0;
for (int i = 0; i < 3; i++) {
if (abs(dipolMomentActuator[i]) > maxDipol) {