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; }; /**