diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 74af9564..deee8c53 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -70,7 +70,7 @@ #include "mission/devices/BpxBatteryHandler.h" #include "mission/devices/GyroADIS1650XHandler.h" #include "mission/devices/HeaterHandler.h" -#include "mission/devices/IMTQHandler.h" +#include #include "mission/devices/Max31865PT1000Handler.h" #include "mission/devices/P60DockHandler.h" #include "mission/devices/PCDUHandler.h" @@ -889,7 +889,7 @@ void ObjectFactory::createStrComponents(PowerSwitchIF* pwrSwitcher) { void ObjectFactory::createImtqComponents(PowerSwitchIF* pwrSwitcher) { I2cCookie* imtqI2cCookie = new I2cCookie(addresses::IMTQ, IMTQ::MAX_REPLY_SIZE, q7s::I2C_DEFAULT_DEV); - auto imtqHandler = new IMTQHandler(objects::IMTQ_HANDLER, objects::I2C_COM_IF, imtqI2cCookie, + auto imtqHandler = new ImtqHandler(objects::IMTQ_HANDLER, objects::I2C_COM_IF, imtqI2cCookie, pcdu::Switches::PDU1_CH3_MGT_5V); imtqHandler->setPowerSwitcher(pwrSwitcher); static_cast(imtqHandler); diff --git a/mission/devices/CMakeLists.txt b/mission/devices/CMakeLists.txt index 56184ea4..0bfb1a2d 100644 --- a/mission/devices/CMakeLists.txt +++ b/mission/devices/CMakeLists.txt @@ -11,7 +11,7 @@ target_sources( SyrlinksHkHandler.cpp Max31865PT1000Handler.cpp Max31865EiveHandler.cpp - IMTQHandler.cpp + ImtqHandler.cpp HeaterHandler.cpp RadiationSensorHandler.cpp GyroADIS1650XHandler.cpp diff --git a/mission/devices/IMTQHandler.cpp b/mission/devices/ImtqHandler.cpp similarity index 97% rename from mission/devices/IMTQHandler.cpp rename to mission/devices/ImtqHandler.cpp index f6047c48..adb9f83b 100644 --- a/mission/devices/IMTQHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -1,13 +1,16 @@ -#include "IMTQHandler.h" - #include #include +#include #include #include "OBSWConfig.h" -IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, +MutexIF* ImtqHandler::TORQUE_LOCK = nullptr; +bool ImtqHandler::TORQUEING = false; +Countdown ImtqHandler::TORQUE_COUNTDOWN = Countdown(); + +ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, power::Switch_t pwrSwitcher) : DeviceHandlerBase(objectId, comIF, comCookie), engHkDataset(this), @@ -24,11 +27,12 @@ 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() {} +ImtqHandler::~ImtqHandler() {} -void IMTQHandler::doStartUp() { +void ImtqHandler::doStartUp() { if (goToNormalMode) { setMode(MODE_NORMAL); } else { @@ -36,9 +40,9 @@ void IMTQHandler::doStartUp() { } } -void IMTQHandler::doShutDown() { setMode(_MODE_POWER_DOWN); } +void ImtqHandler::doShutDown() { setMode(_MODE_POWER_DOWN); } -ReturnValue_t IMTQHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { +ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { switch (communicationStep) { case CommunicationStep::GET_ENG_HK_DATA: *id = IMTQ::GET_ENG_HK_DATA; @@ -64,11 +68,11 @@ ReturnValue_t IMTQHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { return buildCommandFromCommand(*id, NULL, 0); } -ReturnValue_t IMTQHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { +ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { return NOTHING_TO_SEND; } -ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, +ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t* commandData, size_t commandDataLen) { switch (deviceCommand) { @@ -174,7 +178,7 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma return returnvalue::FAILED; } -void IMTQHandler::fillCommandAndReplyMap() { +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); @@ -197,7 +201,7 @@ void IMTQHandler::fillCommandAndReplyMap() { IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT); } -ReturnValue_t IMTQHandler::scanForReply(const uint8_t* start, size_t remainingSize, +ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, size_t* foundLen) { ReturnValue_t result = returnvalue::OK; @@ -243,7 +247,7 @@ ReturnValue_t IMTQHandler::scanForReply(const uint8_t* start, size_t remainingSi return result; } -ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { +ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { ReturnValue_t result = returnvalue::OK; result = parseStatusByte(packet); @@ -287,9 +291,9 @@ ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, const uint return returnvalue::OK; } -void IMTQHandler::setNormalDatapoolEntriesInvalid() {} +void ImtqHandler::setNormalDatapoolEntriesInvalid() {} -LocalPoolDataSetBase* IMTQHandler::getDataSetHandle(sid_t sid) { +LocalPoolDataSetBase* ImtqHandler::getDataSetHandle(sid_t sid) { if (sid == engHkDataset.getSid()) { return &engHkDataset; } else if (sid == calMtmMeasurementSet.getSid()) { @@ -314,9 +318,9 @@ LocalPoolDataSetBase* IMTQHandler::getDataSetHandle(sid_t sid) { } } -uint32_t IMTQHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; } +uint32_t ImtqHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; } -ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, +ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { /** Entries of engineering housekeeping dataset */ localDataPoolMap.emplace(IMTQ::DIGITAL_VOLTAGE_MV, new PoolEntry({0})); @@ -612,7 +616,7 @@ ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDat return returnvalue::OK; } -ReturnValue_t IMTQHandler::getSelfTestCommandId(DeviceCommandId_t* id) { +ReturnValue_t ImtqHandler::getSelfTestCommandId(DeviceCommandId_t* id) { DeviceCommandId_t commandId = getPendingCommand(); switch (commandId) { case IMTQ::POS_X_SELF_TEST: @@ -631,7 +635,7 @@ ReturnValue_t IMTQHandler::getSelfTestCommandId(DeviceCommandId_t* id) { return returnvalue::OK; } -ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) { +ReturnValue_t ImtqHandler::parseStatusByte(const uint8_t* packet) { uint8_t cmdErrorField = *(packet + 1) & 0xF; switch (cmdErrorField) { case 0: @@ -662,7 +666,7 @@ ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) { } } -void IMTQHandler::fillEngHkDataset(const uint8_t* packet) { +void ImtqHandler::fillEngHkDataset(const uint8_t* packet) { PoolReadGuard rg(&engHkDataset); uint8_t offset = 2; engHkDataset.digitalVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset); @@ -709,9 +713,9 @@ void IMTQHandler::fillEngHkDataset(const uint8_t* packet) { } } -void IMTQHandler::setToGoToNormal(bool enable) { this->goToNormalMode = enable; } +void ImtqHandler::setToGoToNormal(bool enable) { this->goToNormalMode = enable; } -void IMTQHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) { +void ImtqHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) { if (wiretappingMode == RAW) { /* Data already sent in doGetRead() */ return; @@ -735,7 +739,7 @@ void IMTQHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCom } } -void IMTQHandler::handleGetCommandedDipoleReply(const uint8_t* packet) { +void ImtqHandler::handleGetCommandedDipoleReply(const uint8_t* packet) { uint8_t tmData[6]; /* Switching endianess of received dipole values */ tmData[0] = *(packet + 3); @@ -747,7 +751,7 @@ void IMTQHandler::handleGetCommandedDipoleReply(const uint8_t* packet) { handleDeviceTM(tmData, sizeof(tmData), IMTQ::GET_COMMANDED_DIPOLE); } -void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) { +void ImtqHandler::fillCalibratedMtmDataset(const uint8_t* packet) { PoolReadGuard rg(&calMtmMeasurementSet); calMtmMeasurementSet.setValidity(true, true); int8_t offset = 2; @@ -777,7 +781,7 @@ void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) { } } -void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) { +void ImtqHandler::fillRawMtmDataset(const uint8_t* packet) { PoolReadGuard rg(&rawMtmMeasurementSet); unsigned int offset = 2; size_t deSerLen = 16; @@ -825,7 +829,7 @@ void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) { } } -void IMTQHandler::handleSelfTestReply(const uint8_t* packet) { +void ImtqHandler::handleSelfTestReply(const uint8_t* packet) { uint16_t offset = 2; checkErrorByte(*(packet + offset), *(packet + offset + 1)); @@ -859,7 +863,7 @@ void IMTQHandler::handleSelfTestReply(const uint8_t* packet) { } } -void IMTQHandler::handlePositiveXSelfTestReply(const uint8_t* packet) { +void ImtqHandler::handlePositiveXSelfTestReply(const uint8_t* packet) { PoolReadGuard rg(&posXselfTestDataset); uint16_t offset = 2; @@ -1071,7 +1075,7 @@ void IMTQHandler::handlePositiveXSelfTestReply(const uint8_t* packet) { } } -void IMTQHandler::handleNegativeXSelfTestReply(const uint8_t* packet) { +void ImtqHandler::handleNegativeXSelfTestReply(const uint8_t* packet) { PoolReadGuard rg(&posXselfTestDataset); uint16_t offset = 2; @@ -1283,7 +1287,7 @@ void IMTQHandler::handleNegativeXSelfTestReply(const uint8_t* packet) { } } -void IMTQHandler::handlePositiveYSelfTestReply(const uint8_t* packet) { +void ImtqHandler::handlePositiveYSelfTestReply(const uint8_t* packet) { PoolReadGuard rg(&posXselfTestDataset); uint16_t offset = 2; @@ -1495,7 +1499,7 @@ void IMTQHandler::handlePositiveYSelfTestReply(const uint8_t* packet) { } } -void IMTQHandler::handleNegativeYSelfTestReply(const uint8_t* packet) { +void ImtqHandler::handleNegativeYSelfTestReply(const uint8_t* packet) { PoolReadGuard rg(&posXselfTestDataset); uint16_t offset = 2; @@ -1707,7 +1711,7 @@ void IMTQHandler::handleNegativeYSelfTestReply(const uint8_t* packet) { } } -void IMTQHandler::handlePositiveZSelfTestReply(const uint8_t* packet) { +void ImtqHandler::handlePositiveZSelfTestReply(const uint8_t* packet) { PoolReadGuard rg(&posXselfTestDataset); uint16_t offset = 2; @@ -1919,7 +1923,7 @@ void IMTQHandler::handlePositiveZSelfTestReply(const uint8_t* packet) { } } -void IMTQHandler::handleNegativeZSelfTestReply(const uint8_t* packet) { +void ImtqHandler::handleNegativeZSelfTestReply(const uint8_t* packet) { PoolReadGuard rg(&posXselfTestDataset); uint16_t offset = 2; @@ -2131,9 +2135,9 @@ void IMTQHandler::handleNegativeZSelfTestReply(const uint8_t* packet) { } } -void IMTQHandler::setDebugMode(bool enable) { this->debugMode = enable; } +void ImtqHandler::setDebugMode(bool enable) { this->debugMode = enable; } -void IMTQHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) { +void ImtqHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) { std::string stepString(""); if (step < 8) { stepString = makeStepString(step); @@ -2191,7 +2195,7 @@ void IMTQHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) { } } -std::string IMTQHandler::makeStepString(const uint8_t step) { +std::string ImtqHandler::makeStepString(const uint8_t step) { std::string stepString(""); switch (step) { case IMTQ::SELF_TEST_STEPS::INIT: @@ -2226,7 +2230,7 @@ std::string IMTQHandler::makeStepString(const uint8_t step) { return stepString; } -ReturnValue_t IMTQHandler::getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) { +ReturnValue_t ImtqHandler::getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) { if (switcher != power::NO_SWITCH) { *numberOfSwitches = 1; *switches = &switcher; @@ -2234,3 +2238,11 @@ 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 similarity index 95% rename from mission/devices/IMTQHandler.h rename to mission/devices/ImtqHandler.h index 92df8aae..612eff7f 100644 --- a/mission/devices/IMTQHandler.h +++ b/mission/devices/ImtqHandler.h @@ -13,11 +13,11 @@ * * @author J. Meier */ -class IMTQHandler : public DeviceHandlerBase { +class ImtqHandler : public DeviceHandlerBase { public: - IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, + ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, power::Switch_t pwrSwitcher); - virtual ~IMTQHandler(); + virtual ~ImtqHandler(); /** * @brief Sets mode to MODE_NORMAL. Can be used for debugging. @@ -206,6 +206,16 @@ 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_ */