From c7bfe4002d7dcf16b13f3f375c225df080198b5f Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 3 Nov 2022 14:24:09 +0100 Subject: [PATCH] added event for SAFE to DETUMBLE transition. changed actuator input from double to int --- mission/controller/AcsController.cpp | 32 +++++++++++++------ mission/controller/AcsController.h | 4 +++ .../AcsCtrlDefinitions.h | 7 ++-- 3 files changed, 31 insertions(+), 12 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 953a91a1..799aa0c3 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -156,12 +156,13 @@ void AcsController::performSafe() { if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { submode = SUBMODE_DETUMBLE; detumbleCounter = 0; + triggerEvent(SAFE_RATE_VIOLATION); } { PoolReadGuard pg(&actuatorCmdData); 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)); actuatorCmdData.rwTargetTorque.setValid(false); std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(double)); @@ -175,8 +176,8 @@ void AcsController::performSafe() { // PoolReadGuard pg(&dipoleSet); // MutexGuard mg(torquer::lazyLock()); // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2], - //torqueDuration); + // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], + // torqueDuration); // } } @@ -215,15 +216,19 @@ void AcsController::performDetumble() { 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); 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)); actuatorCmdData.rwTargetTorque.setValid(false); std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(double)); 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.setValidity(true, false); } @@ -232,7 +237,7 @@ void AcsController::performDetumble() { // PoolReadGuard pg(&dipoleSet); // MutexGuard mg(torquer::lazyLock()); // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2], + // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], // torqueDuration); // } } @@ -275,12 +280,21 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); 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); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double)); - std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdSpeedRws, 4 * sizeof(double)); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolUnits, 3 * sizeof(double)); + std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdRwSpeedInt, 4 * sizeof(double)); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(double)); actuatorCmdData.setValidity(true, true); } } @@ -288,7 +302,7 @@ void AcsController::performPointingCtrl() { // PoolReadGuard pg(&dipoleSet); // MutexGuard mg(torquer::lazyLock()); // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2], + // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], // torqueDuration); // } } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 18c8d824..7f6363ce 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -30,6 +30,10 @@ class AcsController : public ExtendedControllerBase { static const Submode_t SUBMODE_PTG_NADIR = 5; 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: void performSafe(); void performDetumble(); diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index 1b07218f..2652ca3c 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -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 GPS_SET_PROCESSED_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; /** @@ -254,8 +254,9 @@ class ActuatorCmdData : public StaticLocalDataSet { ActuatorCmdData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, ACTUATOR_CMD_DATA) {} lp_vec_t rwTargetTorque = lp_vec_t(sid.objectId, RW_TARGET_TORQUE, this); - lp_vec_t rwTargetSpeed = lp_vec_t(sid.objectId, RW_TARGET_SPEED, this); - lp_vec_t mtqTargetDipole = lp_vec_t(sid.objectId, MTQ_TARGET_DIPOLE, this); + lp_vec_t rwTargetSpeed = lp_vec_t(sid.objectId, RW_TARGET_SPEED, this); + lp_vec_t mtqTargetDipole = + lp_vec_t(sid.objectId, MTQ_TARGET_DIPOLE, this); private: };