From 9041a3376c18cae282a1afcfbcd39c3ef3b09c47 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 10 Feb 2023 10:56:50 +0100 Subject: [PATCH] ActuatorCmd now output their solutions as integers as expected by the sensors --- mission/controller/acs/ActuatorCmd.cpp | 27 +++++++++++++++++--------- mission/controller/acs/ActuatorCmd.h | 6 +++--- 2 files changed, 21 insertions(+), 12 deletions(-) diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 4e0052e0..cbe24e5b 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -38,27 +38,32 @@ void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) { } } -void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, - const int32_t *speedRw2, const int32_t *speedRw3, - const double *rwTorque, double *rwCmdSpeed) { +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) { using namespace Math; // Calculating the commanded speed in RPM for every reaction wheel - double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; + int32_t 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; + int32_t deltaSpeedInt[4] = {0, 0, 0, 0}; VectorOperations::mulScalar(rwTorque, factor, deltaSpeed, 4); - VectorOperations::add(speedRws, deltaSpeed, rwCmdSpeed, 4); + for (int i = 0; i < 4; i++) { + deltaSpeedInt[i] = std::round(deltaSpeed[i]); + } + VectorOperations::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4); } -void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentActuator) { +void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator) { // Convert to actuator frame + double dipolMomentActuatorDouble[3] = {0, 0, 0}; MatrixOperations::multiply(*acsParameters.magnetorquesParameter.inverseAlignment, - dipolMoment, dipolMomentActuator, 3, 3, 1); + dipolMoment, dipolMomentActuatorDouble, 3, 3, 1); // Scaling along largest element if dipol exceeds maximum double maxDipol = acsParameters.magnetorquesParameter.DipolMax; double maxValue = 0; @@ -69,8 +74,12 @@ void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentActu } if (maxValue > maxDipol) { double scalingFactor = maxDipol / maxValue; - VectorOperations::mulScalar(dipolMomentActuator, scalingFactor, dipolMomentActuator, 3); + VectorOperations::mulScalar(dipolMomentActuatorDouble, scalingFactor, + dipolMomentActuatorDouble, 3); } // scale dipole from 1 Am^2 to 1e^-4 Am^2 - VectorOperations::mulScalar(dipolMomentActuator, 1e4, dipolMomentActuator, 3); + VectorOperations::mulScalar(dipolMomentActuatorDouble, 1e4, dipolMomentActuatorDouble, 3); + for (int i = 0; i < 3; i++) { + dipolMomentActuator[i] = std::round(dipolMomentActuatorDouble[i]); + } } diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 04e2b61f..969bd782 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -28,8 +28,8 @@ class ActuatorCmd { * rwCmdSpeed output revolutions per minute for every * reaction wheel */ - void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, - const int32_t *speedRw3, const double *rwTorque, double *rwCmdSpeed); + void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, + const int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed); /* * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques @@ -37,7 +37,7 @@ class ActuatorCmd { * @param: dipolMoment given dipol moment in spacecraft frame * dipolMomentActuator resulting dipol moment in actuator reference frame */ - void cmdDipolMtq(const double *dipolMoment, double *dipolMomentActuator); + void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator); protected: private: