added ReceiversParameterMessagesIF and ParameterHelper to ACS Ctrl
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
This commit is contained in:
parent
4e74dc7bee
commit
887d193526
@ -14,6 +14,7 @@ AcsController::AcsController(object_id_t objectId)
|
||||
detumble(&acsParameters),
|
||||
ptgCtrl(&acsParameters),
|
||||
detumbleCounter{0},
|
||||
parameterHelper(this),
|
||||
mgmDataRaw(this),
|
||||
mgmDataProcessed(this),
|
||||
susDataRaw(this),
|
||||
@ -26,6 +27,23 @@ AcsController::AcsController(object_id_t objectId)
|
||||
actuatorCmdData(this) {}
|
||||
|
||||
ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
|
||||
ReturnValue_t result = actionHelper.handleActionMessage(message);
|
||||
if (result == returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
result = parameterHelper.handleParameterMessage(message);
|
||||
if (result == returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
MessageQueueId_t AcsController::getCommandQueue() const { return commandQueue->getId(); }
|
||||
|
||||
ReturnValue_t AcsController::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
ParameterWrapper* parameterWrapper,
|
||||
const ParameterWrapper* newValues,
|
||||
uint16_t startAtIndex) {
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
@ -53,6 +71,7 @@ void AcsController::performControlOperation() {
|
||||
break;
|
||||
case SUBMODE_IDLE:
|
||||
case SUBMODE_PTG_TARGET:
|
||||
case SUBMODE_PTG_TARGET_GS:
|
||||
case SUBMODE_PTG_NADIR:
|
||||
case SUBMODE_PTG_INERTIAL:
|
||||
performPointingCtrl();
|
||||
@ -83,9 +102,9 @@ void AcsController::performControlOperation() {
|
||||
copyGyrData();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AcsController::performSafe() {
|
||||
void AcsController::performSafe() {
|
||||
ACS::SensorValues sensorValues;
|
||||
|
||||
timeval now;
|
||||
@ -105,17 +124,19 @@ void AcsController::performSafe() {
|
||||
bool magMomMtqValid = false;
|
||||
if (validMekf == returnvalue::OK) {
|
||||
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);
|
||||
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, 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);
|
||||
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};
|
||||
@ -170,11 +191,12 @@ void AcsController::performSafe() {
|
||||
// PoolReadGuard pg(&dipoleSet);
|
||||
// MutexGuard mg(torquer::lazyLock());
|
||||
// torquer::NEW_ACTUATION_FLAG = true;
|
||||
// dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2], torqueDuration);
|
||||
// dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2],
|
||||
// torqueDuration);
|
||||
// }
|
||||
}
|
||||
}
|
||||
|
||||
void AcsController::performDetumble() {
|
||||
void AcsController::performDetumble() {
|
||||
ACS::SensorValues sensorValues;
|
||||
|
||||
timeval now;
|
||||
@ -187,9 +209,9 @@ void AcsController::performDetumble() {
|
||||
&mekfData, &validMekf);
|
||||
|
||||
double magMomMtq[3] = {0, 0, 0};
|
||||
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
|
||||
mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
|
||||
mgmDataProcessed.mgmVecTot.isValid(), 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);
|
||||
|
||||
@ -232,9 +254,9 @@ void AcsController::performDetumble() {
|
||||
// dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2],
|
||||
// torqueDuration);
|
||||
// }
|
||||
}
|
||||
}
|
||||
|
||||
void AcsController::performPointingCtrl() {
|
||||
void AcsController::performPointingCtrl() {
|
||||
ACS::SensorValues sensorValues;
|
||||
|
||||
timeval now;
|
||||
@ -253,10 +275,13 @@ void AcsController::performPointingCtrl() {
|
||||
targetQuat, refSatRate);
|
||||
break;
|
||||
case SUBMODE_PTG_TARGET:
|
||||
guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
|
||||
refSatRate);
|
||||
guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now,
|
||||
targetQuat, refSatRate);
|
||||
break;
|
||||
case SUBMODE_PTG_TARGET_GS:
|
||||
guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed,
|
||||
now, targetQuat, refSatRate);
|
||||
break;
|
||||
|
||||
case SUBMODE_PTG_NADIR:
|
||||
guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
|
||||
refSatRate);
|
||||
@ -288,7 +313,8 @@ void AcsController::performPointingCtrl() {
|
||||
ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled);
|
||||
}
|
||||
|
||||
double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input
|
||||
double cmdSpeedRws[4] = {0, 0, 0,
|
||||
0}; // Should be given to the actuator reaction wheel as input
|
||||
actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value),
|
||||
&(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value),
|
||||
@ -326,10 +352,10 @@ void AcsController::performPointingCtrl() {
|
||||
// dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2],
|
||||
// torqueDuration);
|
||||
// }
|
||||
}
|
||||
}
|
||||
|
||||
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
||||
LocalDataPoolManager &poolManager) {
|
||||
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool & localDataPoolMap,
|
||||
LocalDataPoolManager & poolManager) {
|
||||
// MGM Raw
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0VecRaw);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1VecRaw);
|
||||
@ -412,9 +438,9 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole);
|
||||
poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), false, 5.0});
|
||||
return returnvalue::OK;
|
||||
}
|
||||
}
|
||||
|
||||
LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
|
||||
LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
|
||||
switch (sid.ownerSetId) {
|
||||
case acsctrl::MGM_SENSOR_DATA:
|
||||
return &mgmDataRaw;
|
||||
@ -440,10 +466,10 @@ LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
|
||||
return nullptr;
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
|
||||
uint32_t *msToReachTheMode) {
|
||||
ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
|
||||
uint32_t * msToReachTheMode) {
|
||||
if (mode == MODE_OFF) {
|
||||
if (submode == SUBMODE_NONE) {
|
||||
return returnvalue::OK;
|
||||
@ -458,13 +484,9 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
|
||||
}
|
||||
}
|
||||
return INVALID_MODE;
|
||||
}
|
||||
}
|
||||
|
||||
void AcsController::modeChanged(Mode_t mode, Submode_t submode) {}
|
||||
|
||||
void AcsController::announceMode(bool recursive) {}
|
||||
|
||||
void AcsController::copyMgmData() {
|
||||
void AcsController::copyMgmData() {
|
||||
ACS::SensorValues sensorValues;
|
||||
{
|
||||
PoolReadGuard pg(&sensorValues.mgm0Lis3Set);
|
||||
@ -505,12 +527,13 @@ void AcsController::copyMgmData() {
|
||||
3 * sizeof(float));
|
||||
mgmDataRaw.imtqRaw.setValid(sensorValues.imtqMgmSet.mtmRawNt.isValid());
|
||||
mgmDataRaw.actuationCalStatus.value = sensorValues.imtqMgmSet.coilActuationStatus.value;
|
||||
mgmDataRaw.actuationCalStatus.setValid(sensorValues.imtqMgmSet.coilActuationStatus.isValid());
|
||||
mgmDataRaw.actuationCalStatus.setValid(
|
||||
sensorValues.imtqMgmSet.coilActuationStatus.isValid());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AcsController::copySusData() {
|
||||
void AcsController::copySusData() {
|
||||
ACS::SensorValues sensorValues;
|
||||
{
|
||||
PoolReadGuard pg(&sensorValues.susSets[0]);
|
||||
@ -608,9 +631,9 @@ void AcsController::copySusData() {
|
||||
susDataRaw.sus11.setValid(sensorValues.susSets[11].channels.isValid());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AcsController::copyGyrData() {
|
||||
void AcsController::copyGyrData() {
|
||||
ACS::SensorValues sensorValues;
|
||||
{
|
||||
PoolReadGuard pg(&sensorValues.gyr0AdisSet);
|
||||
@ -656,4 +679,4 @@ void AcsController::copyGyrData() {
|
||||
sensorValues.gyr3L3gSet.angVelocZ.isValid());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -3,6 +3,8 @@
|
||||
|
||||
#include <fsfw/controller/ExtendedControllerBase.h>
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <fsfw/parameters/ParameterHelper.h>
|
||||
#include <fsfw/parameters/ReceivesParameterMessagesIF.h>
|
||||
|
||||
#include "acs/ActuatorCmd.h"
|
||||
#include "acs/Guidance.h"
|
||||
@ -18,7 +20,7 @@
|
||||
#include "mission/devices/devicedefinitions/SusDefinitions.h"
|
||||
#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"
|
||||
|
||||
class AcsController : public ExtendedControllerBase {
|
||||
class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF {
|
||||
public:
|
||||
static constexpr dur_millis_t INIT_DELAY = 500;
|
||||
|
||||
@ -36,6 +38,12 @@ class AcsController : public ExtendedControllerBase {
|
||||
static const Event SAFE_RATE_VIOLATION =
|
||||
MAKE_EVENT(0, severity::MEDIUM); //!< The limits for the rotation in safe mode were violated.
|
||||
|
||||
MessageQueueId_t getCommandQueue() const;
|
||||
|
||||
ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues,
|
||||
uint16_t startAtIndex) override;
|
||||
|
||||
protected:
|
||||
void performSafe();
|
||||
void performDetumble();
|
||||
@ -54,6 +62,8 @@ class AcsController : public ExtendedControllerBase {
|
||||
|
||||
uint8_t detumbleCounter;
|
||||
|
||||
ParameterHelper parameterHelper;
|
||||
|
||||
enum class InternalState { STARTUP, INITIAL_DELAY, READY };
|
||||
|
||||
InternalState internalState = InternalState::STARTUP;
|
||||
@ -68,8 +78,6 @@ class AcsController : public ExtendedControllerBase {
|
||||
// Mode abstract functions
|
||||
ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
|
||||
uint32_t* msToReachTheMode) override;
|
||||
void modeChanged(Mode_t mode, Submode_t submode);
|
||||
void announceMode(bool recursive);
|
||||
|
||||
/* ACS Datasets */
|
||||
IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user