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

added target rotation rate to ctrlValData set
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-02-17 15:15:47 +01:00
parent 6352b65f46
commit 0d32bc0c0a
3 changed files with 9 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
mission/controller/AcsController.cpp
View File

@ -245,7 +245,8 @@ void AcsController::performPointingCtrl() {
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}; double mgtDpDes[3] = {0, 0, 0};
double targetQuat[4] = {0, 0, 0, 0}, targetSatRotRate[3] = {0, 0, 0}; double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1},
errorAngle = 0;
switch (submode) { switch (submode) {
case acs::PTG_IDLE: case acs::PTG_IDLE:
guidance.targetQuatPtgSun(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, guidance.targetQuatPtgSun(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
@ -278,8 +279,8 @@ void AcsController::performPointingCtrl() {
guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value, guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value,
gpsDataProcessed.gpsVelocity.value, targetQuat, gpsDataProcessed.gpsVelocity.value, targetQuat,
targetSatRotRate); targetSatRotRate);
guidance.calculateErrorQuat(targetQuat, mekfData.quatMekf.value, errorQuat, errorAngle);
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, guidance.comparePtg(errorQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
deltaRate); deltaRate);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
@ -536,6 +537,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat); localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat); localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng); localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng);
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_ROT_RATE, &tgtRotRate);
poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), false, 5.0}); poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), false, 5.0});
// Actuator CMD // Actuator CMD
localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque); localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque);

1
mission/controller/AcsController.h
View File

@ -180,6 +180,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
PoolEntry<double> tgtQuat = PoolEntry<double>(4); PoolEntry<double> tgtQuat = PoolEntry<double>(4);
PoolEntry<double> errQuat = PoolEntry<double>(4); PoolEntry<double> errQuat = PoolEntry<double>(4);
PoolEntry<double> errAng = PoolEntry<double>(); PoolEntry<double> errAng = PoolEntry<double>();
PoolEntry<double> tgtRotRate = PoolEntry<double>(4);
// Actuator CMD // Actuator CMD
acsctrl::ActuatorCmdData actuatorCmdData; acsctrl::ActuatorCmdData actuatorCmdData;

4
mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
View File

@ -95,6 +95,7 @@ enum PoolIds : lp_id_t {
TGT_QUAT, TGT_QUAT,
ERROR_QUAT, ERROR_QUAT,
ERROR_ANG, ERROR_ANG,
TGT_ROT_RATE,
// Actuator Cmd // Actuator Cmd
RW_TARGET_TORQUE, RW_TARGET_TORQUE,
RW_TARGET_SPEED, RW_TARGET_SPEED,
@ -109,7 +110,7 @@ static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5; static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5;
static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 4; static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 4;
static constexpr uint8_t MEKF_SET_ENTRIES = 2; static constexpr uint8_t MEKF_SET_ENTRIES = 2;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 3; static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 4;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
/** /**
@ -249,6 +250,7 @@ class CtrlValData : public StaticLocalDataSet<CTRL_VAL_SET_ENTRIES> {
lp_vec_t<double, 4> tgtQuat = lp_vec_t<double, 4>(sid.objectId, TGT_QUAT, this); lp_vec_t<double, 4> tgtQuat = lp_vec_t<double, 4>(sid.objectId, TGT_QUAT, this);
lp_vec_t<double, 4> errQuat = lp_vec_t<double, 4>(sid.objectId, ERROR_QUAT, this); lp_vec_t<double, 4> errQuat = lp_vec_t<double, 4>(sid.objectId, ERROR_QUAT, this);
lp_var_t<double> errAng = lp_var_t<double>(sid.objectId, ERROR_ANG, this); lp_var_t<double> errAng = lp_var_t<double>(sid.objectId, ERROR_ANG, this);
lp_vec_t<double, 3> tgtRotRate = lp_vec_t<double, 3>(sid.objectId, TGT_ROT_RATE, this);
private: private:
}; };