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

function now handles actuator cmd
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-02-10 11:27:25 +01:00
parent 01d6060111
commit ea32d87c25
2 changed files with 66 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

163
mission/controller/AcsController.cpp
View File

@ -116,10 +116,10 @@ void AcsController::performSafe() {
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
&mekfData, &validMekf); &mekfData, &validMekf);
// Give desired satellite rate and sun direction to align // give desired satellite rate and sun direction to align
double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0}; double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
// IF MEKF is working // if MEKF is working
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
bool magMomMtqValid = false; bool magMomMtqValid = false;
if (validMekf == returnvalue::OK) { if (validMekf == returnvalue::OK) {
@ -137,8 +137,8 @@ void AcsController::performSafe() {
sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
} }
double dipolCmdUnits[3] = {0, 0, 0}; int16_t cmdDipolMtqs[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
{ {
PoolReadGuard pg(&ctrlValData); PoolReadGuard pg(&ctrlValData);
@ -180,33 +180,15 @@ void AcsController::performSafe() {
actuatorCmdData.rwTargetTorque.setValid(false); actuatorCmdData.rwTargetTorque.setValid(false);
std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t)); std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t));
actuatorCmdData.rwTargetSpeed.setValid(false); actuatorCmdData.rwTargetSpeed.setValid(false);
std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(int16_t)); std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
actuatorCmdData.mtqTargetDipole.setValid(true); actuatorCmdData.mtqTargetDipole.setValid(true);
actuatorCmdData.setValidity(true, false); actuatorCmdData.setValidity(true, false);
} }
} }
// {
// PoolReadGuard pg(&dipoleSet); commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// MutexGuard mg(torquer::lazyLock()); acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
// torquer::NEW_ACTUATION_FLAG = true; acsParameters.rwHandlingParameters.rampTime);
// dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2], 0);
// }
// {
// PoolReadGuard pg(&rw1SpeedSet);
// rw1SpeedSet.setRwSpeed(0);
// }
// {
// PoolReadGuard pg(&rw2SpeedSet);
// rw2SpeedSet.setRwSpeed(0);
// }
// {
// PoolReadGuard pg(&rw3SpeedSet);
// rw3SpeedSet.setRwSpeed(0);
// }
// {
// PoolReadGuard pg(&rw4SpeedSet);
// rw4SpeedSet.setRwSpeed(0);
// }
} }
void AcsController::performDetumble() { void AcsController::performDetumble() {
@ -223,8 +205,8 @@ 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);
double dipolCmdUnits[3] = {0, 0, 0}; int16_t cmdDipolMtqs[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
if (mekfData.satRotRateMekf.isValid() && if (mekfData.satRotRateMekf.isValid() &&
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) < VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
@ -243,10 +225,6 @@ void AcsController::performDetumble() {
triggerEvent(acs::SAFE_RATE_RECOVERY); triggerEvent(acs::SAFE_RATE_RECOVERY);
} }
int16_t cmdDipolUnitsInt[3] = {0, 0, 0};
for (int i = 0; i < 3; ++i) {
cmdDipolUnitsInt[i] = std::round(dipolCmdUnits[i]);
}
{ {
PoolReadGuard pg(&actuatorCmdData); PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
@ -254,34 +232,15 @@ void AcsController::performDetumble() {
actuatorCmdData.rwTargetTorque.setValid(false); actuatorCmdData.rwTargetTorque.setValid(false);
std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t)); std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t));
actuatorCmdData.rwTargetSpeed.setValid(false); actuatorCmdData.rwTargetSpeed.setValid(false);
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
actuatorCmdData.mtqTargetDipole.setValid(true); actuatorCmdData.mtqTargetDipole.setValid(true);
actuatorCmdData.setValidity(true, false); actuatorCmdData.setValidity(true, false);
} }
} }
// {
// PoolReadGuard pg(&dipoleSet); commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// MutexGuard mg(torquer::lazyLock()); acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
// torquer::NEW_ACTUATION_FLAG = true; acsParameters.rwHandlingParameters.rampTime);
// dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2],
// torqueDuration);
// }
// {
// PoolReadGuard pg(&rw1SpeedSet);
// rw1SpeedSet.setRwSpeed(0);
// }
// {
// PoolReadGuard pg(&rw2SpeedSet);
// rw2SpeedSet.setRwSpeed(0);
// }
// {
// PoolReadGuard pg(&rw3SpeedSet);
// rw3SpeedSet.setRwSpeed(0);
// }
// {
// PoolReadGuard pg(&rw4SpeedSet);
// rw4SpeedSet.setRwSpeed(0);
// }
} }
void AcsController::performPointingCtrl() { void AcsController::performPointingCtrl() {
@ -304,7 +263,7 @@ void AcsController::performPointingCtrl() {
guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}; double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0};
double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol double mgtDpDes[3] = {0, 0, 0};
switch (submode) { switch (submode) {
case acs::PTG_IDLE: case acs::PTG_IDLE:
@ -424,60 +383,62 @@ void AcsController::performPointingCtrl() {
if (enableAntiStiction) { if (enableAntiStiction) {
bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET?
int32_t rwSpeed[4] = { int32_t rwSpeed[4] = {sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
(sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value), sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value};
(sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)};
ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled); ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled);
} }
double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value), actuatorCmd.cmdSpeedToRws(sensorValues.rw1Set.currSpeed.value,
&(sensorValues.rw2Set.currSpeed.value), sensorValues.rw2Set.currSpeed.value,
&(sensorValues.rw3Set.currSpeed.value), sensorValues.rw3Set.currSpeed.value,
&(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws); sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws);
actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); int16_t cmdDipolMtqs[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs);
int16_t cmdDipolUnitsInt[3] = {0, 0, 0};
for (int i = 0; i < 3; ++i) {
cmdDipolUnitsInt[i] = std::round(dipolUnits[i]);
}
int32_t cmdRwSpeedInt[4] = {0, 0, 0, 0};
for (int i = 0; i < 4; ++i) {
cmdRwSpeedInt[i] = std::round(cmdSpeedRws[i]);
}
{ {
PoolReadGuard pg(&actuatorCmdData); PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double)); std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double));
std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdRwSpeedInt, 4 * sizeof(int32_t)); std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdSpeedRws, 4 * sizeof(int32_t));
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
actuatorCmdData.setValidity(true, true); actuatorCmdData.setValidity(true, true);
} }
} }
// {
// PoolReadGuard pg(&dipoleSet); commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// MutexGuard mg(torquer::lazyLock()); acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
// torquer::NEW_ACTUATION_FLAG = true; cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
// dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], acsParameters.rwHandlingParameters.rampTime);
// torqueDuration); }
// }
// { ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
// PoolReadGuard pg(&rw1SpeedSet); uint16_t dipoleTorqueDuration, int32_t rw1Speed,
// rw1SpeedSet.setRwSpeed(cmdRwSpeedInt[0]); int32_t rw2Speed, int32_t rw3Speed, int32_t rw4Speed,
// } uint16_t rampTime) {
// { {
// PoolReadGuard pg(&rw2SpeedSet); PoolReadGuard pg(&dipoleSet);
// rw2SpeedSet.setRwSpeed(cmdRwSpeedInt[1]); MutexGuard mg(torquer::lazyLock());
// } torquer::NEW_ACTUATION_FLAG = true;
// { dipoleSet.setDipoles(xDipole, yDipole, zDipole, dipoleTorqueDuration);
// PoolReadGuard pg(&rw3SpeedSet); }
// rw3SpeedSet.setRwSpeed(cmdRwSpeedInt[2]); {
// } PoolReadGuard pg(&rw1SpeedSet);
// { rw1SpeedSet.setRwSpeed(rw1Speed, rampTime);
// PoolReadGuard pg(&rw4SpeedSet); }
// rw4SpeedSet.setRwSpeed(cmdRwSpeedInt[3]); {
// } PoolReadGuard pg(&rw2SpeedSet);
rw2SpeedSet.setRwSpeed(rw2Speed, rampTime);
}
{
PoolReadGuard pg(&rw3SpeedSet);
rw3SpeedSet.setRwSpeed(rw3Speed, rampTime);
}
{
PoolReadGuard pg(&rw4SpeedSet);
rw4SpeedSet.setRwSpeed(rw4Speed, rampTime);
}
return returnvalue::OK;
} }
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,

4
mission/controller/AcsController.h
View File

@ -70,6 +70,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
void modeChanged(Mode_t mode, Submode_t submode); void modeChanged(Mode_t mode, Submode_t submode);
void announceMode(bool recursive); void announceMode(bool recursive);
ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
uint16_t dipoleTorqueDuration, int32_t rw1Speed, int32_t rw2Speed,
int32_t rw3Speed, int32_t rw4Speed, uint16_t rampTime);
/* ACS Sensor Values */ /* ACS Sensor Values */
ACS::SensorValues sensorValues; ACS::SensorValues sensorValues;