From eb98b98c1403821ccca16c0d6905a5ddba3f1dd3 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 27 Apr 2023 10:19:07 +0200 Subject: [PATCH] RW cmd fix and cleanup --- mission/controller/AcsController.cpp | 7 +++--- mission/controller/acs/ActuatorCmd.cpp | 34 ++++++++++++++------------ mission/controller/acs/ActuatorCmd.h | 21 ++++++++-------- 3 files changed, 32 insertions(+), 30 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index c6089aa6..bd3bea5a 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -489,10 +489,9 @@ void AcsController::performPointingCtrl() { actuatorCmd.cmdSpeedToRws( sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRws, - cmdSpeedRws, acsParameters.onBoardParams.sampleTime, - acsParameters.rwHandlingParameters.maxRwSpeed, - acsParameters.rwHandlingParameters.inertiaWheel); + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, + acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel, + acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws); if (enableAntiStiction) { ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws); } diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index d2fe2d65..17d195e8 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -5,8 +5,6 @@ #include #include -#include - #include "util/CholeskyDecomposition.h" #include "util/MathOperations.h" @@ -25,24 +23,30 @@ void ActuatorCmd::scalingTorqueRws(double *rwTrq, double maxTorque) { } } -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; - - // Calculating the commanded speed in RPM for every reaction wheel +void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, + const int32_t speedRw2, const int32_t speedRw3, + const double sampleTime, const double inertiaWheel, + const int32_t maxRwSpeed, const double *rwTorque, + int32_t *rwCmdSpeed) { + // concentrate RW speed values (in 0.1 [RPM]) in vector int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3}; + + // calculate required RW speed as sum of current RW speed and RW speed delta + // delta w_rw = delta t / I_RW * torque_RW [rad/s] 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}; + const double factor = sampleTime / inertiaWheel * RAD_PER_SEC_TO_RPM * 10; VectorOperations::mulScalar(rwTorque, factor, deltaSpeed, 4); + + // convert double to int32 + int32_t deltaSpeedInt[4] = {0, 0, 0, 0}; for (int i = 0; i < 4; i++) { deltaSpeedInt[i] = std::round(deltaSpeed[i]); } + + // sum of current RW speed and RW speed delta VectorOperations::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4); - VectorOperations::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4); + + // crop values which would exceed the maximum possible RPM for (uint8_t i = 0; i < 4; i++) { if (rwCmdSpeed[i] > maxRwSpeed) { rwCmdSpeed[i] = maxRwSpeed; @@ -54,11 +58,11 @@ void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t spee void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, const double *inverseAlignment, double maxDipol) { - // Convert to actuator frame + // convert to actuator frame double dipolMomentActuatorDouble[3] = {0, 0, 0}; MatrixOperations::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3, 1); - // Scaling along largest element if dipol exceeds maximum + // scaling along largest element if dipole exceeds maximum uint8_t maxIdx = 0; VectorOperations::maxAbsValue(dipolMomentActuatorDouble, 3, &maxIdx); double maxAbsValue = abs(dipolMomentActuatorDouble[maxIdx]); diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 5d5d47f3..2c9624a9 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -1,9 +1,8 @@ #ifndef ACTUATORCMD_H_ #define ACTUATORCMD_H_ -#include "MultiplicativeKalmanFilter.h" -#include "SensorProcessing.h" -#include "SensorValues.h" +#include + class ActuatorCmd { public: @@ -19,17 +18,16 @@ class ActuatorCmd { void scalingTorqueRws(double *rwTrq, double maxTorque); /* - * @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction - * wheels, also will calculate the needed revolutions per minute for the RWs, which will be given - * as Input to the RWs - * @param: rwTrqIn given torque from pointing controller - * rwTrqNS Nullspace torque + * @brief: cmdSpeedToRws() Calculates the RPM for the reaction wheel configuration, + * given the required torque calculated by the controller. Will also scale down the RPM of the + * wheels if they exceed the maximum possible RPM + * @param: rwTrq given torque from pointing controller * rwCmdSpeed output revolutions per minute for every * reaction wheel */ - void 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); + void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, + const int32_t speedRw3, const double sampleTime, const double inertiaWheel, + const int32_t maxRwSpeed, const double *rwTorque, int32_t *rwCmdSpeed); /* * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques @@ -42,6 +40,7 @@ class ActuatorCmd { protected: private: + static constexpr double RAD_PER_SEC_TO_RPM = 60 / (2 * M_PI); }; #endif /* ACTUATORCMD_H_ */