eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 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 4 Commits 35 Files Changed 15 +1213 -2244
56 changed files with 1213 additions and 2244 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 8a65aca7b8 - Show all commits

40
mission/controller/AcsController.cpp
View File

@@ -375,10 +375,10 @@ void AcsController::performPointingCtrl() {
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate, ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace( ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters,
&acsParameters.idleModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
@@ -399,10 +399,10 @@ void AcsController::performPointingCtrl() {
errorSatRotRate, errorAngle); errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate, ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace( ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters,
&acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
@@ -420,10 +420,10 @@ void AcsController::performPointingCtrl() {
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate, ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace( ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters,
&acsParameters.gsTargetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
@@ -444,10 +444,10 @@ void AcsController::performPointingCtrl() {
errorSatRotRate, errorAngle); errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate, ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace( ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters,
&acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
@@ -467,10 +467,10 @@ void AcsController::performPointingCtrl() {
errorSatRotRate, errorAngle); errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate, ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace( ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters,
&acsParameters.inertialModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(

8
mission/controller/acs/control/PtgCtrl.cpp
View File

@@ -96,11 +96,11 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
} }
void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
const int32_t *speedRw0, const int32_t *speedRw1, const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) { const int32_t speedRw3, double *rwTrqNs) {
// concentrate RW speeds as vector and convert to double // concentrate RW speeds as vector and convert to double
double speedRws[4] = {static_cast<double>(*speedRw0), static_cast<double>(*speedRw1), double speedRws[4] = {static_cast<double>(speedRw0), static_cast<double>(speedRw1),
static_cast<double>(*speedRw2), static_cast<double>(*speedRw3)}; static_cast<double>(speedRw2), static_cast<double>(speedRw3)};
// calculate RPM offset utilizing the nullspace // calculate RPM offset utilizing the nullspace
double rpmOffset[4] = {0, 0, 0, 0}; double rpmOffset[4] = {0, 0, 0, 0};

4
mission/controller/acs/control/PtgCtrl.h
View File

@@ -27,8 +27,8 @@ class PtgCtrl {
const double *deltaRate, const double *rwPseudoInv, double *torqueRws); const double *deltaRate, const double *rwPseudoInv, double *torqueRws);
void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
const int32_t *speedRw3, double *rwTrqNs); const int32_t speedRw3, double *rwTrqNs);
void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
const double *magFieldB, const bool magFieldBValid, const double *satRate, const double *magFieldB, const bool magFieldBValid, const double *satRate,