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

First Version of ACS Controller #329

Merged
muellerr merged 106 commits from acs-ctrl-v1 into develop 2022-12-02 16:21:58 +01:00
Conversation 0 Commits 106 Files Changed 50 +31 -12
3 changed files with 31 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit c7bfe4002d - Show all commits

30
mission/controller/AcsController.cpp
View File

@ -156,12 +156,13 @@ void AcsController::performSafe() {
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
submode = SUBMODE_DETUMBLE; submode = SUBMODE_DETUMBLE;
detumbleCounter = 0; detumbleCounter = 0;
triggerEvent(SAFE_RATE_VIOLATION);
} }
{ {
PoolReadGuard pg(&actuatorCmdData); PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
double zeroVec[3] = {0, 0, 0, 0}; double zeroVec[4] = {0, 0, 0, 0};
std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(double)); std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(double));
actuatorCmdData.rwTargetTorque.setValid(false); actuatorCmdData.rwTargetTorque.setValid(false);
std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(double)); std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(double));
@ -175,7 +176,7 @@ void AcsController::performSafe() {
// PoolReadGuard pg(&dipoleSet); // PoolReadGuard pg(&dipoleSet);
// MutexGuard mg(torquer::lazyLock()); // MutexGuard mg(torquer::lazyLock());
// torquer::NEW_ACTUATION_FLAG = true; // torquer::NEW_ACTUATION_FLAG = true;
// dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2], // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2],
// torqueDuration); // torqueDuration);
// } // }
} }
@ -215,15 +216,19 @@ void AcsController::performDetumble() {
detumbleCounter = 0; detumbleCounter = 0;
} }
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) {
double zeroVec[3] = {0, 0, 0, 0}; double zeroVec[4] = {0, 0, 0, 0};
std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(double)); std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(double));
actuatorCmdData.rwTargetTorque.setValid(false); actuatorCmdData.rwTargetTorque.setValid(false);
std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(double)); std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(double));
actuatorCmdData.rwTargetSpeed.setValid(false); actuatorCmdData.rwTargetSpeed.setValid(false);
std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(double)); std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(double));
actuatorCmdData.mtqTargetDipole.setValid(true); actuatorCmdData.mtqTargetDipole.setValid(true);
actuatorCmdData.setValidity(true, false); actuatorCmdData.setValidity(true, false);
} }
@ -232,7 +237,7 @@ void AcsController::performDetumble() {
// PoolReadGuard pg(&dipoleSet); // PoolReadGuard pg(&dipoleSet);
// MutexGuard mg(torquer::lazyLock()); // MutexGuard mg(torquer::lazyLock());
// torquer::NEW_ACTUATION_FLAG = true; // torquer::NEW_ACTUATION_FLAG = true;
// dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2], // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2],
// torqueDuration); // torqueDuration);
// } // }
} }
@ -275,12 +280,21 @@ void AcsController::performPointingCtrl() {
&(sensorValues.rw4Set.currSpeed.value), mgtDpDes); &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
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, cmdSpeedRws, 4 * sizeof(double)); std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdRwSpeedInt, 4 * sizeof(double));
std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolUnits, 3 * sizeof(double)); std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(double));
actuatorCmdData.setValidity(true, true); actuatorCmdData.setValidity(true, true);
} }
} }
@ -288,7 +302,7 @@ void AcsController::performPointingCtrl() {
// PoolReadGuard pg(&dipoleSet); // PoolReadGuard pg(&dipoleSet);
// MutexGuard mg(torquer::lazyLock()); // MutexGuard mg(torquer::lazyLock());
// torquer::NEW_ACTUATION_FLAG = true; // torquer::NEW_ACTUATION_FLAG = true;
// dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2], // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2],
// torqueDuration); // torqueDuration);
// } // }
} }

4
mission/controller/AcsController.h
View File

@ -30,6 +30,10 @@ class AcsController : public ExtendedControllerBase {
static const Submode_t SUBMODE_PTG_NADIR = 5; static const Submode_t SUBMODE_PTG_NADIR = 5;
static const Submode_t SUBMODE_PTG_INERTIAL = 6; static const Submode_t SUBMODE_PTG_INERTIAL = 6;
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
static const Event SAFE_RATE_VIOLATION =
MAKE_EVENT(0, severity::MEDIUM); //!< The limits for the rotation in safe mode were violated.
protected: protected:
void performSafe(); void performSafe();
void performDetumble(); void performDetumble();

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

@ -107,7 +107,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 = 2; static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 2;
static constexpr uint8_t MEKF_SET_ENTRIES = 2; static constexpr uint8_t MEKF_SET_ENTRIES = 2;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 99; static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 3;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
/** /**
@ -254,8 +254,9 @@ class ActuatorCmdData : public StaticLocalDataSet<ACT_CMD_SET_ENTRIES> {
ActuatorCmdData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, ACTUATOR_CMD_DATA) {} ActuatorCmdData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, ACTUATOR_CMD_DATA) {}
lp_vec_t<double, 4> rwTargetTorque = lp_vec_t<double, 4>(sid.objectId, RW_TARGET_TORQUE, this); lp_vec_t<double, 4> rwTargetTorque = lp_vec_t<double, 4>(sid.objectId, RW_TARGET_TORQUE, this);
lp_vec_t<double, 4> rwTargetSpeed = lp_vec_t<double, 4>(sid.objectId, RW_TARGET_SPEED, this); lp_vec_t<int32_t, 4> rwTargetSpeed = lp_vec_t<int32_t, 4>(sid.objectId, RW_TARGET_SPEED, this);
lp_vec_t<double, 3> mtqTargetDipole = lp_vec_t<double, 3>(sid.objectId, MTQ_TARGET_DIPOLE, this); lp_vec_t<int16_t, 3> mtqTargetDipole =
lp_vec_t<int16_t, 3>(sid.objectId, MTQ_TARGET_DIPOLE, this);
private: private:
}; };