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

ACS Ptg Ctrl Fixes #643

Merged
muellerr merged 35 commits from acs-ptg-ctrl-fixes-2 into v3.0.0-dev 2023-06-07 11:49:10 +02:00
Conversation 0 Commits 35 Files Changed 15 +33 -38
6 changed files with 33 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 6705ede2fc - Show all commits

27
mission/controller/AcsController.cpp
View File

@ -210,9 +210,8 @@ void AcsController::performSafe() {
break; break;
} }
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
*acsParameters.magnetorquerParameter.inverseAlignment, acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
acsParameters.magnetorquerParameter.dipolMax);
// detumble check and switch // detumble check and switch
if (mekfData.satRotRateMekf.isValid() && acsParameters.safeModeControllerParameters.useMekf && if (mekfData.satRotRateMekf.isValid() && acsParameters.safeModeControllerParameters.useMekf &&
@ -234,8 +233,8 @@ void AcsController::performSafe() {
} }
updateCtrlValData(errAng, safeCtrlStrat); updateCtrlValData(errAng, safeCtrlStrat);
updateActuatorCmdData(cmdDipolMtqs); updateActuatorCmdData(cmdDipoleMtqs);
commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
acsParameters.magnetorquerParameter.torqueDuration); acsParameters.magnetorquerParameter.torqueDuration);
} }
@ -287,9 +286,8 @@ void AcsController::performDetumble() {
break; break;
} }
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
*acsParameters.magnetorquerParameter.inverseAlignment, acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
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) <
@ -310,8 +308,8 @@ void AcsController::performDetumble() {
} }
disableCtrlValData(); disableCtrlValData();
updateActuatorCmdData(cmdDipolMtqs); updateActuatorCmdData(cmdDipoleMtqs);
commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
acsParameters.magnetorquerParameter.torqueDuration); acsParameters.magnetorquerParameter.torqueDuration);
} }
@ -495,13 +493,12 @@ void AcsController::performPointingCtrl() {
if (enableAntiStiction) { if (enableAntiStiction) {
ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws); ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
} }
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs, actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
*acsParameters.magnetorquerParameter.inverseAlignment, acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs);
acsParameters.magnetorquerParameter.dipolMax);
updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate); updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipolMtqs); updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs);
commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
acsParameters.rwHandlingParameters.rampTime); acsParameters.rwHandlingParameters.rampTime);

2
mission/controller/AcsController.h
View File

@ -69,7 +69,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
bool mekfLost = false; bool mekfLost = false;
int32_t cmdSpeedRws[4] = {0, 0, 0, 0}; int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
int16_t cmdDipolMtqs[3] = {0, 0, 0}; int16_t cmdDipoleMtqs[3] = {0, 0, 0};
#if OBSW_THREAD_TRACING == 1 #if OBSW_THREAD_TRACING == 1
uint32_t opCounter = 0; uint32_t opCounter = 0;

2
mission/controller/acs/AcsParameters.cpp
View File

@ -690,7 +690,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->setMatrix(magnetorquerParameter.inverseAlignment); parameterWrapper->setMatrix(magnetorquerParameter.inverseAlignment);
break; break;
case 0x5: case 0x5:
parameterWrapper->set(magnetorquerParameter.dipolMax); parameterWrapper->set(magnetorquerParameter.dipoleMax);
break; break;
case 0x6: case 0x6:
parameterWrapper->set(magnetorquerParameter.torqueDuration); parameterWrapper->set(magnetorquerParameter.torqueDuration);

2
mission/controller/acs/AcsParameters.h
View File

