diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index e07dd22d..06965ca0 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -49,6 +49,8 @@ #if OBSW_TEST_LIBGPIOD == 1 #include "linux/boardtest/LibgpiodTest.h" #endif +#include + #include #include "fsfw/datapoollocal/LocalDataPoolManager.h" @@ -72,7 +74,6 @@ #include "mission/devices/BpxBatteryHandler.h" #include "mission/devices/GyroADIS1650XHandler.h" #include "mission/devices/HeaterHandler.h" -#include "mission/devices/IMTQHandler.h" #include "mission/devices/Max31865PT1000Handler.h" #include "mission/devices/P60DockHandler.h" #include "mission/devices/PCDUHandler.h" @@ -897,7 +898,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); imtqHandler->connectModeTreeParent(satsystem::ACS_SUBSYSTEM); 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/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 8c2d1c9a..788ee1c4 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), 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 c20ae94d..0cf38bc7 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: @@ -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; @@ -44,6 +43,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); diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index e76c8f7e..5beccb4e 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 56184ea4..b8fb326c 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 @@ -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 similarity index 92% rename from mission/devices/IMTQHandler.cpp rename to mission/devices/ImtqHandler.cpp index 6ab5ed48..d27b6f65 100644 --- a/mission/devices/IMTQHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -1,18 +1,41 @@ -#include "IMTQHandler.h" - +#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 -#include "OBSWConfig.h" +#include "mission/devices/torquer.h" -IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, +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), engHkDataset(this), calMtmMeasurementSet(this), rawMtmMeasurementSet(this), + dipoleSet(*this), posXselfTestDataset(this), negXselfTestDataset(this), posYselfTestDataset(this), @@ -25,9 +48,9 @@ IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF* comC } } -IMTQHandler::~IMTQHandler() {} +ImtqHandler::~ImtqHandler() = default; -void IMTQHandler::doStartUp() { +void ImtqHandler::doStartUp() { if (goToNormalMode) { setMode(MODE_NORMAL); } else { @@ -35,9 +58,11 @@ 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) { + bool buildCommand = true; + // 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; @@ -45,29 +70,61 @@ 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: { + // 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. + 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; + } default: sif::debug << "IMTQHandler::buildNormalDeviceCommand: Invalid communication step" << std::endl; break; } - return buildCommandFromCommand(*id, NULL, 0); -} - -ReturnValue_t IMTQHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { + if (buildCommand) { + return buildCommandFromCommand(*id, nullptr, 0); + } return NOTHING_TO_SEND; } -ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, +ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { + return NOTHING_TO_SEND; +} + +ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t* commandData, size_t commandDataLen) { switch (deviceCommand) { @@ -122,20 +179,36 @@ 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; + ReturnValue_t result; + // Commands override anything which was set in the software + if (commandData != 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); + } + 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; + } + MutexGuard mg(torquer::lazyLock()); + torquer::TORQUEING = true; + torquer::TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value); + return result; } case (IMTQ::GET_ENG_HK_DATA): { commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA; @@ -151,6 +224,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; @@ -173,30 +247,42 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma return returnvalue::FAILED; } -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); +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; } -ReturnValue_t IMTQHandler::scanForReply(const uint8_t* start, size_t remainingSize, +void ImtqHandler::fillCommandAndReplyMap() { + 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, DeviceCommandId_t* foundId, size_t* foundLen) { ReturnValue_t result = returnvalue::OK; @@ -233,8 +319,15 @@ 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; } @@ -242,7 +335,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); @@ -286,9 +379,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()) { @@ -313,9 +406,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})); @@ -330,6 +423,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})); @@ -611,7 +709,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: @@ -630,7 +728,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: @@ -661,7 +759,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); @@ -687,7 +785,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); @@ -708,9 +808,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; @@ -734,7 +834,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); @@ -746,7 +846,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; @@ -776,7 +876,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; @@ -824,7 +924,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)); @@ -858,7 +958,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; @@ -1070,7 +1170,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; @@ -1282,7 +1382,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; @@ -1494,7 +1594,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; @@ -1706,7 +1806,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; @@ -1918,7 +2018,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; @@ -2130,9 +2230,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); @@ -2190,7 +2290,12 @@ void IMTQHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) { } } -std::string IMTQHandler::makeStepString(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) { case IMTQ::SELF_TEST_STEPS::INIT: @@ -2225,7 +2330,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; diff --git a/mission/devices/IMTQHandler.h b/mission/devices/ImtqHandler.h similarity index 90% rename from mission/devices/IMTQHandler.h rename to mission/devices/ImtqHandler.h index 5102b2e0..cfad9d7f 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" @@ -13,12 +13,17 @@ * * @author J. Meier */ -class IMTQHandler : public DeviceHandlerBase { +class ImtqHandler : public DeviceHandlerBase { public: - IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, - power::Switch_t pwrSwitcher); - virtual ~IMTQHandler(); + 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); + + void doSendRead() override; /** * @brief Sets mode to MODE_NORMAL. Can be used for debugging. */ @@ -95,6 +100,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; @@ -102,7 +108,16 @@ class IMTQHandler : public DeviceHandlerBase { IMTQ::PosZSelfTestSet posZselfTestDataset; IMTQ::NegZSelfTestSet negZselfTestDataset; + NormalPollingMode pollingMode = NormalPollingMode::UNCALIBRATED; + 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); power::Switch_t switcher = power::NO_SWITCH; @@ -114,7 +129,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; @@ -196,6 +212,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. * diff --git a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h b/mission/devices/devicedefinitions/imtqHandlerDefinitions.h similarity index 92% rename from mission/devices/devicedefinitions/IMTQHandlerDefinitions.h rename to mission/devices/devicedefinitions/imtqHandlerDefinitions.h index 485be5da..b3598970 100644 --- a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h +++ b/mission/devices/devicedefinitions/imtqHandlerDefinitions.h @@ -1,8 +1,11 @@ #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_ #define MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_ +#include #include +class ImtqHandler; + namespace IMTQ { static const DeviceCommandId_t NONE = 0x0; @@ -27,15 +30,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; @@ -80,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 { @@ -103,7 +110,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 +126,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 +386,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 +409,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 +427,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); @@ -437,6 +451,11 @@ class CommandDipolePacket : public SerialLinkedListAdapter { public: CommandDipolePacket() { setLinks(); } + SerializeElement xDipole; + SerializeElement yDipole; + SerializeElement zDipole; + SerializeElement duration; + private: /** * @brief Constructor @@ -456,10 +475,46 @@ class CommandDipolePacket : public SerialLinkedListAdapter { yDipole.setNext(&zDipole); zDipole.setNext(&duration); } - SerializeElement xDipole; - SerializeElement yDipole; - SerializeElement zDipole; - SerializeElement duration; +}; + +class DipoleActuationSet : public StaticLocalDataSet<4> { + friend class ::ImtqHandler; + + public: + DipoleActuationSet(HasLocalDataPoolIF& owner) + : StaticLocalDataSet(&owner, IMTQ::SetIds::DIPOLES) {} + 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_) { currentTorqueDurationMs = durationMs_; } + + void setDipoles(uint16_t xDipole_, uint16_t yDipole_, uint16_t zDipole_, + uint16_t currentTorqueDurationMs_) { + if (xDipole.value != xDipole_) { + } + xDipole = xDipole_; + if (yDipole.value != yDipole_) { + } + yDipole = yDipole_; + if (zDipole.value != zDipole_) { + } + zDipole = zDipole_; + 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); + 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); }; /** @@ -479,10 +534,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 +611,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 +688,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 +765,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 +842,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 +919,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/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_ */