From c8a2395d61e2e191997ad645779cd245b137d515 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 4 Mar 2023 14:32:18 +0100 Subject: [PATCH] a lot of bugfixes for IMTQ --- bsp_q7s/fs/SdCardManager.h | 2 +- linux/devices/ImtqPollingTask.cpp | 19 +- mission/controller/AcsController.cpp | 6 +- mission/devices/ImtqHandler.cpp | 140 ++++++++----- mission/devices/ImtqHandler.h | 19 +- .../devices/devicedefinitions/imtqHelpers.h | 196 ++++++++---------- 6 files changed, 205 insertions(+), 177 deletions(-) diff --git a/bsp_q7s/fs/SdCardManager.h b/bsp_q7s/fs/SdCardManager.h index 0a7b92c9..76a55ec6 100644 --- a/bsp_q7s/fs/SdCardManager.h +++ b/bsp_q7s/fs/SdCardManager.h @@ -225,7 +225,7 @@ class SdCardManager : public SystemObject, public SdCardMountedIF { bool markedUnusable = false; MutexIF* sdLock = nullptr; static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING; - static constexpr uint32_t LOCK_TIMEOUT = 40; + static constexpr uint32_t LOCK_TIMEOUT = 150; static constexpr char LOCK_CTX[] = "SdCardManager"; SdCardManager(); diff --git a/linux/devices/ImtqPollingTask.cpp b/linux/devices/ImtqPollingTask.cpp index 4d8790ee..0379737a 100644 --- a/linux/devices/ImtqPollingTask.cpp +++ b/linux/devices/ImtqPollingTask.cpp @@ -35,6 +35,9 @@ ReturnValue_t ImtqPollingTask::performOperation(uint8_t operationCode) { handleActuateStep(); break; } + default: { + break; + } }; } return returnvalue::OK; @@ -192,15 +195,15 @@ ReturnValue_t ImtqPollingTask::initializeInterface(CookieIF* cookie) { ReturnValue_t ImtqPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) { - ImtqRequest request(sendData, sendLen); + const auto* imtqReq = reinterpret_cast(sendData); { MutexGuard mg(ipcLock); - currentRequest = request.getRequestType(); - if (currentRequest == imtq::RequestType::ACTUATE) { - std::memcpy(dipoles, request.getDipoles(), 6); - torqueDuration = request.getTorqueDuration(); + if (imtqReq->request == imtq::RequestType::ACTUATE) { + std::memcpy(dipoles, imtqReq->dipoles, sizeof(dipoles)); + torqueDuration = imtqReq->torqueDuration; } - specialRequest = request.getSpecialRequest(); + currentRequest = imtqReq->request; + specialRequest = imtqReq->specialRequest; if (state != InternalState::IDLE) { return returnvalue::FAILED; } @@ -325,9 +328,11 @@ ReturnValue_t ImtqPollingTask::readReceivedMessage(CookieIF* cookie, uint8_t** b if (currentRequest == imtq::RequestType::MEASURE_NO_ACTUATION) { replyLen = getExchangeBufLen(specialRequest); memcpy(exchangeBuf.data(), replyBuf.data(), replyLen); - } else { + } else if (currentRequest == imtq::RequestType::ACTUATE) { replyLen = ImtqRepliesWithTorque::BASE_LEN; memcpy(exchangeBuf.data(), replyBufActuation.data(), replyLen); + } else { + *size = 0; } *buffer = exchangeBuf.data(); *size = replyLen; diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index eb07459f..d62bb13a 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -198,9 +198,9 @@ void AcsController::performSafe() { updateCtrlValData(errAng); updateActuatorCmdData(cmdDipolMtqs); - // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], - // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, - // acsParameters.rwHandlingParameters.rampTime); + //commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2] /*500, 500, 500 */, + // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, + // acsParameters.rwHandlingParameters.rampTime); } void AcsController::performDetumble() { diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index 068741fa..a7564444 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -53,6 +53,13 @@ ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comC ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) { uint8_t dhbOpCode = DeviceHandlerIF::PERFORM_OPERATION; + auto actuateStep = [&]() { + if (ignoreActForRestOfComSteps) { + requestStep = imtq::RequestType::DO_NOTHING; + } else { + requestStep = imtq::RequestType::ACTUATE; + } + }; switch (static_cast(opCode)) { case (imtq::ComStep::DHB_OP): { break; @@ -78,22 +85,38 @@ ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) { break; } case (imtq::ComStep::START_ACTUATE_SEND): { - requestStep = imtq::RequestType::ACTUATE; + if (manualTorqueCmdActive) { + if (manuallyCommandedTorqueDuration.isBusy()) { + ignoreActForRestOfComSteps = true; + requestStep = imtq::RequestType::DO_NOTHING; + } else { + manualTorqueCmdActive = false; + PoolReadGuard pg(&dipoleSet); + dipoleSet.dipoles[0] = 0; + dipoleSet.dipoles[1] = 0; + dipoleSet.dipoles[2] = 0; + dipoleSet.currentTorqueDurationMs = 0; + requestStep = imtq::RequestType::ACTUATE; + ignoreActForRestOfComSteps = false; + } + } else { + requestStep = imtq::RequestType::ACTUATE; + } dhbOpCode = DeviceHandlerIF::SEND_WRITE; break; } case (imtq::ComStep::START_ACTUATE_GET): { - requestStep = imtq::RequestType::ACTUATE; + actuateStep(); dhbOpCode = DeviceHandlerIF::GET_WRITE; break; } case (imtq::ComStep::READ_ACTUATE_SEND): { - requestStep = imtq::RequestType::ACTUATE; + actuateStep(); dhbOpCode = DeviceHandlerIF::SEND_READ; break; } case (imtq::ComStep::READ_ACTUATE_GET): { - requestStep = imtq::RequestType::ACTUATE; + actuateStep(); dhbOpCode = DeviceHandlerIF::GET_READ; break; } @@ -133,6 +156,9 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { *id = imtq::cmdIds::START_ACTUATION_DIPOLE; return buildCommandFromCommand(*id, nullptr, 0); } + default: { + return NOTHING_TO_SEND; + } } return NOTHING_TO_SEND; } @@ -145,11 +171,13 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma const uint8_t* commandData, size_t commandDataLen) { auto genericSpecialRequest = [&](imtq::SpecialRequest specialRequest) { - ImtqRequest request(commandBuffer, sizeof(commandBuffer)); - request.setMeasureRequest(specialRequest); + // ImtqRequest request(commandBuffer, sizeof(commandBuffer)); + request.request = imtq::RequestType::MEASURE_NO_ACTUATION; + request.specialRequest = specialRequest; + // request.setMeasureRequest(specialRequest); specialRequestActive = true; - rawPacket = commandBuffer; - rawPacketLen = ImtqRequest::REQUEST_LEN; + rawPacket = reinterpret_cast(&request); + rawPacketLen = sizeof(ImtqRequest); }; switch (deviceCommand) { case (imtq::cmdIds::POS_X_SELF_TEST): { @@ -181,10 +209,10 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma return returnvalue::OK; } case (imtq::cmdIds::REQUEST): { - ImtqRequest request(commandBuffer, sizeof(commandBuffer)); - request.setMeasureRequest(imtq::SpecialRequest::NONE); - rawPacket = commandBuffer; - rawPacketLen = ImtqRequest::REQUEST_LEN; + request.request = imtq::RequestType::MEASURE_NO_ACTUATION; + request.specialRequest = imtq::SpecialRequest::NONE; + rawPacket = reinterpret_cast(&request); + rawPacketLen = sizeof(ImtqRequest); return returnvalue::OK; } case (imtq::cmdIds::START_ACTUATION_DIPOLE): { @@ -193,7 +221,6 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma if (commandData != nullptr && commandDataLen < 8) { return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; } - ImtqRequest request(commandBuffer, sizeof(commandBuffer)); { PoolReadGuard pg(&dipoleSet); // Commands override anything which was set in the software @@ -205,22 +232,26 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma if (result != returnvalue::OK) { return result; } + manualTorqueCmdActive = true; + manuallyCommandedTorqueDuration.setTimeout(dipoleSet.currentTorqueDurationMs.value); } } - request.setActuateRequest(dipoleSet.xDipole.value, dipoleSet.yDipole.value, - dipoleSet.zDipole.value, dipoleSet.currentTorqueDurationMs.value); + request.request = imtq::RequestType::ACTUATE; + request.specialRequest = imtq::SpecialRequest::NONE; + std::memcpy(request.dipoles, dipoleSet.dipoles.value, sizeof(request.dipoles)); + request.torqueDuration = dipoleSet.currentTorqueDurationMs.value; if (ACTUATION_WIRETAPPING) { - sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.xDipole.value - << ", y = " << dipoleSet.yDipole.value << ", z = " << dipoleSet.zDipole.value + sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.dipoles[0] + << ", y = " << dipoleSet.dipoles[1] << ", z = " << dipoleSet.dipoles[2] << ", duration = " << dipoleSet.currentTorqueDurationMs.value << std::endl; } MutexGuard mg(torquer::lazyLock(), torquer::LOCK_TYPE, torquer::LOCK_TIMEOUT, torquer::LOCK_CTX); torquer::TORQUEING = true; torquer::TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value); - rawPacket = commandBuffer; - rawPacketLen = ImtqRequest::REQUEST_LEN; + rawPacket = reinterpret_cast(&request); + rawPacketLen = sizeof(ImtqRequest); return returnvalue::OK; } default: @@ -264,7 +295,6 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint } // arrayprinter::print(packet, ImtqReplies::BASE_LEN); if (requestStep == imtq::RequestType::MEASURE_NO_ACTUATION) { - requestStep = imtq::RequestType::ACTUATE; // sif::debug << "handle measure" << std::endl; ImtqRepliesDefault replies(packet); if (specialRequestActive) { @@ -329,9 +359,8 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint } else { status = result; } - } else { + } else if (requestStep == imtq::RequestType::ACTUATE) { // sif::debug << "handle measure with torque" << std::endl; - requestStep = imtq::RequestType::MEASURE_NO_ACTUATION; ImtqRepliesWithTorque replies(packet); if (replies.wasDipoleActuationRead()) { parseStatusByte(imtq::CC::START_ACTUATION_DIPOLE, replies.getDipoleActuation()); @@ -411,21 +440,26 @@ ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDat localDataPoolMap.emplace(imtq::STATUS_BYTE_CONF, &statusConfig); localDataPoolMap.emplace(imtq::STATUS_BYTE_ERROR, &statusError); localDataPoolMap.emplace(imtq::STATUS_BYTE_UPTIME, &statusUptime); + + // ENG HK No Torque localDataPoolMap.emplace(imtq::DIGITAL_VOLTAGE_MV, new PoolEntry({0})); localDataPoolMap.emplace(imtq::ANALOG_VOLTAGE_MV, new PoolEntry({0})); localDataPoolMap.emplace(imtq::DIGITAL_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(imtq::ANALOG_CURRENT, new PoolEntry({0})); - localDataPoolMap.emplace(imtq::COIL_X_CURRENT, new PoolEntry({0})); - localDataPoolMap.emplace(imtq::COIL_Y_CURRENT, new PoolEntry({0})); - localDataPoolMap.emplace(imtq::COIL_Z_CURRENT, new PoolEntry({0})); - localDataPoolMap.emplace(imtq::COIL_X_TEMPERATURE, new PoolEntry({0})); - localDataPoolMap.emplace(imtq::COIL_Y_TEMPERATURE, new PoolEntry({0})); - localDataPoolMap.emplace(imtq::COIL_Z_TEMPERATURE, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::COIL_CURRENTS, &coilCurrentsMilliampsNoTorque); + localDataPoolMap.emplace(imtq::COIL_TEMPERATURES, &coilTempsNoTorque); 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); + // ENG HK With Torque + localDataPoolMap.emplace(imtq::DIGITAL_VOLTAGE_MV_WT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::ANALOG_VOLTAGE_MV_WT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::DIGITAL_CURRENT_WT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::ANALOG_CURRENT_WT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::COIL_CURRENTS_WT, &coilCurrentsMilliampsWithTorque); + localDataPoolMap.emplace(imtq::COIL_TEMPERATURES_WT, &coilTempsWithTorque); + localDataPoolMap.emplace(imtq::MCU_TEMPERATURE_WT, new PoolEntry({0})); + + localDataPoolMap.emplace(imtq::DIPOLES_ID, &dipolesPoolEntry); localDataPoolMap.emplace(imtq::CURRENT_TORQUE_DURATION, &torqueDurationEntry); /** Entries of calibrated MTM measurement dataset */ @@ -433,8 +467,11 @@ ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDat localDataPoolMap.emplace(imtq::ACTUATION_CAL_STATUS, new PoolEntry({0})); /** Entries of raw MTM measurement dataset */ - localDataPoolMap.emplace(imtq::MTM_RAW, new PoolEntry(3)); - localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::MTM_RAW, &mtmRawNoTorque); + localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS, &actStatusNoTorque); + + localDataPoolMap.emplace(imtq::MTM_RAW_WT, &mtmRawWithTorque); + localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS_WT, &actStatusWithTorque); /** INIT measurements for positive X axis test */ localDataPoolMap.emplace(imtq::INIT_POS_X_ERR, new PoolEntry({0})); @@ -733,7 +770,7 @@ ReturnValue_t ImtqHandler::getSelfTestCommandId(DeviceCommandId_t* id) { } ReturnValue_t ImtqHandler::parseStatusByte(imtq::CC::CC command, const uint8_t* packet) { - uint8_t cmdErrorField = *(packet + 1) & 0xF; + uint8_t cmdErrorField = packet[1] & 0xF; if (cmdErrorField == 0) { return returnvalue::OK; } @@ -762,10 +799,12 @@ ReturnValue_t ImtqHandler::parseStatusByte(imtq::CC::CC command, const uint8_t* sif::error << "IMTQ::parseStatusByte: IMTQ replied internal processing error" << std::endl; return imtq::INTERNAL_PROCESSING_ERROR; default: - sif::error << "IMTQ::parseStatusByte: CMD Error field contains unknown error code 0x" - << static_cast(cmdErrorField) << std::endl; + sif::error << "IMTQ::parseStatusByte: CMD error field for command 0x" << std::setw(2) + << command << " contains unknown error code 0x" << static_cast(cmdErrorField) + << std::endl; return imtq::CMD_ERR_UNKNOWN; } + sif::error << std::dec; } void ImtqHandler::fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* packet) { @@ -779,20 +818,20 @@ void ImtqHandler::fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* pa offset += 2; hkDataset.analogCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; - hkDataset.coilXCurrentmA = + hkDataset.coilCurrentsMilliamps[0] = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; - hkDataset.coilYCurrentmA = + hkDataset.coilCurrentsMilliamps[1] = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; - hkDataset.coilZCurrentmA = + hkDataset.coilCurrentsMilliamps[2] = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; - hkDataset.coilXTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); + hkDataset.coilTemperatures[0] = (*(packet + offset + 1) << 8 | *(packet + offset)); offset += 2; - hkDataset.coilYTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); + hkDataset.coilTemperatures[1] = (*(packet + offset + 1) << 8 | *(packet + offset)); offset += 2; - hkDataset.coilZTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); + hkDataset.coilTemperatures[2] = (*(packet + offset + 1) << 8 | *(packet + offset)); offset += 2; size_t dummy = 2; SerializeAdapter::deSerialize(&hkDataset.mcuTemperature.value, packet + offset, &dummy, @@ -806,12 +845,15 @@ void ImtqHandler::fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* pa sif::info << "IMTQ analog voltage: " << hkDataset.analogVoltageMv << " mV" << std::endl; sif::info << "IMTQ digital current: " << hkDataset.digitalCurrentmA << " mA" << std::endl; sif::info << "IMTQ analog current: " << hkDataset.analogCurrentmA << " mA" << std::endl; - sif::info << "IMTQ coil X current: " << hkDataset.coilXCurrentmA << " mA" << std::endl; - sif::info << "IMTQ coil Y current: " << hkDataset.coilYCurrentmA << " mA" << std::endl; - sif::info << "IMTQ coil Z current: " << hkDataset.coilZCurrentmA << " mA" << std::endl; - sif::info << "IMTQ coil X temperature: " << hkDataset.coilXTemperature << " °C" << std::endl; - sif::info << "IMTQ coil Y temperature: " << hkDataset.coilYTemperature << " °C" << std::endl; - sif::info << "IMTQ coil Z temperature: " << hkDataset.coilZTemperature << " °C" << std::endl; + sif::info << "IMTQ coil X current: " << hkDataset.coilCurrentsMilliamps[0] << " mA" + << std::endl; + sif::info << "IMTQ coil Y current: " << hkDataset.coilCurrentsMilliamps[1] << " mA" + << std::endl; + sif::info << "IMTQ coil Z current: " << hkDataset.coilCurrentsMilliamps[2] << " mA" + << std::endl; + sif::info << "IMTQ coil X temperature: " << hkDataset.coilTemperatures[0] << " °C" << std::endl; + sif::info << "IMTQ coil Y temperature: " << hkDataset.coilTemperatures[1] << " °C" << std::endl; + sif::info << "IMTQ coil Z temperature: " << hkDataset.coilTemperatures[2] << " °C" << std::endl; sif::info << "IMTQ coil MCU temperature: " << hkDataset.mcuTemperature << " °C" << std::endl; #endif } @@ -852,7 +894,7 @@ void ImtqHandler::fillCalibratedMtmDataset(const uint8_t* packet) { void ImtqHandler::fillRawMtmDataset(imtq::RawMtmMeasurementSet& set, const uint8_t* packet) { PoolReadGuard rg(&set); if (rg.getReadResult() != returnvalue::OK) { - sif::error << "ImtqHandler::fillRawMtmDataset: Lock failure" << std::endl; + sif::error << "ImtqHandler::fillRawMtmDataset: Read failure" << std::endl; } unsigned int offset = 2; size_t deSerLen = 16; diff --git a/mission/devices/ImtqHandler.h b/mission/devices/ImtqHandler.h index 7bb16c42..1d9c03df 100644 --- a/mission/devices/ImtqHandler.h +++ b/mission/devices/ImtqHandler.h @@ -98,6 +98,9 @@ class ImtqHandler : public DeviceHandlerBase { imtq::NegYSelfTestSet negYselfTestDataset; imtq::PosZSelfTestSet posZselfTestDataset; imtq::NegZSelfTestSet negZselfTestDataset; + bool manualTorqueCmdActive = false; + bool ignoreActForRestOfComSteps = false; + Countdown manuallyCommandedTorqueDuration = Countdown(); NormalPollingMode pollingMode = NormalPollingMode::UNCALIBRATED; @@ -107,13 +110,21 @@ class ImtqHandler : public DeviceHandlerBase { PoolEntry statusUptime = PoolEntry({0}); PoolEntry mgmCalEntry = PoolEntry(3); - PoolEntry dipoleXEntry = PoolEntry({0}, false); - PoolEntry dipoleYEntry = PoolEntry({0}, false); - PoolEntry dipoleZEntry = PoolEntry({0}, false); + PoolEntry dipolesPoolEntry = PoolEntry({0, 0, 0}, false); PoolEntry torqueDurationEntry = PoolEntry({0}, false); + PoolEntry coilCurrentsMilliampsNoTorque = PoolEntry(3); + PoolEntry coilCurrentsMilliampsWithTorque = PoolEntry(3); + PoolEntry coilTempsNoTorque = PoolEntry(3); + PoolEntry coilTempsWithTorque = PoolEntry(3); + PoolEntry mtmRawNoTorque = PoolEntry(3); + PoolEntry actStatusNoTorque = PoolEntry(1); + PoolEntry mtmRawWithTorque = PoolEntry(3); + PoolEntry actStatusWithTorque = PoolEntry(1); + power::Switch_t switcher = power::NO_SWITCH; - uint8_t commandBuffer[imtq::MAX_COMMAND_SIZE]; + // uint8_t commandBuffer[imtq::MAX_COMMAND_SIZE]; + ImtqRequest request{}; bool goToNormalMode = false; bool debugMode = false; bool specialRequestActive = false; diff --git a/mission/devices/devicedefinitions/imtqHelpers.h b/mission/devices/devicedefinitions/imtqHelpers.h index 125b80c9..e961e636 100644 --- a/mission/devices/devicedefinitions/imtqHelpers.h +++ b/mission/devices/devicedefinitions/imtqHelpers.h @@ -22,7 +22,7 @@ enum ComStep : uint8_t { READ_ACTUATE_GET = 8, }; -enum class RequestType : uint8_t { MEASURE_NO_ACTUATION, ACTUATE }; +enum class RequestType : uint8_t { MEASURE_NO_ACTUATION, ACTUATE, DO_NOTHING }; enum class SpecialRequest : uint8_t { NONE = 0, @@ -195,20 +195,28 @@ enum PoolIds : lp_id_t { ANALOG_VOLTAGE_MV, DIGITAL_CURRENT, ANALOG_CURRENT, - COIL_X_CURRENT, - COIL_Y_CURRENT, - COIL_Z_CURRENT, - COIL_X_TEMPERATURE, - COIL_Y_TEMPERATURE, - COIL_Z_TEMPERATURE, + COIL_CURRENTS, + COIL_TEMPERATURES, MCU_TEMPERATURE, + + DIGITAL_VOLTAGE_MV_WT, + ANALOG_VOLTAGE_MV_WT, + DIGITAL_CURRENT_WT, + ANALOG_CURRENT_WT, + COIL_CURRENTS_WT, + COIL_TEMPERATURES_WT, + MCU_TEMPERATURE_WT, + MGM_CAL_NT, ACTUATION_CAL_STATUS, + MTM_RAW, ACTUATION_RAW_STATUS, - DIPOLES_X, - DIPOLES_Y, - DIPOLES_Z, + + MTM_RAW_WT, + ACTUATION_RAW_STATUS_WT, + + DIPOLES_ID, CURRENT_TORQUE_DURATION, INIT_POS_X_ERR, @@ -476,34 +484,56 @@ class StatusDataset : public StaticLocalDataSet<4> { class HkDataset : public StaticLocalDataSet { public: - HkDataset(HasLocalDataPoolIF* owner, uint32_t setId) : StaticLocalDataSet(owner, setId) {} + HkDataset(HasLocalDataPoolIF* owner, uint32_t setId, std::array pids) + : StaticLocalDataSet(owner, setId), + digitalVoltageMv(sid.objectId, pids[0], this), + analogVoltageMv(sid.objectId, pids[1], this), + digitalCurrentmA(sid.objectId, pids[2], this), + analogCurrentmA(sid.objectId, pids[3], this), + coilCurrentsMilliamps(sid.objectId, pids[4], this), + /** All temperatures in [C] for X, Y, Z */ + coilTemperatures(sid.objectId, pids[5], this), + mcuTemperature(sid.objectId, pids[6], this) {} - HkDataset(object_id_t objectId, uint32_t setId) : StaticLocalDataSet(sid_t(objectId, setId)) {} + HkDataset(object_id_t objectId, uint32_t setId, std::array pids) + : StaticLocalDataSet(sid_t(objectId, setId)), + digitalVoltageMv(sid.objectId, pids[0], this), + analogVoltageMv(sid.objectId, pids[1], this), + digitalCurrentmA(sid.objectId, pids[2], this), + analogCurrentmA(sid.objectId, pids[3], this), + coilCurrentsMilliamps(sid.objectId, pids[4], this), + /** All temperatures in [C] for X, Y, Z */ + coilTemperatures(sid.objectId, pids[5], this), + mcuTemperature(sid.objectId, pids[6], this) {} // Engineering HK variables - 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); - lp_var_t digitalCurrentmA = lp_var_t(sid.objectId, DIGITAL_CURRENT, this); - lp_var_t analogCurrentmA = lp_var_t(sid.objectId, ANALOG_CURRENT, this); - lp_var_t coilXCurrentmA = lp_var_t(sid.objectId, COIL_X_CURRENT, this); - lp_var_t coilYCurrentmA = lp_var_t(sid.objectId, COIL_Y_CURRENT, this); - lp_var_t coilZCurrentmA = lp_var_t(sid.objectId, COIL_Z_CURRENT, this); - /** All temperatures in [�C] */ - lp_var_t coilXTemperature = lp_var_t(sid.objectId, COIL_X_TEMPERATURE, this); - lp_var_t coilYTemperature = lp_var_t(sid.objectId, COIL_Y_TEMPERATURE, this); - lp_var_t coilZTemperature = lp_var_t(sid.objectId, COIL_Z_TEMPERATURE, this); - lp_var_t mcuTemperature = lp_var_t(sid.objectId, MCU_TEMPERATURE, this); + lp_var_t digitalVoltageMv; + lp_var_t analogVoltageMv; + lp_var_t digitalCurrentmA; + lp_var_t analogCurrentmA; + lp_vec_t coilCurrentsMilliamps; + /** All temperatures in [C] for X, Y, Z */ + lp_vec_t coilTemperatures; + lp_var_t mcuTemperature; + + private: }; class HkDatasetNoTorque : public HkDataset { public: - HkDatasetNoTorque(HasLocalDataPoolIF* owner) : HkDataset(owner, imtq::SetIds::ENG_HK_NO_TORQUE) {} + HkDatasetNoTorque(HasLocalDataPoolIF* owner) + : HkDataset(owner, imtq::SetIds::ENG_HK_NO_TORQUE, + {DIGITAL_VOLTAGE_MV, ANALOG_VOLTAGE_MV, DIGITAL_CURRENT, ANALOG_CURRENT, + COIL_CURRENTS, COIL_TEMPERATURES, MCU_TEMPERATURE}) {} }; class HkDatasetWithTorque : public HkDataset { public: HkDatasetWithTorque(HasLocalDataPoolIF* owner) - : HkDataset(owner, imtq::SetIds::ENG_HK_SET_WITH_TORQUE) {} + : HkDataset(owner, imtq::SetIds::ENG_HK_SET_WITH_TORQUE, + {DIGITAL_VOLTAGE_MV_WT, ANALOG_VOLTAGE_MV_WT, DIGITAL_CURRENT_WT, + ANALOG_CURRENT_WT, COIL_CURRENTS_WT, COIL_TEMPERATURES_WT, MCU_TEMPERATURE_WT}) { + } }; /** * @@ -529,32 +559,39 @@ class CalibratedMtmMeasurementSet : public StaticLocalDataSet { public: - RawMtmMeasurementSet(HasLocalDataPoolIF* owner, uint32_t setId) - : StaticLocalDataSet(owner, setId) {} + RawMtmMeasurementSet(object_id_t objectId, uint32_t setId, std::array pids) + : StaticLocalDataSet(sid_t(objectId, setId)), + mtmRawNt(sid.objectId, pids.at(0), this), + coilActuationStatus(sid.objectId, pids.at(1), this) {} - RawMtmMeasurementSet(object_id_t objectId, uint32_t setId) - : StaticLocalDataSet(sid_t(objectId, setId)) {} + RawMtmMeasurementSet(HasLocalDataPoolIF* owner, uint32_t setId, std::array pids) + : StaticLocalDataSet(owner, setId), + mtmRawNt(sid.objectId, pids.at(0), this), + coilActuationStatus(sid.objectId, pids.at(1), this) {} /** The unit of all measurements is nT */ - lp_vec_t mtmRawNt = lp_vec_t(sid.objectId, MTM_RAW, this); + lp_vec_t mtmRawNt; /** 1 if coils were actuating during measurement otherwise 0 */ - lp_var_t coilActuationStatus = - lp_var_t(sid.objectId, ACTUATION_RAW_STATUS, this); + lp_var_t coilActuationStatus; }; class RawMtmMeasurementNoTorque : public RawMtmMeasurementSet { public: RawMtmMeasurementNoTorque(HasLocalDataPoolIF* owner) - : RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_NO_TORQUE) {} + : RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_NO_TORQUE, + {PoolIds::MTM_RAW, PoolIds::ACTUATION_RAW_STATUS}) {} RawMtmMeasurementNoTorque(object_id_t objectId) - : RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_NO_TORQUE) {} + : RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_NO_TORQUE, + {PoolIds::MTM_RAW, PoolIds::ACTUATION_RAW_STATUS}) {} }; class RawMtmMeasurementWithTorque : public RawMtmMeasurementSet { public: RawMtmMeasurementWithTorque(HasLocalDataPoolIF* owner) - : RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_WITH_TORQUE) {} + : RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_WITH_TORQUE, + {PoolIds::MTM_RAW_WT, PoolIds::ACTUATION_RAW_STATUS_WT}) {} RawMtmMeasurementWithTorque(object_id_t objectId) - : RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_WITH_TORQUE) {} + : RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_WITH_TORQUE, + {PoolIds::MTM_RAW_WT, PoolIds::ACTUATION_RAW_STATUS_WT}) {} }; /** @@ -608,28 +645,16 @@ class DipoleActuationSet : public StaticLocalDataSet<4> { void setDipoles(int16_t xDipole_, int16_t yDipole_, int16_t zDipole_, uint16_t currentTorqueDurationMs_) { - if (xDipole.value != xDipole_) { - xDipole = xDipole_; - } - if (yDipole.value != yDipole_) { - yDipole = yDipole_; - } - if (zDipole.value != zDipole_) { - zDipole = zDipole_; - } + dipoles[0] = xDipole_; + dipoles[1] = yDipole_; + dipoles[2] = zDipole_; currentTorqueDurationMs = currentTorqueDurationMs_; } - void getDipoles(int16_t& xDipole_, int16_t& yDipole_, int16_t& zDipole_) { - xDipole_ = xDipole.value; - yDipole_ = yDipole.value; - zDipole_ = zDipole.value; - } + const int16_t* getDipoles() const { return dipoles.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_vec_t dipoles = lp_vec_t(sid.objectId, DIPOLES_ID, this); lp_var_t currentTorqueDurationMs = lp_var_t(sid.objectId, CURRENT_TORQUE_DURATION, this); }; @@ -1099,65 +1124,10 @@ class NegZSelfTestSet : public StaticLocalDataSet { } // namespace imtq struct ImtqRequest { - friend class ImtqHandler; - - public: - static constexpr size_t REQUEST_LEN = 10; - - ImtqRequest(const uint8_t* data, size_t maxSize) - : rawData(const_cast(data)), maxSize(maxSize) {} - - imtq::RequestType getRequestType() const { return static_cast(rawData[0]); } - - void setMeasureRequest(imtq::SpecialRequest specialRequest) { - rawData[0] = static_cast(imtq::RequestType::MEASURE_NO_ACTUATION); - rawData[1] = static_cast(specialRequest); - } - - void setActuateRequest(int16_t dipoleX, int16_t dipoleY, int16_t dipoleZ, - uint16_t torqueDuration) { - size_t dummy = 0; - rawData[0] = static_cast(imtq::RequestType::ACTUATE); - rawData[1] = static_cast(imtq::SpecialRequest::NONE); - uint8_t* serPtr = rawData + 2; - SerializeAdapter::serialize(&dipoleX, &serPtr, &dummy, maxSize, - SerializeIF::Endianness::MACHINE); - SerializeAdapter::serialize(&dipoleY, &serPtr, &dummy, maxSize, - SerializeIF::Endianness::MACHINE); - SerializeAdapter::serialize(&dipoleZ, &serPtr, &dummy, maxSize, - SerializeIF::Endianness::MACHINE); - SerializeAdapter::serialize(&torqueDuration, &serPtr, &dummy, maxSize, - SerializeIF::Endianness::MACHINE); - } - - uint8_t* startOfActuateDataPtr() { return rawData + 2; } - - int16_t* getDipoles() { return reinterpret_cast(rawData + 2); } - - uint16_t getTorqueDuration() { - uint8_t* data = rawData + 2 + 6; - uint16_t value = 0; - size_t dummy = 0; - SerializeAdapter::deSerialize(&value, data, &dummy, SerializeIF::Endianness::MACHINE); - return value; - } - - void setSpecialRequest(imtq::SpecialRequest specialRequest) { - rawData[1] = static_cast(specialRequest); - } - - imtq::SpecialRequest getSpecialRequest() const { - return static_cast(rawData[1]); - } - - private: - ImtqRequest(uint8_t* rawData, size_t maxLen) : rawData(rawData) { - if (rawData != nullptr) { - rawData[0] = static_cast(imtq::RequestType::MEASURE_NO_ACTUATION); - } - } - uint8_t* rawData; - size_t maxSize = 0; + imtq::RequestType request = imtq::RequestType::MEASURE_NO_ACTUATION; + imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE; + int16_t dipoles[3]{}; + uint16_t torqueDuration = 0; }; struct ImtqRepliesDefault {