From 75a4cd1b6977166bdfaaf684ad10917aea161cdc Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 20 Oct 2022 16:09:18 +0200 Subject: [PATCH] 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 {