From 26369039dada40cba14c824a606f8de523a3a159 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 27 Feb 2023 17:36:33 +0100 Subject: [PATCH] better solution for actuatorCmd --- mission/controller/AcsController.cpp | 30 ++++++++++++++++++-------- mission/controller/acs/ActuatorCmd.cpp | 27 +++++++++-------------- mission/controller/acs/ActuatorCmd.h | 14 ++++++------ 3 files changed, 37 insertions(+), 34 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index b056f093..d6d2fab6 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -170,7 +170,9 @@ void AcsController::performSafe() { sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); } - actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, &acsParameters.magnetorquerParameter); + actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, + *acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipolMax); // detumble check and switch if (mekfData.satRotRateMekf.isValid() && @@ -218,7 +220,9 @@ void AcsController::performDetumble() { detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); - actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, &acsParameters.magnetorquerParameter); + actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, + *acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipolMax); if (mekfData.satRotRateMekf.isValid() && VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < @@ -300,7 +304,8 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, + acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -324,7 +329,8 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, + acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -345,7 +351,8 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, + acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -369,7 +376,8 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, + acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -392,7 +400,8 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, + acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -409,8 +418,11 @@ void AcsController::performPointingCtrl() { actuatorCmd.cmdSpeedToRws( sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, - cmdSpeedRws, &acsParameters); - actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs, &acsParameters.magnetorquerParameter); + cmdSpeedRws, acsParameters.onBoardParams.sampleTime, + acsParameters.rwHandlingParameters.inertiaWheel); + actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs, + *acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipolMax); updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate); updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs); diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 7173151d..cbe6b504 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -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::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::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::multiply(*magnetorquerParameter->inverseAlignment, dipolMoment, - dipolMomentActuatorDouble, 3, 3, 1); + MatrixOperations::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) { diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index ca0deb5b..def3c1b6 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -1,14 +1,13 @@ #ifndef ACTUATORCMD_H_ #define ACTUATORCMD_H_ -#include "AcsParameters.h" #include "MultiplicativeKalmanFilter.h" #include "SensorProcessing.h" #include "SensorValues.h" class ActuatorCmd { public: - ActuatorCmd(); // Input mode ? + ActuatorCmd(); virtual ~ActuatorCmd(); /* @@ -17,8 +16,7 @@ class ActuatorCmd { * @param: rwTrq given torque for reaction wheels * rwTrqScaled possible scaled torque */ - void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled, - AcsParameters::RwHandlingParameters *rwHandlingParameters); + void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled, double maxTorque); /* * @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction @@ -29,9 +27,9 @@ 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, int32_t *rwCmdSpeed, - AcsParameters *acsParameters); + void cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, int32_t speedRw3, + const double *rwTorque, int32_t *rwCmdSpeed, double sampleTime, + double inertiaWheel); /* * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques @@ -40,7 +38,7 @@ class ActuatorCmd { * dipolMomentActuator resulting dipol moment in actuator reference frame */ void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, - AcsParameters::MagnetorquerParameter *magnetorquerParameter); + const double *inverseAlignment, double maxDipol); protected: private: