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 +15 -16
2 changed files with 15 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 8cc368b131 - Show all commits

27
mission/controller/AcsController.cpp
View File

@ -164,22 +164,21 @@ void AcsController::performSafe() {
{ {
PoolReadGuard pg(&actuatorCmdData); PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
double zeroVec[4] = {0, 0, 0, 0}; int32_t zeroVec[4] = {0, 0, 0, 0};
std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(double)); std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(int32_t));
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(int32_t));
actuatorCmdData.rwTargetSpeed.setValid(false); actuatorCmdData.rwTargetSpeed.setValid(false);
std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(double)); std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(int16_t));
actuatorCmdData.mtqTargetDipole.setValid(true); actuatorCmdData.mtqTargetDipole.setValid(true);
actuatorCmdData.setValidity(true, false); actuatorCmdData.setValidity(true, false);
} }
} }
// { // {
// 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(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], // dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2], torqueDuration);
// torqueDuration);
// } // }
} }
@ -225,12 +224,12 @@ void AcsController::performDetumble() {
{ {
PoolReadGuard pg(&actuatorCmdData); PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
double zeroVec[4] = {0, 0, 0, 0}; int32_t 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(int32_t));
actuatorCmdData.rwTargetSpeed.setValid(false); actuatorCmdData.rwTargetSpeed.setValid(false);
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(double)); std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t));
actuatorCmdData.mtqTargetDipole.setValid(true); actuatorCmdData.mtqTargetDipole.setValid(true);
actuatorCmdData.setValidity(true, false); actuatorCmdData.setValidity(true, false);
} }
@ -295,8 +294,8 @@ void AcsController::performPointingCtrl() {
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, cmdRwSpeedInt, 4 * sizeof(double)); std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdRwSpeedInt, 4 * sizeof(int32_t));
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(double)); std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t));
actuatorCmdData.setValidity(true, true); actuatorCmdData.setValidity(true, true);
} }
} }

4
mission/controller/AcsController.h
View File

@ -158,8 +158,8 @@ IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HAND
// Actuator CMD // Actuator CMD
acsctrl::ActuatorCmdData actuatorCmdData; acsctrl::ActuatorCmdData actuatorCmdData;
PoolEntry<double> rwTargetTorque = PoolEntry<double>(4); PoolEntry<double> rwTargetTorque = PoolEntry<double>(4);
PoolEntry<double> rwTargetSpeed = PoolEntry<double>(4); PoolEntry<int32_t> rwTargetSpeed = PoolEntry<int32_t>(4);
PoolEntry<double> mtqTargetDipole = PoolEntry<double>(3); PoolEntry<int16_t> mtqTargetDipole = PoolEntry<int16_t>(3);
// Initial delay to make sure all pool variables have been initialized their owners // Initial delay to make sure all pool variables have been initialized their owners
Countdown initialCountdown = Countdown(INIT_DELAY); Countdown initialCountdown = Countdown(INIT_DELAY);