From 29a34256a72c97391556d6b970161a2b6f2fc620 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 20 Oct 2022 15:08:33 +0200 Subject: [PATCH] old code seems to work --- dummies/ImtqDummy.cpp | 2 +- fsfw | 2 +- mission/controller/AcsController.cpp | 11 +++ mission/controller/AcsController.h | 2 +- mission/controller/ThermalController.cpp | 2 +- mission/devices/CMakeLists.txt | 3 +- mission/devices/ImtqHandler.cpp | 74 +++++++++---------- mission/devices/ImtqHandler.h | 13 +--- ...Definitions.h => imtqHandlerDefinitions.h} | 17 +---- mission/devices/torquer.cpp | 27 +++++++ mission/devices/torquer.h | 22 ++++++ tmtc | 2 +- 12 files changed, 108 insertions(+), 69 deletions(-) rename mission/devices/devicedefinitions/{IMTQHandlerDefinitions.h => imtqHandlerDefinitions.h} (98%) create mode 100644 mission/devices/torquer.cpp create mode 100644 mission/devices/torquer.h diff --git a/dummies/ImtqDummy.cpp b/dummies/ImtqDummy.cpp index 6dc451b3..aad1478b 100644 --- a/dummies/ImtqDummy.cpp +++ b/dummies/ImtqDummy.cpp @@ -1,6 +1,6 @@ #include "ImtqDummy.h" -#include +#include ImtqDummy::ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) : DeviceHandlerBase(objectId, comif, comCookie) {} diff --git a/fsfw b/fsfw index 692be9df..0c5c2f6c 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 692be9df8d06beb3bfc83aad77cefd727d8f7c35 +Subproject commit 0c5c2f6c4f07959a73a083eb3c6e1f3125642477 diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 09e91e26..b8b1e8ca 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -2,6 +2,8 @@ #include +#include "mission/devices/torquer.h" + AcsController::AcsController(object_id_t objectId) : ExtendedControllerBase(objectId, objects::NO_OBJECT), mgmData(this) {} @@ -29,6 +31,15 @@ void AcsController::performControlOperation() { break; } + { + // TODO: Calculate actuator output + PoolReadGuard pg(&dipoleSet); + MutexGuard mg(torquer::lazyLock()); + torquer::NEW_ACTUATION_FLAG = true; + // TODO: Insert correct values here + dipoleSet.setDipoles(500, 500, 500, 150); + } + { PoolReadGuard pg(&mgmData); if (pg.getReadResult() == returnvalue::OK) { diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 290938b4..adb6fbb6 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -7,8 +7,8 @@ #include "eive/objects.h" #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" -#include "mission/devices/devicedefinitions/IMTQHandlerDefinitions.h" #include "mission/devices/devicedefinitions/SusDefinitions.h" +#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h" class AcsController : public ExtendedControllerBase { public: diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 608b68b7..174d1e9f 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -8,9 +8,9 @@ #include #include #include -#include #include #include +#include #include #include diff --git a/mission/devices/CMakeLists.txt b/mission/devices/CMakeLists.txt index 0bfb1a2d..b8fb326c 100644 --- a/mission/devices/CMakeLists.txt +++ b/mission/devices/CMakeLists.txt @@ -20,6 +20,7 @@ target_sources( SusHandler.cpp PayloadPcduHandler.cpp SolarArrayDeploymentHandler.cpp - ScexDeviceHandler.cpp) + ScexDeviceHandler.cpp + torquer.cpp) add_subdirectory(devicedefinitions) diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index cc80d1f9..c65674ce 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -25,9 +25,7 @@ #include #include -MutexIF* ImtqHandler::TORQUE_LOCK = nullptr; -bool ImtqHandler::TORQUEING = false; -Countdown ImtqHandler::TORQUE_COUNTDOWN = Countdown(); +#include "mission/devices/torquer.h" ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, power::Switch_t pwrSwitcher) @@ -46,7 +44,6 @@ ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comC if (comCookie == nullptr) { sif::error << "IMTQHandler: Invalid com cookie" << std::endl; } - TORQUE_LOCK = MutexFactory::instance()->createMutex(); } ImtqHandler::~ImtqHandler() {} @@ -62,6 +59,7 @@ void ImtqHandler::doStartUp() { void ImtqHandler::doShutDown() { setMode(_MODE_POWER_DOWN); } ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { + bool buildCommand = true; // Depending on the normal polling mode configuration, 3-4 communication steps are recommended switch (communicationStep) { case CommunicationStep::GET_ENG_HK_DATA: @@ -99,9 +97,12 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { // If the dipole is not commanded but set by the ACS control algorithm, // the dipoles will be set by the ACS controller directly using the dipole local pool set. // This set has a flag to determine whether the ACS controller actually set any new input. - PoolReadGuard pg(&dipoleSet); - if (dipoleSet.newActuation) { + MutexGuard mg(torquer::lazyLock()); + if (torquer::NEW_ACTUATION_FLAG) { *id = IMTQ::START_ACTUATION_DIPOLE; + torquer::NEW_ACTUATION_FLAG = false; + } else { + buildCommand = false; } communicationStep = CommunicationStep::GET_ENG_HK_DATA; break; @@ -111,7 +112,10 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { << std::endl; break; } - return buildCommandFromCommand(*id, nullptr, 0); + if (buildCommand) { + return buildCommandFromCommand(*id, nullptr, 0); + } + return NOTHING_TO_SEND; } ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { @@ -176,23 +180,27 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma if (commandData != nullptr && commandDataLen < 8) { return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; } - PoolReadGuard pg(&dipoleSet); ReturnValue_t result; // Commands override anything which was set in the software if (commandBuffer != nullptr) { + dipoleSet.setValidityBufferGeneration(false); result = dipoleSet.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::NETWORK); + dipoleSet.setValidityBufferGeneration(true); if (result != returnvalue::OK) { return result; } + } else { + // Read set dipole values from local pool + PoolReadGuard pg(&dipoleSet); } result = buildDipoleActuationCommand(); if (result != returnvalue::OK) { return result; } - MutexGuard mg(TORQUE_LOCK); - TORQUEING = true; - TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value); + MutexGuard mg(torquer::lazyLock()); + torquer::TORQUEING = true; + torquer::TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value); return result; } case (IMTQ::GET_ENG_HK_DATA): { @@ -249,26 +257,22 @@ ReturnValue_t ImtqHandler::buildDipoleActuationCommand() { } void ImtqHandler::fillCommandAndReplyMap() { - this->insertInCommandAndReplyMap(IMTQ::POS_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); - this->insertInCommandAndReplyMap(IMTQ::NEG_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); - this->insertInCommandAndReplyMap(IMTQ::POS_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); - this->insertInCommandAndReplyMap(IMTQ::NEG_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); - this->insertInCommandAndReplyMap(IMTQ::POS_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); - this->insertInCommandAndReplyMap(IMTQ::NEG_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); - this->insertInCommandAndReplyMap(IMTQ::GET_SELF_TEST_RESULT, 1, nullptr, - IMTQ::SIZE_SELF_TEST_RESULTS); - this->insertInCommandAndReplyMap(IMTQ::START_ACTUATION_DIPOLE, 1, nullptr, - IMTQ::SIZE_STATUS_REPLY); - this->insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset, - IMTQ::SIZE_ENG_HK_DATA_REPLY); - this->insertInCommandAndReplyMap(IMTQ::GET_COMMANDED_DIPOLE, 1, nullptr, - IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY); - this->insertInCommandAndReplyMap(IMTQ::START_MTM_MEASUREMENT, 1, nullptr, - IMTQ::SIZE_STATUS_REPLY); - this->insertInCommandAndReplyMap(IMTQ::GET_CAL_MTM_MEASUREMENT, 1, &calMtmMeasurementSet, - IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT); - this->insertInCommandAndReplyMap(IMTQ::GET_RAW_MTM_MEASUREMENT, 1, &rawMtmMeasurementSet, - IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT); + insertInCommandAndReplyMap(IMTQ::POS_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + insertInCommandAndReplyMap(IMTQ::NEG_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + insertInCommandAndReplyMap(IMTQ::POS_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + insertInCommandAndReplyMap(IMTQ::NEG_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + insertInCommandAndReplyMap(IMTQ::POS_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + insertInCommandAndReplyMap(IMTQ::NEG_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + insertInCommandAndReplyMap(IMTQ::GET_SELF_TEST_RESULT, 1, nullptr, IMTQ::SIZE_SELF_TEST_RESULTS); + insertInCommandAndReplyMap(IMTQ::START_ACTUATION_DIPOLE, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset, IMTQ::SIZE_ENG_HK_DATA_REPLY); + insertInCommandAndReplyMap(IMTQ::GET_COMMANDED_DIPOLE, 1, nullptr, + IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY); + insertInCommandAndReplyMap(IMTQ::START_MTM_MEASUREMENT, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + insertInCommandAndReplyMap(IMTQ::GET_CAL_MTM_MEASUREMENT, 1, &calMtmMeasurementSet, + IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT); + insertInCommandAndReplyMap(IMTQ::GET_RAW_MTM_MEASUREMENT, 1, &rawMtmMeasurementSet, + IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT); } ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSize, @@ -2315,11 +2319,3 @@ ReturnValue_t ImtqHandler::getSwitches(const uint8_t** switches, uint8_t* number } return DeviceHandlerBase::NO_SWITCH; } - -bool ImtqHandler::mgtIsTorqueing(dur_millis_t* remainingTorqueDuration) { - MutexGuard mg(TORQUE_LOCK); - if (TORQUEING and remainingTorqueDuration != nullptr) { - *remainingTorqueDuration = TORQUE_COUNTDOWN.getRemainingMillis() + TORQUE_BUFFER_TIME_MS; - } - return TORQUEING; -} diff --git a/mission/devices/ImtqHandler.h b/mission/devices/ImtqHandler.h index 7ddb4e4c..d414be0f 100644 --- a/mission/devices/ImtqHandler.h +++ b/mission/devices/ImtqHandler.h @@ -2,7 +2,7 @@ #define MISSION_DEVICES_IMTQHANDLER_H_ #include -#include +#include #include #include "events/subsystemIdRanges.h" @@ -107,7 +107,7 @@ class ImtqHandler : public DeviceHandlerBase { IMTQ::PosZSelfTestSet posZselfTestDataset; IMTQ::NegZSelfTestSet negZselfTestDataset; - NormalPollingMode pollingMode = NormalPollingMode::BOTH; + NormalPollingMode pollingMode = NormalPollingMode::UNCALIBRATED; PoolEntry mgmCalEntry = PoolEntry(3); PoolEntry dipoleXEntry = PoolEntry(0, false); @@ -221,15 +221,6 @@ class ImtqHandler : public DeviceHandlerBase { void checkErrorByte(const uint8_t errorByte, const uint8_t step); std::string makeStepString(const uint8_t step); - - static MutexIF* TORQUE_LOCK; - static bool TORQUEING; - static Countdown TORQUE_COUNTDOWN; - // Additional buffer time to accont for time until I2C command arrives and ramp up / ramp down - // time of the MGT - static constexpr dur_millis_t TORQUE_BUFFER_TIME_MS = 20; - - static bool mgtIsTorqueing(dur_millis_t* remainingTorqueDuration); }; #endif /* MISSION_DEVICES_IMTQHANDLER_H_ */ diff --git a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h b/mission/devices/devicedefinitions/imtqHandlerDefinitions.h similarity index 98% rename from mission/devices/devicedefinitions/IMTQHandlerDefinitions.h rename to mission/devices/devicedefinitions/imtqHandlerDefinitions.h index 320220dd..1799d347 100644 --- a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h +++ b/mission/devices/devicedefinitions/imtqHandlerDefinitions.h @@ -486,24 +486,17 @@ class DipoleActuationSet : public StaticLocalDataSet<4> { : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::DIPOLES)) {} // Refresh torque command without changing any of the set dipoles. - void refreshTorqueing(uint16_t durationMs_) { - newActuation = true; - currentTorqueDurationMs = durationMs_; - } + void refreshTorqueing(uint16_t durationMs_) { currentTorqueDurationMs = durationMs_; } void setDipoles(uint16_t xDipole_, uint16_t yDipole_, uint16_t zDipole_, uint16_t currentTorqueDurationMs_) { - newActuation = false; if (xDipole.value != xDipole_) { - newActuation = true; } xDipole = xDipole_; if (yDipole.value != yDipole_) { - newActuation = true; } yDipole = yDipole_; if (zDipole.value != zDipole_) { - newActuation = true; } zDipole = zDipole_; currentTorqueDurationMs = currentTorqueDurationMs_; @@ -516,13 +509,11 @@ class DipoleActuationSet : public StaticLocalDataSet<4> { } private: - lp_var_t xDipole = lp_var_t(sid.objectId, DIPOLES_X, this); - lp_var_t yDipole = lp_var_t(sid.objectId, DIPOLES_Y, this); - lp_var_t zDipole = lp_var_t(sid.objectId, DIPOLES_Z, this); + lp_var_t xDipole = lp_var_t(sid.objectId, DIPOLES_X, this); + lp_var_t yDipole = lp_var_t(sid.objectId, DIPOLES_Y, this); + lp_var_t zDipole = lp_var_t(sid.objectId, DIPOLES_Z, this); lp_var_t currentTorqueDurationMs = lp_var_t(sid.objectId, CURRENT_TORQUE_DURATION, this); - - bool newActuation = false; }; /** diff --git a/mission/devices/torquer.cpp b/mission/devices/torquer.cpp new file mode 100644 index 00000000..27a32bef --- /dev/null +++ b/mission/devices/torquer.cpp @@ -0,0 +1,27 @@ +#include "torquer.h" + +#include + +MutexIF* TORQUE_LOCK = nullptr; + +namespace torquer { + +bool TORQUEING = false; +bool NEW_ACTUATION_FLAG = false; +Countdown TORQUE_COUNTDOWN = Countdown(); + +bool mgtIsTorqueing(dur_millis_t* remainingTorqueDuration) { + if (TORQUEING and remainingTorqueDuration != nullptr) { + *remainingTorqueDuration = TORQUE_COUNTDOWN.getRemainingMillis() + TORQUE_BUFFER_TIME_MS; + } + return TORQUEING; +} + +MutexIF* lazyLock() { + if (TORQUE_LOCK == nullptr) { + TORQUE_LOCK = MutexFactory::instance()->createMutex(); + } + return TORQUE_LOCK; +} + +} // namespace torquer diff --git a/mission/devices/torquer.h b/mission/devices/torquer.h new file mode 100644 index 00000000..10a27991 --- /dev/null +++ b/mission/devices/torquer.h @@ -0,0 +1,22 @@ +#ifndef MISSION_DEVICES_TOQUER_H_ +#define MISSION_DEVICES_TOQUER_H_ + +#include +#include + +namespace torquer { + +// Additional buffer time to accont for time until I2C command arrives and ramp up / ramp down +// time of the MGT +static constexpr dur_millis_t TORQUE_BUFFER_TIME_MS = 20; + +MutexIF* lazyLock(); +extern bool TORQUEING; +extern bool NEW_ACTUATION_FLAG; +extern Countdown TORQUE_COUNTDOWN; + +bool mgtIsTorqueing(dur_millis_t* remainingTorqueDuration); + +} // namespace torquer + +#endif /* MISSION_DEVICES_TOQUER_H_ */ diff --git a/tmtc b/tmtc index 42b962de..1dfc2fca 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 42b962dede547975488e72b22abe2360065c0404 +Subproject commit 1dfc2fca2f58f8d226fab01c87eb529ba7ec8376