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

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

@ -95,6 +95,7 @@ enum PoolIds : lp_id_t {
TGT_QUAT,
ERROR_QUAT,
ERROR_ANG,
TGT_ROT_RATE,
// Actuator Cmd
RW_TARGET_TORQUE,
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 GPS_SET_PROCESSED_ENTRIES = 4;
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;
/**
@ -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> 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_vec_t<double, 3> tgtRotRate = lp_vec_t<double, 3>(sid.objectId, TGT_ROT_RATE, this);
private:
};