eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
mission/controller/AcsController.cpp
View File

@ -170,7 +170,9 @@ void AcsController::performSafe() {
sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); 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 // detumble check and switch
if (mekfData.satRotRateMekf.isValid() && if (mekfData.satRotRateMekf.isValid() &&
@ -218,7 +220,9 @@ void AcsController::performDetumble() {
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, &acsParameters.magnetorquerParameter); actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
*acsParameters.magnetorquerParameter.inverseAlignment,
acsParameters.magnetorquerParameter.dipolMax);
if (mekfData.satRotRateMekf.isValid() && if (mekfData.satRotRateMekf.isValid() &&
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) < VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
@ -300,7 +304,8 @@ void AcsController::performPointingCtrl() {
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters); actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled,
acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -324,7 +329,8 @@ void AcsController::performPointingCtrl() {
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters); actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled,
acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -345,7 +351,8 @@ void AcsController::performPointingCtrl() {
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters); actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled,
acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -369,7 +376,8 @@ void AcsController::performPointingCtrl() {
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters); actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled,
acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -392,7 +400,8 @@ void AcsController::performPointingCtrl() {
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters); actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled,
acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -409,8 +418,11 @@ void AcsController::performPointingCtrl() {
actuatorCmd.cmdSpeedToRws( actuatorCmd.cmdSpeedToRws(
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRwsScaled,
cmdSpeedRws, &acsParameters); cmdSpeedRws, acsParameters.onBoardParams.sampleTime,
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs, &acsParameters.magnetorquerParameter); acsParameters.rwHandlingParameters.inertiaWheel);
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs,
*acsParameters.magnetorquerParameter.inverseAlignment,
acsParameters.magnetorquerParameter.dipolMax);
updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate); updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs); updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs);

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

@ -14,11 +14,7 @@ ActuatorCmd::ActuatorCmd() {}
ActuatorCmd::~ActuatorCmd() {} ActuatorCmd::~ActuatorCmd() {}
void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled, void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled, double maxTorque) {
AcsParameters::RwHandlingParameters *rwHandlingParameters) {
// Scaling the commanded torque to a maximum value
double maxTrq = rwHandlingParameters->maxTrq;
double maxValue = 0; double maxValue = 0;
for (int i = 0; i < 4; i++) { // size of torque, always 4 ? for (int i = 0; i < 4; i++) { // size of torque, always 4 ?
if (abs(rwTrq[i]) > maxValue) { if (abs(rwTrq[i]) > maxValue) {
@ -26,16 +22,15 @@ void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled,
} }
} }
if (maxValue > maxTrq) { if (maxValue > maxTorque) {
double scalingFactor = maxTrq / maxValue; double scalingFactor = maxTorque / maxValue;
VectorOperations<double>::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4); VectorOperations<double>::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4);
} }
} }
void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2,
const int32_t speedRw2, const int32_t speedRw3, int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed,
const double *rwTorque, int32_t *rwCmdSpeed, double sampleTime, double inertiaWheel) {
AcsParameters *acsParameters) {
using namespace Math; using namespace Math;
// Calculating the commanded speed in RPM for every reaction wheel // 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 deltaSpeed[4] = {0, 0, 0, 0};
double radToRpm = 60 / (2 * PI); // factor for conversion to RPM double radToRpm = 60 / (2 * PI); // factor for conversion to RPM
// W_RW = Torque_RW / I_RW * delta t [rad/s] // W_RW = Torque_RW / I_RW * delta t [rad/s]
double factor = acsParameters->onBoardParams.sampleTime / double factor = sampleTime / inertiaWheel * radToRpm;
acsParameters->rwHandlingParameters.inertiaWheel * radToRpm;
int32_t deltaSpeedInt[4] = {0, 0, 0, 0}; int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4); VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4);
for (int i = 0; i < 4; i++) { 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, void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator,
AcsParameters::MagnetorquerParameter *magnetorquerParameter) { const double *inverseAlignment, double maxDipol) {
// Convert to actuator frame // Convert to actuator frame
double dipolMomentActuatorDouble[3] = {0, 0, 0}; double dipolMomentActuatorDouble[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*magnetorquerParameter->inverseAlignment, dipolMoment, MatrixOperations<double>::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3,
dipolMomentActuatorDouble, 3, 3, 1); 1);
// Scaling along largest element if dipol exceeds maximum // Scaling along largest element if dipol exceeds maximum
double maxDipol = magnetorquerParameter->dipolMax;
double maxValue = 0; double maxValue = 0;
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
if (abs(dipolMomentActuator[i]) > maxDipol) { if (abs(dipolMomentActuator[i]) > maxDipol) {

14
mission/controller/acs/ActuatorCmd.h
View File

@ -1,14 +1,13 @@
#ifndef ACTUATORCMD_H_ #ifndef ACTUATORCMD_H_
#define ACTUATORCMD_H_ #define ACTUATORCMD_H_
#include "AcsParameters.h"
#include "MultiplicativeKalmanFilter.h" #include "MultiplicativeKalmanFilter.h"
#include "SensorProcessing.h" #include "SensorProcessing.h"
#include "SensorValues.h" #include "SensorValues.h"
class ActuatorCmd { class ActuatorCmd {
public: public:
ActuatorCmd(); // Input mode ? ActuatorCmd();
virtual ~ActuatorCmd(); virtual ~ActuatorCmd();
/* /*
@ -17,8 +16,7 @@ class ActuatorCmd {
* @param: rwTrq given torque for reaction wheels * @param: rwTrq given torque for reaction wheels
* rwTrqScaled possible scaled torque * rwTrqScaled possible scaled torque
*/ */
void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled, void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled, double maxTorque);
AcsParameters::RwHandlingParameters *rwHandlingParameters);
/* /*
* @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction * @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction
@ -29,9 +27,9 @@ class ActuatorCmd {
* rwCmdSpeed output revolutions per minute for every * rwCmdSpeed output revolutions per minute for every
* reaction wheel * reaction wheel
*/ */
void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, void cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, int32_t speedRw3,
const int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed, const double *rwTorque, int32_t *rwCmdSpeed, double sampleTime,
AcsParameters *acsParameters); double inertiaWheel);
/* /*
* @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques
@ -40,7 +38,7 @@ class ActuatorCmd {
* dipolMomentActuator resulting dipol moment in actuator reference frame * dipolMomentActuator resulting dipol moment in actuator reference frame
*/ */
void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator,
AcsParameters::MagnetorquerParameter *magnetorquerParameter); const double *inverseAlignment, double maxDipol);
protected: protected:
private: private: