diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 2bf36743..2631a628 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -683,8 +683,8 @@ void ObjectFactory::createReactionWheelComponents(LinuxLibgpioIF* gpioComIF, RwDefinitions::MAX_REPLY_SIZE, spi::RW_MODE, spi::RW_SPEED, &rwSpiCallback::spiCallback, nullptr); auto* rwHandler = new RwHandler(rwIds[idx], objects::SPI_RW_COM_IF, rwCookies[idx], gpioComIF, - rwGpioIds[idx]); - rwCookies[idx]->setCallbackArgs(rws[idx]); + rwGpioIds[idx], idx); + rwCookies[idx]->setCallbackArgs(rwHandler); #if OBSW_TEST_RW == 1 rws[idx]->setStartUpImmediately(); #endif diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp index e404cdf7..7e4cafeb 100644 --- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp +++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp @@ -633,80 +633,80 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg } if (cfg.scheduleRws) { - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ); } if (cfg.scheduleRws) { diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index b370b7d9..93e9dae8 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -183,22 +183,20 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; } 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; - } - result = dipoleSet.commit(); - if (result != returnvalue::OK) { - sif::error << "ImtqHandler::buildCommandFromCommand: commit failed" << std::endl; - } - } else { + { // Read set dipole values from local pool PoolReadGuard pg(&dipoleSet); + + // 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; + } + } } if (ACTUATION_WIRETAPPING) { sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.xDipole.value diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp index 5c6914be..9d4a7255 100644 --- a/mission/devices/RwHandler.cpp +++ b/mission/devices/RwHandler.cpp @@ -7,14 +7,15 @@ #include "OBSWConfig.h" RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, - GpioIF* gpioComIF, gpioId_t enableGpio) + GpioIF* gpioComIF, gpioId_t enableGpio, uint8_t rwIdx) : DeviceHandlerBase(objectId, comIF, comCookie), gpioComIF(gpioComIF), enableGpio(enableGpio), statusSet(this), lastResetStatusSet(this), tmDataset(this), - rwSpeedActuationSet(*this) { + rwSpeedActuationSet(*this), + rwIdx(rwIdx) { if (comCookie == nullptr) { sif::error << "RwHandler: Invalid com cookie" << std::endl; } @@ -43,6 +44,10 @@ void RwHandler::doShutDown() { ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { switch (internalState) { + case InternalState::SET_SPEED: + *id = RwDefinitions::SET_SPEED; + internalState = InternalState::GET_RESET_STATUS; + break; case InternalState::GET_RESET_STATUS: *id = RwDefinitions::GET_LAST_RESET_STATUS; internalState = InternalState::READ_TEMPERATURE; @@ -53,10 +58,6 @@ ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { break; case InternalState::GET_RW_SATUS: *id = RwDefinitions::GET_RW_STATUS; - internalState = InternalState::SET_SPEED; - break; - case InternalState::SET_SPEED: - *id = RwDefinitions::SET_SPEED; internalState = InternalState::CLEAR_RESET_STATUS; break; case InternalState::CLEAR_RESET_STATUS: @@ -107,29 +108,26 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand << " invalid length" << std::endl; return SET_SPEED_COMMAND_INVALID_LENGTH; } - // Commands override anything which was set in the software - if (commandData != nullptr) { - rwSpeedActuationSet.setValidityBufferGeneration(false); - result = rwSpeedActuationSet.deSerialize(&commandData, &commandDataLen, - SerializeIF::Endianness::NETWORK); - rwSpeedActuationSet.setValidityBufferGeneration(true); - if (result != returnvalue::OK) { - return result; - } - result = rwSpeedActuationSet.commit(); - if (result != returnvalue::OK) { - sif::error << "RwHandler::buildCommandFromCommand: commit failed" << std::endl; - } - } else { - // Read set rw speed value from local pool + + { PoolReadGuard pg(&rwSpeedActuationSet); + // Commands override anything which was set in the software + if (commandData != nullptr) { + rwSpeedActuationSet.setValidityBufferGeneration(false); + result = rwSpeedActuationSet.deSerialize(&commandData, &commandDataLen, + SerializeIF::Endianness::NETWORK); + rwSpeedActuationSet.setValidityBufferGeneration(true); + if (result != returnvalue::OK) { + return result; + } + } } if (ACTUATION_WIRETAPPING) { - int32_t speed; - uint16_t rampTime; + int32_t speed = 0; + uint16_t rampTime = 0; rwSpeedActuationSet.getRwSpeed(speed, rampTime); - sif::debug << "Actuating RW with speed = " << speed << " and rampTime = " << rampTime - << std::endl; + sif::debug << "Actuating RW " << static_cast(rwIdx) << " with speed = " << speed + << " and rampTime = " << rampTime << std::endl; } result = checkSpeedAndRampTime(); if (result != returnvalue::OK) { diff --git a/mission/devices/RwHandler.h b/mission/devices/RwHandler.h index 148a6359..ee18960d 100644 --- a/mission/devices/RwHandler.h +++ b/mission/devices/RwHandler.h @@ -9,7 +9,7 @@ #include "events/subsystemIdRanges.h" #include "returnvalues/classIds.h" -static constexpr bool ACTUATION_WIRETAPPING = true; +static constexpr bool ACTUATION_WIRETAPPING = false; class GpioIF; @@ -36,7 +36,7 @@ class RwHandler : public DeviceHandlerBase { * to high to enable the device. */ RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, GpioIF* gpioComIF, - gpioId_t enableGpio); + gpioId_t enableGpio, uint8_t rwIdx); void setDebugMode(bool enable); @@ -98,9 +98,10 @@ class RwHandler : public DeviceHandlerBase { RwDefinitions::RwSpeedActuationSet rwSpeedActuationSet; uint8_t commandBuffer[RwDefinitions::MAX_CMD_SIZE]; + uint8_t rwIdx; - PoolEntry rwSpeed = PoolEntry(0, false); - PoolEntry rampTime = PoolEntry(10, false); + PoolEntry rwSpeed = PoolEntry({0}); + PoolEntry rampTime = PoolEntry({10}); enum class InternalState { GET_RESET_STATUS,