@ -931,7 +931,7 @@ class AcsParameters : public HasParametersIF {
double mtq2orientationMatrix[3][3] = {{0, 0, 1}, {0, 1, 0}, {-1, 0, 0}}; double mtq2orientationMatrix[3][3] = {{0, 0, 1}, {0, 1, 0}, {-1, 0, 0}};
double alignmentMatrixMtq[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; double alignmentMatrixMtq[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}};
double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}}; double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}};
double dipolMax = 0.2; // [Am^2] double dipoleMax = 0.2; // [Am^2]
uint16_t torqueDuration = 300; // [ms] uint16_t torqueDuration = 300; // [ms]
} magnetorquerParameter; } magnetorquerParameter;

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

@ -5,9 +5,6 @@
#include <fsfw/globalfunctions/math/QuaternionOperations.h> #include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h> #include <fsfw/globalfunctions/math/VectorOperations.h>
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
ActuatorCmd::ActuatorCmd() {} ActuatorCmd::ActuatorCmd() {}
ActuatorCmd::~ActuatorCmd() {} ActuatorCmd::~ActuatorCmd() {}
@ -56,24 +53,24 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1,
} }
} }
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, void ActuatorCmd::cmdDipoleMtq(const double *inverseAlignment, const double maxDipole,
const double *inverseAlignment, double maxDipol) { const double *dipoleMoment, int16_t *dipoleMomentActuator) {
// convert to actuator frame // convert to actuator frame
double dipolMomentActuatorDouble[3] = {0, 0, 0}; double dipoleMomentActuatorDouble[3] = {0, 0, 0};
MatrixOperations<double>::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3, MatrixOperations<double>::multiply(inverseAlignment, dipoleMoment, dipoleMomentActuatorDouble, 3, 3,
1); 1);
// scaling along largest element if dipole exceeds maximum // scaling along largest element if dipole exceeds maximum
uint8_t maxIdx = 0; uint8_t maxIdx = 0;
VectorOperations<double>::maxAbsValue(dipolMomentActuatorDouble, 3, &maxIdx); VectorOperations<double>::maxAbsValue(dipoleMomentActuatorDouble, 3, &maxIdx);
double maxAbsValue = abs(dipolMomentActuatorDouble[maxIdx]); double maxAbsValue = abs(dipoleMomentActuatorDouble[maxIdx]);
if (maxAbsValue > maxDipol) { if (maxAbsValue > maxDipole) {
double scalingFactor = maxDipol / maxAbsValue; double scalingFactor = maxDipole / maxAbsValue;
VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, scalingFactor, VectorOperations<double>::mulScalar(dipoleMomentActuatorDouble, scalingFactor,
dipolMomentActuatorDouble, 3); dipoleMomentActuatorDouble, 3);
} }
// scale dipole from 1 Am^2 to 1e^-4 Am^2 // scale dipole from 1 Am^2 to 1e^-4 Am^2
VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, 1e4, dipolMomentActuatorDouble, 3); VectorOperations<double>::mulScalar(dipoleMomentActuatorDouble, 1e4, dipoleMomentActuatorDouble, 3);
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
dipolMomentActuator[i] = std::round(dipolMomentActuatorDouble[i]); dipoleMomentActuator[i] = std::round(dipoleMomentActuatorDouble[i]);
} }
} }

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

@ -30,13 +30,14 @@ class ActuatorCmd {
const int32_t maxRwSpeed, const double *rwTorque, int32_t *rwCmdSpeed); const int32_t maxRwSpeed, const double *rwTorque, int32_t *rwCmdSpeed);
/* /*
* @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques * @brief: cmdDipoleMtq() gives the commanded dipole moment for the
* magnetorquer
* *
* @param: dipolMoment given dipol moment in spacecraft frame * @param: dipoleMoment given dipole moment in spacecraft frame
* dipolMomentActuator resulting dipol moment in actuator reference frame * dipoleMomentActuator resulting dipole moment in actuator reference frame
*/ */
void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, void cmdDipoleMtq(const double *inverseAlignment, const double maxDipole,
const double *inverseAlignment, double maxDipol); const double *dipoleMoment, int16_t *dipoleMomentActuator);
protected: protected:
private: private: