From a0356a50926019f0496acdf66b758432187504c2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 19 Oct 2022 15:36:57 +0200 Subject: [PATCH 01/12] imtq update --- mission/controller/AcsController.h | 3 + mission/devices/IMTQHandler.cpp | 1 + mission/devices/IMTQHandler.h | 1 + mission/devices/PCDUHandler.cpp | 11 +- .../devicedefinitions/GomspaceDefinitions.h | 6 +- .../IMTQHandlerDefinitions.h | 103 +++++++++++++----- tmtc | 2 +- 7 files changed, 86 insertions(+), 41 deletions(-) diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index c20ae94d..b1ad7090 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -44,6 +44,7 @@ class AcsController : public ExtendedControllerBase { RM3100::Rm3100PrimaryDataset mgm3Rm3100Set = RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER); IMTQ::RawMtmMeasurementSet imtqMgmSet = IMTQ::RawMtmMeasurementSet(objects::IMTQ_HANDLER); + IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER); PoolEntry mgm0PoolVec = PoolEntry(3); PoolEntry mgm1PoolVec = PoolEntry(3); @@ -70,6 +71,8 @@ class AcsController : public ExtendedControllerBase { SUS::SusDataset(objects::SUS_11_R_LOC_XBYMZB_PT_ZB), }; + + // Initial delay to make sure all pool variables have been initialized their owners Countdown initialCountdown = Countdown(INIT_DELAY); }; diff --git a/mission/devices/IMTQHandler.cpp b/mission/devices/IMTQHandler.cpp index 6ab5ed48..f6047c48 100644 --- a/mission/devices/IMTQHandler.cpp +++ b/mission/devices/IMTQHandler.cpp @@ -13,6 +13,7 @@ IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF* comC engHkDataset(this), calMtmMeasurementSet(this), rawMtmMeasurementSet(this), + dipoleSet(*this), posXselfTestDataset(this), negXselfTestDataset(this), posYselfTestDataset(this), diff --git a/mission/devices/IMTQHandler.h b/mission/devices/IMTQHandler.h index 5102b2e0..92df8aae 100644 --- a/mission/devices/IMTQHandler.h +++ b/mission/devices/IMTQHandler.h @@ -95,6 +95,7 @@ class IMTQHandler : public DeviceHandlerBase { IMTQ::EngHkDataset engHkDataset; IMTQ::CalibratedMtmMeasurementSet calMtmMeasurementSet; IMTQ::RawMtmMeasurementSet rawMtmMeasurementSet; + IMTQ::DipoleActuationSet dipoleSet; IMTQ::PosXSelfTestSet posXselfTestDataset; IMTQ::NegXSelfTestSet negXselfTestDataset; IMTQ::PosYSelfTestSet posYselfTestDataset; diff --git a/mission/devices/PCDUHandler.cpp b/mission/devices/PCDUHandler.cpp index 39a09d5a..91881db5 100644 --- a/mission/devices/PCDUHandler.cpp +++ b/mission/devices/PCDUHandler.cpp @@ -52,8 +52,8 @@ ReturnValue_t PCDUHandler::initialize() { return returnvalue::FAILED; } result = pdu2Handler->getSubscriptionInterface()->subscribeForSetUpdateMessage( - static_cast(P60System::SetIds::CORE), this->getObjectId(), - commandQueue->getId(), true); + static_cast(P60System::SetIds::CORE), this->getObjectId(), commandQueue->getId(), + true); if (result != returnvalue::OK) { sif::error << "PCDUHandler::initialize: Failed to subscribe for set update messages from " << "PDU2Handler" << std::endl; @@ -68,8 +68,8 @@ ReturnValue_t PCDUHandler::initialize() { return returnvalue::FAILED; } result = pdu1Handler->getSubscriptionInterface()->subscribeForSetUpdateMessage( - static_cast(P60System::SetIds::CORE), this->getObjectId(), - commandQueue->getId(), true); + static_cast(P60System::SetIds::CORE), this->getObjectId(), commandQueue->getId(), + true); if (result != returnvalue::OK) { sif::error << "PCDUHandler::initialize: Failed to subscribe for set update messages from " << "PDU1Handler" << std::endl; @@ -113,8 +113,7 @@ void PCDUHandler::handleChangedDataset(sid_t sid, store_address_t storeId, bool* if (sid == sid_t(objects::PDU2_HANDLER, static_cast(P60System::SetIds::CORE))) { updateHkTableDataset(storeId, &pdu2CoreHk, &timeStampPdu2HkDataset); updatePdu2SwitchStates(); - } else if (sid == - sid_t(objects::PDU1_HANDLER, static_cast(P60System::SetIds::CORE))) { + } else if (sid == sid_t(objects::PDU1_HANDLER, static_cast(P60System::SetIds::CORE))) { updateHkTableDataset(storeId, &pdu1CoreHk, &timeStampPdu1HkDataset); updatePdu1SwitchStates(); } else { diff --git a/mission/devices/devicedefinitions/GomspaceDefinitions.h b/mission/devices/devicedefinitions/GomspaceDefinitions.h index 325a2a0c..d2ffdc25 100644 --- a/mission/devices/devicedefinitions/GomspaceDefinitions.h +++ b/mission/devices/devicedefinitions/GomspaceDefinitions.h @@ -102,11 +102,7 @@ namespace P60System { enum class BatteryModes : uint8_t { CRITICAL = 1, SAFE = 2, NORMAL = 3, FULL = 4 }; -enum class SetIds : uint32_t { - CORE = 1, - AUX = 2, - CONFIG = 3 -}; +enum class SetIds : uint32_t { CORE = 1, AUX = 2, CONFIG = 3 }; } // namespace P60System diff --git a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h index 485be5da..ddb0af5b 100644 --- a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h +++ b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h @@ -1,6 +1,7 @@ #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_ #define MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_ +#include #include namespace IMTQ { @@ -27,15 +28,18 @@ static const uint8_t GET_TEMP_REPLY_SIZE = 2; static const uint8_t CFGR_CMD_SIZE = 3; static const uint8_t POINTER_REG_SIZE = 1; -static const uint32_t ENG_HK_DATA_SET_ID = 1; -static const uint32_t CAL_MTM_SET = 2; -static const uint32_t RAW_MTM_SET = 3; -static const uint32_t POS_X_TEST_DATASET = 4; -static const uint32_t NEG_X_TEST_DATASET = 5; -static const uint32_t POS_Y_TEST_DATASET = 6; -static const uint32_t NEG_Y_TEST_DATASET = 7; -static const uint32_t POS_Z_TEST_DATASET = 8; -static const uint32_t NEG_Z_TEST_DATASET = 9; +enum SetIds : uint32_t { + ENG_HK = 1, + CAL_MGM = 2, + RAW_MGM = 3, + POS_X_TEST = 4, + NEG_X_TEST = 5, + POS_Y_TEST = 6, + NEG_Y_TEST = 7, + POS_Z_TEST = 8, + NEG_Z_TEST = 9, + DIPOLES = 10 +}; static const uint8_t SIZE_ENG_HK_COMMAND = 1; static const uint8_t SIZE_STATUS_REPLY = 2; @@ -103,7 +107,7 @@ static const uint8_t Z_NEGATIVE = 0x6; static const uint8_t FINA = 0x7; } // namespace SELF_TEST_STEPS -enum IMTQPoolIds : lp_id_t { +enum PoolIds : lp_id_t { DIGITAL_VOLTAGE_MV, ANALOG_VOLTAGE_MV, DIGITAL_CURRENT, @@ -119,6 +123,10 @@ enum IMTQPoolIds : lp_id_t { ACTUATION_CAL_STATUS, MTM_RAW, ACTUATION_RAW_STATUS, + DIPOLES_X, + DIPOLES_Y, + DIPOLES_Z, + CURRENT_TORQUE_DURATION, INIT_POS_X_ERR, INIT_POS_X_RAW_MAG_X, @@ -375,9 +383,9 @@ enum IMTQPoolIds : lp_id_t { class EngHkDataset : public StaticLocalDataSet { public: - EngHkDataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, ENG_HK_DATA_SET_ID) {} + EngHkDataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, IMTQ::SetIds::ENG_HK) {} - EngHkDataset(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, ENG_HK_DATA_SET_ID)) {} + EngHkDataset(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::ENG_HK)) {} lp_var_t digitalVoltageMv = lp_var_t(sid.objectId, DIGITAL_VOLTAGE_MV, this); lp_var_t analogVoltageMv = lp_var_t(sid.objectId, ANALOG_VOLTAGE_MV, this); @@ -398,13 +406,14 @@ class EngHkDataset : public StaticLocalDataSet { */ class CalibratedMtmMeasurementSet : public StaticLocalDataSet { public: - CalibratedMtmMeasurementSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, CAL_MTM_SET) {} + CalibratedMtmMeasurementSet(HasLocalDataPoolIF* owner) + : StaticLocalDataSet(owner, IMTQ::SetIds::CAL_MGM) {} CalibratedMtmMeasurementSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, CAL_MTM_SET)) {} + : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::CAL_MGM)) {} /** The unit of all measurements is nT */ - lp_vec_t mgmXyz = lp_vec_t(sid.objectId, MGM_CAL_NT); + lp_vec_t mgmXyz = lp_vec_t(sid.objectId, MGM_CAL_NT, this); /** 1 if coils were actuating during measurement otherwise 0 */ lp_var_t coilActuationStatus = lp_var_t(sid.objectId, ACTUATION_CAL_STATUS, this); @@ -415,9 +424,11 @@ class CalibratedMtmMeasurementSet : public StaticLocalDataSet { public: - RawMtmMeasurementSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, RAW_MTM_SET) {} + RawMtmMeasurementSet(HasLocalDataPoolIF* owner) + : StaticLocalDataSet(owner, IMTQ::SetIds::RAW_MGM) {} - RawMtmMeasurementSet(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, RAW_MTM_SET)) {} + RawMtmMeasurementSet(object_id_t objectId) + : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::RAW_MGM)) {} /** The unit of all measurements is nT */ lp_vec_t mtmRawNt = lp_vec_t(sid.objectId, MTM_RAW, this); @@ -462,6 +473,40 @@ class CommandDipolePacket : public SerialLinkedListAdapter { SerializeElement duration; }; +class DipoleActuationSet : public StaticLocalDataSet<4> { + public: + DipoleActuationSet(HasLocalDataPoolIF& owner) + : StaticLocalDataSet(&owner, IMTQ::SetIds::DIPOLES) {} + DipoleActuationSet(object_id_t objectId) + : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::DIPOLES)) {} + + void setDipoles(uint16_t xDipole_, uint16_t yDipole_, uint16_t zDipole_, + uint16_t currentTorqueDurationMs_) { + PoolReadGuard pg(this); + 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_; + } + + 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 currentTorqueDurationMs = + lp_var_t(sid.objectId, CURRENT_TORQUE_DURATION, this); + bool newActuation = false; +}; /** * @brief This dataset can be used to store the self test results of the +X self test. * @@ -479,10 +524,10 @@ class CommandDipolePacket : public SerialLinkedListAdapter { class PosXSelfTestSet : public StaticLocalDataSet { public: PosXSelfTestSet(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, IMTQ::POS_X_TEST_DATASET) {} + : StaticLocalDataSet(owner, IMTQ::SetIds::POS_X_TEST) {} PosXSelfTestSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, IMTQ::POS_X_TEST_DATASET)) {} + : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::POS_X_TEST)) {} /** INIT block */ lp_var_t initErr = lp_var_t(sid.objectId, INIT_POS_X_ERR, this); @@ -556,10 +601,10 @@ class PosXSelfTestSet : public StaticLocalDataSet { class NegXSelfTestSet : public StaticLocalDataSet { public: NegXSelfTestSet(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, IMTQ::NEG_X_TEST_DATASET) {} + : StaticLocalDataSet(owner, IMTQ::SetIds::NEG_X_TEST) {} NegXSelfTestSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, IMTQ::NEG_X_TEST_DATASET)) {} + : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::NEG_X_TEST)) {} /** INIT block */ lp_var_t initErr = lp_var_t(sid.objectId, INIT_NEG_X_ERR, this); @@ -633,10 +678,10 @@ class NegXSelfTestSet : public StaticLocalDataSet { class PosYSelfTestSet : public StaticLocalDataSet { public: PosYSelfTestSet(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, IMTQ::POS_Y_TEST_DATASET) {} + : StaticLocalDataSet(owner, IMTQ::SetIds::POS_Y_TEST) {} PosYSelfTestSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, IMTQ::POS_Y_TEST_DATASET)) {} + : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::POS_Y_TEST)) {} /** INIT block */ lp_var_t initErr = lp_var_t(sid.objectId, INIT_POS_Y_ERR, this); @@ -710,10 +755,10 @@ class PosYSelfTestSet : public StaticLocalDataSet { class NegYSelfTestSet : public StaticLocalDataSet { public: NegYSelfTestSet(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, IMTQ::NEG_Y_TEST_DATASET) {} + : StaticLocalDataSet(owner, IMTQ::SetIds::NEG_Y_TEST) {} NegYSelfTestSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, IMTQ::NEG_Y_TEST_DATASET)) {} + : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::NEG_Y_TEST)) {} /** INIT block */ lp_var_t initErr = lp_var_t(sid.objectId, INIT_NEG_Y_ERR, this); @@ -787,10 +832,10 @@ class NegYSelfTestSet : public StaticLocalDataSet { class PosZSelfTestSet : public StaticLocalDataSet { public: PosZSelfTestSet(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, IMTQ::POS_Z_TEST_DATASET) {} + : StaticLocalDataSet(owner, IMTQ::SetIds::POS_Z_TEST) {} PosZSelfTestSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, IMTQ::POS_Z_TEST_DATASET)) {} + : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::POS_Z_TEST)) {} /** INIT block */ lp_var_t initErr = lp_var_t(sid.objectId, INIT_POS_Z_ERR, this); @@ -864,10 +909,10 @@ class PosZSelfTestSet : public StaticLocalDataSet { class NegZSelfTestSet : public StaticLocalDataSet { public: NegZSelfTestSet(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, IMTQ::NEG_Z_TEST_DATASET) {} + : StaticLocalDataSet(owner, IMTQ::SetIds::NEG_Z_TEST) {} NegZSelfTestSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, IMTQ::NEG_Z_TEST_DATASET)) {} + : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::NEG_Z_TEST)) {} /** INIT block */ lp_var_t initErr = lp_var_t(sid.objectId, INIT_NEG_Z_ERR, this); diff --git a/tmtc b/tmtc index b2bab4c9..87e40fab 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit b2bab4c964848d6f78a6f44525166dcf24cc46b6 +Subproject commit 87e40fab0f45b4b40f0f477ad5e946e61dcb5e07 From 386fc604417f70abf9295d42d595915609eca983 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 19 Oct 2022 18:52:16 +0200 Subject: [PATCH 02/12] new static variables to make torque status available --- bsp_q7s/core/ObjectFactory.cpp | 4 +- mission/devices/CMakeLists.txt | 2 +- .../{IMTQHandler.cpp => ImtqHandler.cpp} | 82 +++++++++++-------- .../devices/{IMTQHandler.h => ImtqHandler.h} | 16 +++- 4 files changed, 63 insertions(+), 41 deletions(-) rename mission/devices/{IMTQHandler.cpp => ImtqHandler.cpp} (97%) rename mission/devices/{IMTQHandler.h => ImtqHandler.h} (95%) 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_ */ From 6bdc420d0ecd906fb8ea819350d57450e412cc5d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 19 Oct 2022 19:08:36 +0200 Subject: [PATCH 03/12] bump tmtc --- mission/devices/ImtqHandler.cpp | 5 ++++- tmtc | 2 +- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index adb9f83b..e8cb8c6b 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -692,7 +693,9 @@ void ImtqHandler::fillEngHkDataset(const uint8_t* packet) { offset += 2; engHkDataset.coilZTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); offset += 2; - engHkDataset.mcuTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); + size_t dummy = 2; + SerializeAdapter::deSerialize(&engHkDataset.mcuTemperature.value, packet + offset, &dummy, + SerializeIF::Endianness::LITTLE); engHkDataset.setValidity(true, true); diff --git a/tmtc b/tmtc index 87e40fab..42b962de 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 87e40fab0f45b4b40f0f477ad5e946e61dcb5e07 +Subproject commit 42b962dede547975488e72b22abe2360065c0404 From 42bb2f554fa46eae403a570d719901c8a597cdbb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 19 Oct 2022 19:11:21 +0200 Subject: [PATCH 04/12] added fat TODO --- mission/devices/ImtqHandler.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index e8cb8c6b..f040edf2 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -44,6 +44,11 @@ void ImtqHandler::doStartUp() { void ImtqHandler::doShutDown() { setMode(_MODE_POWER_DOWN); } ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { + // TODO: Add IMTQ torque handling here, will be set in the software and thus must be part + // of normal handling. Keep in mind that torqueing should happen after all HK data was read + // because the MGM values might be useless if the IMTQ is torqueing. Also, remove + // calibrated MGM polling, we have our own calibration. Thus, there are 3 communication + // steps per IMTQ cycle. switch (communicationStep) { case CommunicationStep::GET_ENG_HK_DATA: *id = IMTQ::GET_ENG_HK_DATA; From ab273a8a597348e285d70fcf5b4ad5a64b396bb3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 20 Oct 2022 10:32:17 +0200 Subject: [PATCH 05/12] re-use dipole set in IMTQ handler --- bsp_q7s/core/ObjectFactory.cpp | 3 +- mission/controller/AcsController.h | 2 - mission/devices/ImtqHandler.cpp | 109 ++++++++++++++---- mission/devices/ImtqHandler.h | 14 ++- .../IMTQHandlerDefinitions.h | 13 ++- 5 files changed, 107 insertions(+), 34 deletions(-) diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index deee8c53..f6beadb6 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -47,6 +47,8 @@ #if OBSW_TEST_LIBGPIOD == 1 #include "linux/boardtest/LibgpiodTest.h" #endif +#include + #include #include "fsfw/datapoollocal/LocalDataPoolManager.h" @@ -70,7 +72,6 @@ #include "mission/devices/BpxBatteryHandler.h" #include "mission/devices/GyroADIS1650XHandler.h" #include "mission/devices/HeaterHandler.h" -#include #include "mission/devices/Max31865PT1000Handler.h" #include "mission/devices/P60DockHandler.h" #include "mission/devices/PCDUHandler.h" diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index b1ad7090..290938b4 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -71,8 +71,6 @@ class AcsController : public ExtendedControllerBase { SUS::SusDataset(objects::SUS_11_R_LOC_XBYMZB_PT_ZB), }; - - // Initial delay to make sure all pool variables have been initialized their owners Countdown initialCountdown = Countdown(INIT_DELAY); }; diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index f040edf2..23de759b 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -1,11 +1,29 @@ +#include +#include +#include #include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include +#include +#include +#include +#include +#include #include #include - -#include "OBSWConfig.h" +#include MutexIF* ImtqHandler::TORQUE_LOCK = nullptr; bool ImtqHandler::TORQUEING = false; @@ -56,22 +74,43 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { break; case CommunicationStep::START_MTM_MEASUREMENT: *id = IMTQ::START_MTM_MEASUREMENT; - communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT; - break; - case CommunicationStep::GET_CAL_MTM_MEASUREMENT: - *id = IMTQ::GET_CAL_MTM_MEASUREMENT; - communicationStep = CommunicationStep::GET_RAW_MTM_MEASUREMENT; + if (pollingMode == NormalPollingMode::BOTH or + pollingMode == NormalPollingMode::UNCALIBRATED) { + communicationStep = CommunicationStep::GET_RAW_MTM_MEASUREMENT; + } else { + communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT; + } break; case CommunicationStep::GET_RAW_MTM_MEASUREMENT: + if (integrationTimeCd.getRemainingMillis() > 0) { + TaskFactory::delayTask(integrationTimeCd.getRemainingMillis()); + } *id = IMTQ::GET_RAW_MTM_MEASUREMENT; + if (pollingMode == NormalPollingMode::BOTH) { + communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT; + } else { + communicationStep = CommunicationStep::DIPOLE_ACTUATION; + } + break; + case CommunicationStep::GET_CAL_MTM_MEASUREMENT: + if (integrationTimeCd.getRemainingMillis() > 0) { + TaskFactory::delayTask(integrationTimeCd.getRemainingMillis()); + } + *id = IMTQ::GET_CAL_MTM_MEASUREMENT; + communicationStep = CommunicationStep::DIPOLE_ACTUATION; + break; + case CommunicationStep::DIPOLE_ACTUATION: { + // TODO: Set correct ID if actuation is necessary + *id = IMTQ::START_ACTUATION_DIPOLE; communicationStep = CommunicationStep::GET_ENG_HK_DATA; break; + } default: sif::debug << "IMTQHandler::buildNormalDeviceCommand: Invalid communication step" << std::endl; break; } - return buildCommandFromCommand(*id, NULL, 0); + return buildCommandFromCommand(*id, nullptr, 0); } ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { @@ -133,20 +172,27 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma case (IMTQ::START_ACTUATION_DIPOLE): { /* IMTQ expects low byte first */ commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE; - if (commandData == nullptr) { + if (commandData != nullptr && commandDataLen < 8) { return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; } - commandBuffer[1] = commandData[1]; - commandBuffer[2] = commandData[0]; - commandBuffer[3] = commandData[3]; - commandBuffer[4] = commandData[2]; - commandBuffer[5] = commandData[5]; - commandBuffer[6] = commandData[4]; - commandBuffer[7] = commandData[7]; - commandBuffer[8] = commandData[6]; - rawPacket = commandBuffer; - rawPacketLen = 9; - return returnvalue::OK; + PoolReadGuard pg(&dipoleSet); + ReturnValue_t result; + // Commands override anything which was set in the software + if (commandBuffer != nullptr) { + result = + dipoleSet.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::NETWORK); + if (result != returnvalue::OK) { + return result; + } + } + result = buildDipoleActuationCommand(); + if (result != returnvalue::OK) { + return result; + } + MutexGuard mg(TORQUE_LOCK); + TORQUEING = true; + TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value); + return result; } case (IMTQ::GET_ENG_HK_DATA): { commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA; @@ -162,6 +208,7 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma } case (IMTQ::START_MTM_MEASUREMENT): { commandBuffer[0] = IMTQ::CC::START_MTM_MEASUREMENT; + integrationTimeCd.resetTimer(); rawPacket = commandBuffer; rawPacketLen = 1; return returnvalue::OK; @@ -184,6 +231,22 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma return returnvalue::FAILED; } +ReturnValue_t ImtqHandler::buildDipoleActuationCommand() { + commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE; + uint8_t* serPtr = commandBuffer + 1; + size_t serSize = 1; + dipoleSet.setValidityBufferGeneration(false); + ReturnValue_t result = dipoleSet.serialize(&serPtr, &serSize, sizeof(commandBuffer), + SerializeIF::Endianness::LITTLE); + dipoleSet.setValidityBufferGeneration(true); + if (result != returnvalue::OK) { + return result; + } + rawPacket = commandBuffer; + rawPacketLen = 9; + return result; +} + 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); @@ -700,7 +763,7 @@ void ImtqHandler::fillEngHkDataset(const uint8_t* packet) { offset += 2; size_t dummy = 2; SerializeAdapter::deSerialize(&engHkDataset.mcuTemperature.value, packet + offset, &dummy, - SerializeIF::Endianness::LITTLE); + SerializeIF::Endianness::LITTLE); engHkDataset.setValidity(true, true); @@ -2247,7 +2310,7 @@ ReturnValue_t ImtqHandler::getSwitches(const uint8_t** switches, uint8_t* number return DeviceHandlerBase::NO_SWITCH; } -bool ImtqHandler::mgtIsTorqueing(dur_millis_t *remainingTorqueDuration) { +bool ImtqHandler::mgtIsTorqueing(dur_millis_t* remainingTorqueDuration) { MutexGuard mg(TORQUE_LOCK); if (TORQUEING and remainingTorqueDuration != nullptr) { *remainingTorqueDuration = TORQUE_COUNTDOWN.getRemainingMillis() + TORQUE_BUFFER_TIME_MS; diff --git a/mission/devices/ImtqHandler.h b/mission/devices/ImtqHandler.h index 612eff7f..401c6743 100644 --- a/mission/devices/ImtqHandler.h +++ b/mission/devices/ImtqHandler.h @@ -15,10 +15,14 @@ */ class ImtqHandler : public DeviceHandlerBase { public: + enum NormalPollingMode { UNCALIBRATED = 0, CALIBRATED = 1, BOTH = 2 }; + ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, power::Switch_t pwrSwitcher); virtual ~ImtqHandler(); + void setPollingMode(NormalPollingMode pollMode); + /** * @brief Sets mode to MODE_NORMAL. Can be used for debugging. */ @@ -103,7 +107,12 @@ class ImtqHandler : public DeviceHandlerBase { IMTQ::PosZSelfTestSet posZselfTestDataset; IMTQ::NegZSelfTestSet negZselfTestDataset; + NormalPollingMode pollingMode = NormalPollingMode::BOTH; + PoolEntry mgmCalEntry = PoolEntry(3); + // Hardcoded to default integration time of 10 ms. + // SHOULDDO: Support for other integration times + Countdown integrationTimeCd = Countdown(10); power::Switch_t switcher = power::NO_SWITCH; @@ -115,7 +124,8 @@ class ImtqHandler : public DeviceHandlerBase { GET_ENG_HK_DATA, START_MTM_MEASUREMENT, GET_CAL_MTM_MEASUREMENT, - GET_RAW_MTM_MEASUREMENT + GET_RAW_MTM_MEASUREMENT, + DIPOLE_ACTUATION }; CommunicationStep communicationStep = CommunicationStep::GET_ENG_HK_DATA; @@ -197,6 +207,7 @@ class ImtqHandler : public DeviceHandlerBase { void handlePositiveZSelfTestReply(const uint8_t* packet); void handleNegativeZSelfTestReply(const uint8_t* packet); + ReturnValue_t buildDipoleActuationCommand(); /** * @brief This function checks the error byte of a self test measurement. * @@ -215,7 +226,6 @@ class ImtqHandler : public DeviceHandlerBase { 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 index ddb0af5b..df9aa44e 100644 --- a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h +++ b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h @@ -448,6 +448,11 @@ class CommandDipolePacket : public SerialLinkedListAdapter { public: CommandDipolePacket() { setLinks(); } + SerializeElement xDipole; + SerializeElement yDipole; + SerializeElement zDipole; + SerializeElement duration; + private: /** * @brief Constructor @@ -467,10 +472,6 @@ class CommandDipolePacket : public SerialLinkedListAdapter { yDipole.setNext(&zDipole); zDipole.setNext(&duration); } - SerializeElement xDipole; - SerializeElement yDipole; - SerializeElement zDipole; - SerializeElement duration; }; class DipoleActuationSet : public StaticLocalDataSet<4> { @@ -498,13 +499,13 @@ class DipoleActuationSet : public StaticLocalDataSet<4> { zDipole = zDipole_; currentTorqueDurationMs = currentTorqueDurationMs_; } - - 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 currentTorqueDurationMs = lp_var_t(sid.objectId, CURRENT_TORQUE_DURATION, this); + + private: bool newActuation = false; }; /** From 2b6334b9dcbc2d49e650f0bf9d68b949713071e8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 20 Oct 2022 10:44:28 +0200 Subject: [PATCH 06/12] this works --- mission/devices/ImtqHandler.cpp | 11 +++++++++-- mission/devices/ImtqHandler.h | 4 ++++ .../devicedefinitions/IMTQHandlerDefinitions.h | 8 +++++++- 3 files changed, 20 insertions(+), 3 deletions(-) diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index 23de759b..5e78b90f 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -100,8 +100,10 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { communicationStep = CommunicationStep::DIPOLE_ACTUATION; break; case CommunicationStep::DIPOLE_ACTUATION: { - // TODO: Set correct ID if actuation is necessary - *id = IMTQ::START_ACTUATION_DIPOLE; + // The dipoles will be set by the ACS controller directly using the dipole local pool set. + if (dipoleSet.newActuation) { + *id = IMTQ::START_ACTUATION_DIPOLE; + } communicationStep = CommunicationStep::GET_ENG_HK_DATA; break; } @@ -404,6 +406,11 @@ ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDat localDataPoolMap.emplace(IMTQ::COIL_Z_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry({0})); + localDataPoolMap.emplace(IMTQ::DIPOLES_X, &dipoleXEntry); + localDataPoolMap.emplace(IMTQ::DIPOLES_Y, &dipoleYEntry); + localDataPoolMap.emplace(IMTQ::DIPOLES_Z, &dipoleZEntry); + localDataPoolMap.emplace(IMTQ::CURRENT_TORQUE_DURATION, &torqueDurationEntry); + /** Entries of calibrated MTM measurement dataset */ localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, &mgmCalEntry); localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry({0})); diff --git a/mission/devices/ImtqHandler.h b/mission/devices/ImtqHandler.h index 401c6743..7ddb4e4c 100644 --- a/mission/devices/ImtqHandler.h +++ b/mission/devices/ImtqHandler.h @@ -110,6 +110,10 @@ class ImtqHandler : public DeviceHandlerBase { NormalPollingMode pollingMode = NormalPollingMode::BOTH; PoolEntry mgmCalEntry = PoolEntry(3); + PoolEntry dipoleXEntry = PoolEntry(0, false); + PoolEntry dipoleYEntry = PoolEntry(0, false); + PoolEntry dipoleZEntry = PoolEntry(0, false); + PoolEntry torqueDurationEntry = PoolEntry(0, false); // Hardcoded to default integration time of 10 ms. // SHOULDDO: Support for other integration times Countdown integrationTimeCd = Countdown(10); diff --git a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h index df9aa44e..3ec0aedd 100644 --- a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h +++ b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h @@ -4,6 +4,8 @@ #include #include +class ImtqHandler; + namespace IMTQ { static const DeviceCommandId_t NONE = 0x0; @@ -475,6 +477,8 @@ class CommandDipolePacket : public SerialLinkedListAdapter { }; class DipoleActuationSet : public StaticLocalDataSet<4> { + friend class ::ImtqHandler; + public: DipoleActuationSet(HasLocalDataPoolIF& owner) : StaticLocalDataSet(&owner, IMTQ::SetIds::DIPOLES) {} @@ -499,15 +503,17 @@ class DipoleActuationSet : public StaticLocalDataSet<4> { zDipole = zDipole_; currentTorqueDurationMs = currentTorqueDurationMs_; } + + 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 currentTorqueDurationMs = lp_var_t(sid.objectId, CURRENT_TORQUE_DURATION, this); - private: bool newActuation = false; }; + /** * @brief This dataset can be used to store the self test results of the +X self test. * From 07fe2cb1229f0f4b20f5eb019c8cc313eb21518b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 20 Oct 2022 10:51:19 +0200 Subject: [PATCH 07/12] some tweaks for dipole set handling --- mission/devices/ImtqHandler.cpp | 5 ++++- .../devicedefinitions/IMTQHandlerDefinitions.h | 13 ++++++++++++- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index 5e78b90f..5a331c11 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -100,7 +100,10 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { communicationStep = CommunicationStep::DIPOLE_ACTUATION; break; case CommunicationStep::DIPOLE_ACTUATION: { - // The dipoles will be set by the ACS controller directly using the dipole local pool set. + // 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) { *id = IMTQ::START_ACTUATION_DIPOLE; } diff --git a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h index 3ec0aedd..320220dd 100644 --- a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h +++ b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h @@ -485,9 +485,14 @@ class DipoleActuationSet : public StaticLocalDataSet<4> { DipoleActuationSet(object_id_t objectId) : 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 setDipoles(uint16_t xDipole_, uint16_t yDipole_, uint16_t zDipole_, uint16_t currentTorqueDurationMs_) { - PoolReadGuard pg(this); newActuation = false; if (xDipole.value != xDipole_) { newActuation = true; @@ -504,6 +509,12 @@ class DipoleActuationSet : public StaticLocalDataSet<4> { currentTorqueDurationMs = currentTorqueDurationMs_; } + void getDipoles(uint16_t& xDipole_, uint16_t& yDipole_, uint16_t& zDipole_) { + xDipole_ = xDipole.value; + yDipole_ = yDipole.value; + zDipole_ = zDipole.value; + } + 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); From ada1111252229ad544667f34a2c44030d0ec8521 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 20 Oct 2022 10:52:13 +0200 Subject: [PATCH 08/12] removed TODO --- mission/devices/ImtqHandler.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index 5a331c11..cc80d1f9 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -62,11 +62,7 @@ void ImtqHandler::doStartUp() { void ImtqHandler::doShutDown() { setMode(_MODE_POWER_DOWN); } ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { - // TODO: Add IMTQ torque handling here, will be set in the software and thus must be part - // of normal handling. Keep in mind that torqueing should happen after all HK data was read - // because the MGM values might be useless if the IMTQ is torqueing. Also, remove - // calibrated MGM polling, we have our own calibration. Thus, there are 3 communication - // steps per IMTQ cycle. + // Depending on the normal polling mode configuration, 3-4 communication steps are recommended switch (communicationStep) { case CommunicationStep::GET_ENG_HK_DATA: *id = IMTQ::GET_ENG_HK_DATA; From 29a34256a72c97391556d6b970161a2b6f2fc620 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 20 Oct 2022 15:08:33 +0200 Subject: [PATCH 09/12] 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 From 75a4cd1b6977166bdfaaf684ad10917aea161cdc Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 20 Oct 2022 16:09:18 +0200 Subject: [PATCH 10/12] awful solution but works --- fsfw | 2 +- .../pollingSequenceFactory.cpp | 18 ++++++++++--- mission/controller/AcsController.cpp | 12 ++++----- mission/controller/AcsController.h | 1 - mission/devices/ImtqHandler.cpp | 27 ++++++++++++++++--- mission/devices/ImtqHandler.h | 1 + .../imtqHandlerDefinitions.h | 1 + 7 files changed, 47 insertions(+), 15 deletions(-) diff --git a/fsfw b/fsfw index 0c5c2f6c..73454c62 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 0c5c2f6c4f07959a73a083eb3c6e1f3125642477 +Subproject commit 73454c629c042de8efe7aa4fa6dcbf1e184b0961 diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp index 0d75588f..59f07d1f 100644 --- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp +++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp @@ -430,11 +430,21 @@ ReturnValue_t pst::pstI2c(FixedTimeslotTaskIF *thisSequence) { uint32_t length = thisSequence->getPeriodMs(); static_cast(length); #if OBSW_ADD_MGT == 1 - thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::PERFORM_OPERATION); thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_READ); #endif #if OBSW_ADD_BPX_BATTERY_HANDLER == 1 thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index b8b1e8ca..c94a2cb6 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -32,12 +32,12 @@ void AcsController::performControlOperation() { } { - // 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); + // 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); } { diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index adb6fbb6..0cf38bc7 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -20,7 +20,6 @@ class AcsController : public ExtendedControllerBase { enum class InternalState { STARTUP, INITIAL_DELAY, READY }; InternalState internalState = InternalState::STARTUP; - ReturnValue_t handleCommandMessage(CommandMessage* message) override; void performControlOperation() override; diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index c65674ce..bec21d95 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -27,6 +27,8 @@ #include "mission/devices/torquer.h" +static constexpr bool ACTUATION_WIRETAPPING = false; + ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, power::Switch_t pwrSwitcher) : DeviceHandlerBase(objectId, comIF, comCookie), @@ -46,7 +48,7 @@ ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comC } } -ImtqHandler::~ImtqHandler() {} +ImtqHandler::~ImtqHandler() = default; void ImtqHandler::doStartUp() { if (goToNormalMode) { @@ -182,7 +184,7 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma } ReturnValue_t result; // Commands override anything which was set in the software - if (commandBuffer != nullptr) { + if (commandData != nullptr) { dipoleSet.setValidityBufferGeneration(false); result = dipoleSet.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::NETWORK); @@ -194,6 +196,11 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma // Read set dipole values from local pool PoolReadGuard pg(&dipoleSet); } + if (ACTUATION_WIRETAPPING) { + sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.xDipole.value + << ", y = " << dipoleSet.yDipole.value << ", z = " << dipoleSet.zDipole.value + << ", duration = " << dipoleSet.currentTorqueDurationMs.value << std::endl; + } result = buildDipoleActuationCommand(); if (result != returnvalue::OK) { return result; @@ -312,8 +319,17 @@ ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSi *foundLen = IMTQ::SIZE_SELF_TEST_RESULTS; *foundId = IMTQ::GET_SELF_TEST_RESULT; break; + case (IMTQ::CC::PAST_AVAILABLE_RESPONSE_BYTES): { + sif::warning << "IMTQHandler::scanForReply: Read 0xFF command byte, reading past available " + "bytes. Keep 1 ms delay" + " between I2C send and read" + << std::endl; + result = IGNORE_REPLY_DATA; + break; + } default: - sif::debug << "IMTQHandler::scanForReply: Reply contains invalid command code" << std::endl; + sif::debug << "IMTQHandler::scanForReply: Reply with length " << remainingSize + << "contains invalid command code " << static_cast(*start) << std::endl; result = IGNORE_REPLY_DATA; break; } @@ -2276,6 +2292,11 @@ void ImtqHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) { } } +void ImtqHandler::doSendRead() { + TaskFactory::delayTask(1); + DeviceHandlerBase::doSendRead(); +} + std::string ImtqHandler::makeStepString(const uint8_t step) { std::string stepString(""); switch (step) { diff --git a/mission/devices/ImtqHandler.h b/mission/devices/ImtqHandler.h index d414be0f..cfad9d7f 100644 --- a/mission/devices/ImtqHandler.h +++ b/mission/devices/ImtqHandler.h @@ -23,6 +23,7 @@ class ImtqHandler : public DeviceHandlerBase { void setPollingMode(NormalPollingMode pollMode); + void doSendRead() override; /** * @brief Sets mode to MODE_NORMAL. Can be used for debugging. */ diff --git a/mission/devices/devicedefinitions/imtqHandlerDefinitions.h b/mission/devices/devicedefinitions/imtqHandlerDefinitions.h index 1799d347..b3598970 100644 --- a/mission/devices/devicedefinitions/imtqHandlerDefinitions.h +++ b/mission/devices/devicedefinitions/imtqHandlerDefinitions.h @@ -86,6 +86,7 @@ static const uint8_t GET_COMMANDED_DIPOLE = 0x46; static const uint8_t GET_RAW_MTM_MEASUREMENT = 0x42; static const uint8_t GET_CAL_MTM_MEASUREMENT = 0x43; static const uint8_t GET_SELF_TEST_RESULT = 0x47; +static const uint8_t PAST_AVAILABLE_RESPONSE_BYTES = 0xff; }; // namespace CC namespace SELF_TEST_AXIS { From bff4f6fa8d0b03cb81e5bae60f22b5df2ac08cb0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 20 Oct 2022 16:09:49 +0200 Subject: [PATCH 11/12] reformatinng --- mission/devices/ImtqHandler.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index bec21d95..d27b6f65 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -321,9 +321,7 @@ ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSi break; case (IMTQ::CC::PAST_AVAILABLE_RESPONSE_BYTES): { sif::warning << "IMTQHandler::scanForReply: Read 0xFF command byte, reading past available " - "bytes. Keep 1 ms delay" - " between I2C send and read" - << std::endl; + "bytes. Keep 1 ms delay between I2C send and read" << std::endl; result = IGNORE_REPLY_DATA; break; } From a3ed5aef8e85001b4163239c3dcebdf056817216 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 24 Oct 2022 14:26:35 +0200 Subject: [PATCH 12/12] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 754b71a3..b0c5a49b 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 754b71a35fc27208d7c679ea58783cacda973996 +Subproject commit b0c5a49b504708ec9130228100d7bbd49025598d