removed OutputValues
amended sumbode list inserted writes to output DataPools
This commit is contained in:
@ -20,6 +20,7 @@ AcsController::AcsController(object_id_t objectId)
|
||||
gyrDataProcessed(this),
|
||||
gpsDataProcessed(this),
|
||||
mekfData(this),
|
||||
ctrlValData(this),
|
||||
actuatorCmdData(this) {}
|
||||
|
||||
ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
|
||||
@ -48,7 +49,9 @@ void AcsController::performControlOperation() {
|
||||
case SUBMODE_DETUMBLE:
|
||||
performDetumble();
|
||||
break;
|
||||
case SUBMODE_PTG_GS:
|
||||
case SUBMODE_PTG_TARGET:
|
||||
case SUBMODE_PTG_NADIR:
|
||||
case SUBMODE_PTG_INERTIAL:
|
||||
performPointingCtrl();
|
||||
break;
|
||||
}
|
||||
@ -80,7 +83,7 @@ void AcsController::performControlOperation() {
|
||||
|
||||
// DEBUG : REMOVE AFTER COMPLETION
|
||||
mode = MODE_ON;
|
||||
submode = SUBMODE_DETUMBLE;
|
||||
submode = SUBMODE_SAFE;
|
||||
// DEBUG END
|
||||
}
|
||||
|
||||
@ -90,7 +93,6 @@ void AcsController::performSafe() {
|
||||
// another place since we have to do it for every mode regardless of safe or not
|
||||
|
||||
ACS::SensorValues sensorValues;
|
||||
ACS::OutputValues outputValues;
|
||||
|
||||
timeval now;
|
||||
Clock::getClock_timeval(&now);
|
||||
@ -98,38 +100,54 @@ void AcsController::performSafe() {
|
||||
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
|
||||
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
|
||||
ReturnValue_t validMekf;
|
||||
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
|
||||
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
|
||||
&mekfData, &validMekf);
|
||||
|
||||
// Give desired satellite rate and sun direction to align
|
||||
double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
|
||||
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
|
||||
// IF MEKF is working
|
||||
double magMomMtq[3] = {0, 0, 0};
|
||||
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
|
||||
bool magMomMtqValid = false;
|
||||
if (validMekf == returnvalue::OK) {
|
||||
safeCtrl.safeMekf(now, (outputValues.quatMekfBJ), &(outputValues.quatMekfBJValid),
|
||||
(outputValues.magFieldModel), &(outputValues.magFieldModelValid),
|
||||
(outputValues.sunDirModel), &(outputValues.sunDirModelValid),
|
||||
(outputValues.satRateMekf), &(outputValues.satRateMekfValid), sunTargetDir,
|
||||
satRateSafe, magMomMtq, &magMomMtqValid);
|
||||
safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(),
|
||||
mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(),
|
||||
susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(),
|
||||
mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(),
|
||||
sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
|
||||
} else {
|
||||
safeCtrl.safeNoMekf(now, outputValues.sunDirEst, &outputValues.sunDirEstValid,
|
||||
outputValues.sunVectorDerivative, &(outputValues.sunVectorDerivativeValid),
|
||||
outputValues.magFieldEst, &(outputValues.magFieldEstValid),
|
||||
outputValues.magneticFieldVectorDerivative,
|
||||
&(outputValues.magneticFieldVectorDerivativeValid), sunTargetDir,
|
||||
satRateSafe, magMomMtq, &magMomMtqValid);
|
||||
safeCtrl.safeNoMekf(
|
||||
now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(),
|
||||
susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.isValid(),
|
||||
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
|
||||
mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(),
|
||||
sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
|
||||
}
|
||||
|
||||
double dipolCmdUnits[3] = {0, 0, 0};
|
||||
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
|
||||
|
||||
{
|
||||
PoolReadGuard pg(&ctrlValData);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
double zeroQuat[4] = {0, 0, 0, 0};
|
||||
std::memcpy(ctrlValData.tgtQuat.value, zeroQuat, 4 * sizeof(double));
|
||||
ctrlValData.tgtQuat.setValid(false);
|
||||
std::memcpy(ctrlValData.errQuat.value, zeroQuat, 4 * sizeof(double));
|
||||
ctrlValData.errQuat.setValid(false);
|
||||
ctrlValData.errAng.value = errAng;
|
||||
ctrlValData.errAng.setValid(true);
|
||||
ctrlValData.setValidity(true, false);
|
||||
}
|
||||
}
|
||||
|
||||
// Detumble check and switch
|
||||
if (outputValues.satRateMekfValid && VectorOperations<double>::norm(outputValues.satRateMekf, 3) >
|
||||
acsParameters.detumbleParameter.omegaDetumbleStart) {
|
||||
if (mekfData.satRotRateMekf.isValid() &&
|
||||
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) >
|
||||
acsParameters.detumbleParameter.omegaDetumbleStart) {
|
||||
detumbleCounter++;
|
||||
} else if (outputValues.satRateEstValid &&
|
||||
VectorOperations<double>::norm(outputValues.satRateEst, 3) >
|
||||
} else if (gyrDataProcessed.gyrVecTot.isValid() &&
|
||||
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) >
|
||||
acsParameters.detumbleParameter.omegaDetumbleStart) {
|
||||
detumbleCounter++;
|
||||
} else {
|
||||
@ -139,12 +157,31 @@ void AcsController::performSafe() {
|
||||
submode = SUBMODE_DETUMBLE;
|
||||
detumbleCounter = 0;
|
||||
}
|
||||
// commanding.magnetorquesDipol();
|
||||
|
||||
{
|
||||
PoolReadGuard pg(&actuatorCmdData);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
double zeroVec[3] = {0, 0, 0, 0};
|
||||
std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(double));
|
||||
actuatorCmdData.rwTargetTorque.setValid(false);
|
||||
std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(double));
|
||||
actuatorCmdData.rwTargetSpeed.setValid(false);
|
||||
std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(double));
|
||||
actuatorCmdData.mtqTargetDipole.setValid(true);
|
||||
actuatorCmdData.setValidity(true, false);
|
||||
}
|
||||
}
|
||||
// {
|
||||
// PoolReadGuard pg(&dipoleSet);
|
||||
// MutexGuard mg(torquer::lazyLock());
|
||||
// torquer::NEW_ACTUATION_FLAG = true;
|
||||
// dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2],
|
||||
//torqueDuration);
|
||||
// }
|
||||
}
|
||||
|
||||
void AcsController::performDetumble() {
|
||||
ACS::SensorValues sensorValues;
|
||||
ACS::OutputValues outputValues;
|
||||
|
||||
timeval now;
|
||||
Clock::getClock_timeval(&now);
|
||||
@ -152,20 +189,22 @@ void AcsController::performDetumble() {
|
||||
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
|
||||
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
|
||||
ReturnValue_t validMekf;
|
||||
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
|
||||
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
|
||||
&mekfData, &validMekf);
|
||||
|
||||
double magMomMtq[3] = {0, 0, 0};
|
||||
detumble.bDotLaw(outputValues.magneticFieldVectorDerivative,
|
||||
&outputValues.magneticFieldVectorDerivativeValid, outputValues.magFieldEst,
|
||||
&outputValues.magFieldEstValid, magMomMtq);
|
||||
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
|
||||
mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
|
||||
mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
|
||||
double dipolCmdUnits[3] = {0, 0, 0};
|
||||
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
|
||||
|
||||
if (outputValues.satRateMekfValid && VectorOperations<double>::norm(outputValues.satRateMekf, 3) <
|
||||
acsParameters.detumbleParameter.omegaDetumbleEnd) {
|
||||
if (mekfData.satRotRateMekf.isValid() &&
|
||||
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
|
||||
acsParameters.detumbleParameter.omegaDetumbleEnd) {
|
||||
detumbleCounter++;
|
||||
} else if (outputValues.satRateEstValid &&
|
||||
VectorOperations<double>::norm(outputValues.satRateEst, 3) <
|
||||
} else if (gyrDataProcessed.gyrVecTot.isValid() &&
|
||||
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
|
||||
acsParameters.detumbleParameter.omegaDetumbleEnd) {
|
||||
detumbleCounter++;
|
||||
} else {
|
||||
@ -175,11 +214,31 @@ void AcsController::performDetumble() {
|
||||
submode = SUBMODE_SAFE;
|
||||
detumbleCounter = 0;
|
||||
}
|
||||
|
||||
{
|
||||
PoolReadGuard pg(&actuatorCmdData);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
double zeroVec[3] = {0, 0, 0, 0};
|
||||
std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(double));
|
||||
actuatorCmdData.rwTargetTorque.setValid(false);
|
||||
std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(double));
|
||||
actuatorCmdData.rwTargetSpeed.setValid(false);
|
||||
std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(double));
|
||||
actuatorCmdData.mtqTargetDipole.setValid(true);
|
||||
actuatorCmdData.setValidity(true, false);
|
||||
}
|
||||
}
|
||||
// {
|
||||
// PoolReadGuard pg(&dipoleSet);
|
||||
// MutexGuard mg(torquer::lazyLock());
|
||||
// torquer::NEW_ACTUATION_FLAG = true;
|
||||
// dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2],
|
||||
// torqueDuration);
|
||||
// }
|
||||
}
|
||||
|
||||
void AcsController::performPointingCtrl() {
|
||||
ACS::SensorValues sensorValues;
|
||||
ACS::OutputValues outputValues;
|
||||
|
||||
timeval now;
|
||||
Clock::getClock_timeval(&now);
|
||||
@ -187,12 +246,14 @@ void AcsController::performPointingCtrl() {
|
||||
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
|
||||
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
|
||||
ReturnValue_t validMekf;
|
||||
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
|
||||
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
|
||||
&mekfData, &validMekf);
|
||||
|
||||
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
|
||||
guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
|
||||
double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0};
|
||||
guidance.comparePtg(targetQuat, &outputValues, refSatRate, quatError, deltaRate);
|
||||
guidance.targetQuatPtg(&sensorValues, &mekfData, &susDataProcessed, now, targetQuat, refSatRate);
|
||||
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);
|
||||
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;
|
||||
@ -207,11 +268,29 @@ void AcsController::performPointingCtrl() {
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torquePtgRws,
|
||||
rwTrqNs, cmdSpeedRws);
|
||||
double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol
|
||||
ptgCtrl.ptgDesaturation(
|
||||
outputValues.magFieldEst, &outputValues.magFieldEstValid, outputValues.satRateMekf,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
||||
ptgCtrl.ptgDesaturation(mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
|
||||
mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value),
|
||||
&(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value),
|
||||
&(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
||||
actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
|
||||
|
||||
{
|
||||
PoolReadGuard pg(&actuatorCmdData);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double));
|
||||
std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdSpeedRws, 4 * sizeof(double));
|
||||
std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolUnits, 3 * sizeof(double));
|
||||
actuatorCmdData.setValidity(true, true);
|
||||
}
|
||||
}
|
||||
// {
|
||||
// PoolReadGuard pg(&dipoleSet);
|
||||
// MutexGuard mg(torquer::lazyLock());
|
||||
// torquer::NEW_ACTUATION_FLAG = true;
|
||||
// dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2],
|
||||
// torqueDuration);
|
||||
// }
|
||||
}
|
||||
|
||||
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
||||
@ -286,6 +365,11 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf);
|
||||
poolManager.subscribeForRegularPeriodicPacket({mekfData.getSid(), false, 5.0});
|
||||
// Ctrl Values
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng);
|
||||
poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), false, 5.0});
|
||||
// Actuator CMD
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed);
|
||||
@ -312,6 +396,8 @@ LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
|
||||
return &gpsDataProcessed;
|
||||
case acsctrl::MEKF_DATA:
|
||||
return &mekfData;
|
||||
case acsctrl::CTRL_VAL_DATA:
|
||||
return &ctrlValData;
|
||||
case acsctrl::ACTUATOR_CMD_DATA:
|
||||
return &actuatorCmdData;
|
||||
default:
|
||||
@ -328,7 +414,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
|
||||
return INVALID_SUBMODE;
|
||||
}
|
||||
} else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) {
|
||||
if ((submode > 5) || (submode < 2)) {
|
||||
if ((submode > 6) || (submode < 2)) {
|
||||
return INVALID_SUBMODE;
|
||||
} else {
|
||||
return returnvalue::OK;
|
||||
|
||||
Reference in New Issue
Block a user