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

changed pointingParameters struct
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Robin Marquardt
2023-01-12 15:19:21 +01:00
parent 25867e76b1
commit d8c0ba19fd
7 changed files with 117 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
mission/controller/AcsController.cpp
View File

@ -269,30 +269,56 @@ void AcsController::performControlOperation() {
&mekfData, &validMekf);
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
double quatRef[4] = {0, 0, 0, 0};
uint8_t enableAntiStiction = true;
switch (submode) {
case SUBMODE_IDLE:
guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
targetQuat, refSatRate);
for ( uint8_t i = 0; i < 4; i++ ) {
quatRef[i] = acsParameters.targetModeControllerParameters.quatRef[i];
}
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
break;
case SUBMODE_PTG_TARGET:
guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now,
targetQuat, refSatRate);
for ( uint8_t i = 0; i < 4; i++ ) {
quatRef[i] = acsParameters.targetModeControllerParameters.quatRef[i];
}
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
break;
case SUBMODE_PTG_TARGET_GS:
guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed,
now, targetQuat, refSatRate);
for ( uint8_t i = 0; i < 4; i++ ) {
quatRef[i] = acsParameters.targetModeControllerParameters.quatRef[i];
}
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
break;
case SUBMODE_PTG_NADIR:
guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
refSatRate);
for ( uint8_t i = 0; i < 4; i++ ) {
quatRef[i] = acsParameters.nadirModeControllerParameters.quatRef[i];
}
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
break;
case SUBMODE_PTG_INERTIAL:
guidance.inertialQuatPtg(targetQuat, refSatRate);
for ( uint8_t i = 0; i < 4; i++ ) {
quatRef[i] = acsParameters.inertialModeControllerParameters.quatRef[i];
}
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
break;
}
double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed
guidance.comparePtg(targetQuat, &mekfData, refSatRate, quatErrorComplete, quatError, deltaRate);
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate);
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0;
@ -305,7 +331,7 @@ void AcsController::performControlOperation() {
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
if (acsParameters.pointingModeControllerParameters.enableAntiStiction) {
if ( enableAntiStiction ) {
bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET?
int32_t rwSpeed[4] = {
(sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value),