From f0416553788cf9264b5c63e7d6d330557ffcaaba Mon Sep 17 00:00:00 2001 From: meggert Date: Sat, 4 Mar 2023 10:15:51 +0100 Subject: [PATCH 1/8] use dipoleSet or overwrite it in case of external command --- mission/devices/ImtqHandler.cpp | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index c9ceec04..068741fa 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -194,17 +194,18 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; } ImtqRequest request(commandBuffer, sizeof(commandBuffer)); - // Commands override anything which was set in the software - if (commandData != nullptr) { - // Read set dipole values from local pool + { PoolReadGuard pg(&dipoleSet); - - int16_t xDipole = 0, yDipole = 0, zDipole = 0; - uint16_t torqueDuration = 0; - dipoleSet.xDipole = xDipole; - dipoleSet.yDipole = yDipole; - dipoleSet.zDipole = zDipole; - dipoleSet.currentTorqueDurationMs = torqueDuration; + // Commands override anything which was set in the software + if (commandData != nullptr) { + dipoleSet.setValidityBufferGeneration(false); + ReturnValue_t result = dipoleSet.deSerialize(&commandData, &commandDataLen, + SerializeIF::Endianness::NETWORK); + dipoleSet.setValidityBufferGeneration(true); + if (result != returnvalue::OK) { + return result; + } + } } request.setActuateRequest(dipoleSet.xDipole.value, dipoleSet.yDipole.value, From 773e1c9ecca4828ba6cd0117be0b93abcbec8252 Mon Sep 17 00:00:00 2001 From: meggert Date: Sat, 4 Mar 2023 10:33:47 +0100 Subject: [PATCH 2/8] fixed UB on startup --- mission/devices/ImtqHandler.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/mission/devices/ImtqHandler.h b/mission/devices/ImtqHandler.h index 419fad33..7bb16c42 100644 --- a/mission/devices/ImtqHandler.h +++ b/mission/devices/ImtqHandler.h @@ -107,10 +107,10 @@ 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 torqueDurationEntry = PoolEntry(0, false); + PoolEntry dipoleXEntry = PoolEntry({0}, false); + PoolEntry dipoleYEntry = PoolEntry({0}, false); + PoolEntry dipoleZEntry = PoolEntry({0}, false); + PoolEntry torqueDurationEntry = PoolEntry({0}, false); power::Switch_t switcher = power::NO_SWITCH; uint8_t commandBuffer[imtq::MAX_COMMAND_SIZE]; From 300f3230d18440184ef1767cdcf2eb801182794d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 4 Mar 2023 12:00:38 +0100 Subject: [PATCH 3/8] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 33de1520..2c5af91d 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 33de15205b4560c54a108e35609536374b026c22 +Subproject commit 2c5af91db170ffd19f5cc6726642593fa9e3a059 From f6f3f17505dae5510e9a1c5090bf28955efbb3e2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 4 Mar 2023 12:01:15 +0100 Subject: [PATCH 4/8] changelog --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 20ad03f1..f5dcd6ef 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,10 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Fixed + +- Bumped FSFW: `Countdown` and `Stopwatch` use new monotonic clock API now. + # [v1.34.0] 2023-03-03 eive-tmtc: v2.16.3 From c8a2395d61e2e191997ad645779cd245b137d515 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 4 Mar 2023 14:32:18 +0100 Subject: [PATCH 5/8] 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 { From 4c1b79fd66f44cc74afe4bfd91c50c83b9c7e71f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 4 Mar 2023 17:06:23 +0100 Subject: [PATCH 6/8] that was an evil bug --- common/config/eive/definitions.h | 2 +- linux/devices/ImtqPollingTask.cpp | 34 ++++++++++++++- linux/devices/ImtqPollingTask.h | 2 + mission/controller/AcsController.cpp | 2 +- mission/devices/ImtqHandler.cpp | 43 +++++++++++++------ mission/devices/ImtqHandler.h | 2 +- .../devices/devicedefinitions/imtqHelpers.h | 3 +- tmtc | 2 +- 8 files changed, 70 insertions(+), 20 deletions(-) diff --git a/common/config/eive/definitions.h b/common/config/eive/definitions.h index 99ce4437..e3a05a56 100644 --- a/common/config/eive/definitions.h +++ b/common/config/eive/definitions.h @@ -62,7 +62,7 @@ static constexpr uint32_t SCHED_BLOCK_2_SENSOR_READ_MS = 30; static constexpr uint32_t SCHED_BLOCK_3_READ_IMTQ_MGM_MS = 42; static constexpr uint32_t SCHED_BLOCK_4_ACS_CTRL_MS = 45; static constexpr uint32_t SCHED_BLOCK_5_ACTUATOR_MS = 55; -static constexpr uint32_t SCHED_BLOCK_6_IMTQ_BLOCK_2_MS = 95; +static constexpr uint32_t SCHED_BLOCK_6_IMTQ_BLOCK_2_MS = 105; static constexpr uint32_t SCHED_BLOCK_RTD = 150; static constexpr uint32_t SCHED_BLOCK_7_RW_READ_MS = 300; static constexpr uint32_t SCHED_BLOCK_8_PLPCDU_MS = 320; diff --git a/linux/devices/ImtqPollingTask.cpp b/linux/devices/ImtqPollingTask.cpp index 0379737a..51618981 100644 --- a/linux/devices/ImtqPollingTask.cpp +++ b/linux/devices/ImtqPollingTask.cpp @@ -121,12 +121,24 @@ void ImtqPollingTask::handleMeasureStep() { } // Takes a bit of time to take measurements. Subtract a bit because of the delay of previous // commands. - TaskFactory::delayTask(currentIntegrationTimeMs); + TaskFactory::delayTask(currentIntegrationTimeMs + MGM_READ_BUFFER_TIME_MS); cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT; if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) { return; } + // See p.39 of the iMTQ user manual. If the NEW bit of the STAT bitfield is not set, we probably + // have old data. Which can be really bad for ACS. And everything. + if ((replyPtr[2] >> 7) == 0) { + sif::error << "IMTQ: MGM measurement too old" << std::endl; + TaskFactory::delayTask(2); + if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) { + return; + } + if ((replyPtr[2] >> 7) == 0b1) { + replyPtr[0] = false; + } + } cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA; if (i2cCmdExecMeasure(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) { @@ -160,18 +172,34 @@ void ImtqPollingTask::handleActuateStep() { return; } + TaskFactory::delayTask(10); + cmdLen = 1; cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT; if (i2cCmdExecActuate(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) { return; } - TaskFactory::delayTask(currentIntegrationTimeMs); + TaskFactory::delayTask(currentIntegrationTimeMs + MGM_READ_BUFFER_TIME_MS); cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT; if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) { return; } + // See p.39 of the iMTQ user manual. If the NEW bit of the STAT bitfield is not set, we probably + // have old data. Which can be really bad for ACS. And everything. + if ((replyPtr[2] >> 7) == 0) { + sif::error << "IMTQ: MGM measurement too old" << std::endl; + TaskFactory::delayTask(2); + cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT; + if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) { + return; + } + if ((replyPtr[2] >> 7) == 0b1) { + replyPtr[0] = false; + } + } + cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA; if (i2cCmdExecActuate(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) { return; @@ -312,6 +340,8 @@ void ImtqPollingTask::buildDipoleCommand() { } SerializeAdapter::serialize(&torqueDuration, &serPtr, &serLen, cmdBuf.size(), SerializeIF::Endianness::LITTLE); + // sif::debug << "Dipole X: " << dipoles[0] << std::endl; + // sif::debug << "Torqeu Dur: " << torqueDuration << std::endl; cmdLen = 1 + serLen; } diff --git a/linux/devices/ImtqPollingTask.h b/linux/devices/ImtqPollingTask.h index 00a05fc6..5ae8f1f9 100644 --- a/linux/devices/ImtqPollingTask.h +++ b/linux/devices/ImtqPollingTask.h @@ -32,6 +32,8 @@ class ImtqPollingTask : public SystemObject, const char* i2cDev = nullptr; address_t i2cAddr = 0; uint32_t currentIntegrationTimeMs = 10; + // Required in addition to integration time, otherwise old data might be read. + static constexpr uint32_t MGM_READ_BUFFER_TIME_MS = 3; bool ignoreNextActuateRequest = false; imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE; diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index d62bb13a..38a7424b 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -198,7 +198,7 @@ void AcsController::performSafe() { updateCtrlValData(errAng); updateActuatorCmdData(cmdDipolMtqs); - //commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2] /*500, 500, 500 */, + //commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2]/*500, 500, 500*/, // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, // acsParameters.rwHandlingParameters.rampTime); } diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index a7564444..b76f0575 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -131,7 +131,8 @@ ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) { ImtqHandler::~ImtqHandler() = default; void ImtqHandler::doStartUp() { - updatePeriodicReply(true, imtq::cmdIds::REPLY); + updatePeriodicReply(true, imtq::cmdIds::REPLY_NO_TORQUE); + updatePeriodicReply(true, imtq::cmdIds::REPLY_WITH_TORQUE); if (goToNormalMode) { setMode(MODE_NORMAL); } else { @@ -140,7 +141,8 @@ void ImtqHandler::doStartUp() { } void ImtqHandler::doShutDown() { - updatePeriodicReply(false, imtq::cmdIds::REPLY); + updatePeriodicReply(false, imtq::cmdIds::REPLY_NO_TORQUE); + updatePeriodicReply(false, imtq::cmdIds::REPLY_WITH_TORQUE); specialRequestActive = false; firstReplyCycle = true; setMode(_MODE_POWER_DOWN); @@ -157,7 +159,12 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { return buildCommandFromCommand(*id, nullptr, 0); } default: { - return NOTHING_TO_SEND; + *id = imtq::cmdIds::REQUEST; + request.request = imtq::RequestType::DO_NOTHING; + request.specialRequest = imtq::SpecialRequest::NONE; + rawPacket = reinterpret_cast(&request); + rawPacketLen = sizeof(ImtqRequest); + return returnvalue::OK; } } return NOTHING_TO_SEND; @@ -171,10 +178,9 @@ 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.request = imtq::RequestType::MEASURE_NO_ACTUATION; request.specialRequest = specialRequest; - // request.setMeasureRequest(specialRequest); + expectedReply = imtq::cmdIds::REPLY_NO_TORQUE; specialRequestActive = true; rawPacket = reinterpret_cast(&request); rawPacketLen = sizeof(ImtqRequest); @@ -211,17 +217,17 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma case (imtq::cmdIds::REQUEST): { request.request = imtq::RequestType::MEASURE_NO_ACTUATION; request.specialRequest = imtq::SpecialRequest::NONE; + expectedReply = imtq::cmdIds::REPLY_NO_TORQUE; rawPacket = reinterpret_cast(&request); rawPacketLen = sizeof(ImtqRequest); return returnvalue::OK; } case (imtq::cmdIds::START_ACTUATION_DIPOLE): { - /* IMTQ expects low byte first */ - // commandBuffer[0] = imtq::CC::START_ACTUATION_DIPOLE; if (commandData != nullptr && commandDataLen < 8) { return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; } { + // Do this in any case to read values which might be commanded by the ACS controller. PoolReadGuard pg(&dipoleSet); // Commands override anything which was set in the software if (commandData != nullptr) { @@ -237,6 +243,7 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma } } + expectedReply = imtq::cmdIds::REPLY_WITH_TORQUE; request.request = imtq::RequestType::ACTUATE; request.specialRequest = imtq::SpecialRequest::NONE; std::memcpy(request.dipoles, dipoleSet.dipoles.value, sizeof(request.dipoles)); @@ -263,7 +270,8 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma void ImtqHandler::fillCommandAndReplyMap() { insertInCommandMap(imtq::cmdIds::REQUEST); insertInCommandMap(imtq::cmdIds::START_ACTUATION_DIPOLE); - insertInReplyMap(imtq::cmdIds::REPLY, 5, nullptr, 0, true); + insertInReplyMap(imtq::cmdIds::REPLY_NO_TORQUE, 5, nullptr, 0, true); + insertInReplyMap(imtq::cmdIds::REPLY_WITH_TORQUE, 20, nullptr, 0, true); insertInCommandMap(imtq::cmdIds::POS_X_SELF_TEST); insertInCommandMap(imtq::cmdIds::NEG_X_SELF_TEST); insertInCommandMap(imtq::cmdIds::POS_Y_SELF_TEST); @@ -280,7 +288,7 @@ ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSi } if (remainingSize > 0) { *foundLen = remainingSize; - *foundId = imtq::cmdIds::REPLY; + *foundId = expectedReply; return returnvalue::OK; } return returnvalue::FAILED; @@ -294,7 +302,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint return returnvalue::OK; } // arrayprinter::print(packet, ImtqReplies::BASE_LEN); - if (requestStep == imtq::RequestType::MEASURE_NO_ACTUATION) { + if (expectedReply == imtq::cmdIds::REPLY_NO_TORQUE) { // sif::debug << "handle measure" << std::endl; ImtqRepliesDefault replies(packet); if (specialRequestActive) { @@ -359,7 +367,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint } else { status = result; } - } else if (requestStep == imtq::RequestType::ACTUATE) { + } else if (expectedReply == imtq::cmdIds::REPLY_WITH_TORQUE) { // sif::debug << "handle measure with torque" << std::endl; ImtqRepliesWithTorque replies(packet); if (replies.wasDipoleActuationRead()) { @@ -405,6 +413,8 @@ LocalPoolDataSetBase* ImtqHandler::getDataSetHandle(sid_t sid) { return &hkDatasetNoTorque; } else if (sid == dipoleSet.getSid()) { return &dipoleSet; + } else if (sid == statusSet.getSid()) { + return &statusSet; } else if (sid == hkDatasetWithTorque.getSid()) { return &hkDatasetWithTorque; } else if (sid == rawMtmWithTorque.getSid()) { @@ -747,6 +757,10 @@ ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDat subdp::DiagnosticsHkPeriodicParams(rawMtmWithTorque.getSid(), false, 10.0)); poolManager.subscribeForDiagPeriodicPacket( subdp::DiagnosticsHkPeriodicParams(calMtmMeasurementSet.getSid(), false, 10.0)); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(statusSet.getSid(), false, 10.0)); + poolManager.subscribeForDiagPeriodicPacket( + subdp::DiagnosticsHkPeriodicParams(dipoleSet.getSid(), false, 10.0)); return DeviceHandlerBase::initializeLocalDataPool(localDataPoolMap, poolManager); } @@ -793,10 +807,12 @@ ReturnValue_t ImtqHandler::parseStatusByte(imtq::CC::CC command, const uint8_t* << " has invalid parameter" << std::endl; return imtq::PARAMETER_INVALID; case 5: - sif::error << "IMTQ::parseStatusByte: CC 0x" << std::setw(2) << " unavailable" << std::endl; + sif::error << "IMTQ::parseStatusByte: CC 0x" << std::setw(2) << command << " unavailable" + << std::endl; return imtq::CC_UNAVAILABLE; case 7: - sif::error << "IMTQ::parseStatusByte: IMTQ replied internal processing error" << std::endl; + sif::error << "IMTQ::parseStatusByte: Internal processing error for command 0x" + << std::setw(2) << command << std::endl; return imtq::INTERNAL_PROCESSING_ERROR; default: sif::error << "IMTQ::parseStatusByte: CMD error field for command 0x" << std::setw(2) @@ -930,6 +946,7 @@ void ImtqHandler::fillRawMtmDataset(imtq::RawMtmMeasurementSet& set, const uint8 set.setValidity(true, true); if (debugMode) { #if OBSW_VERBOSE_LEVEL >= 1 + sif::info << "Set ID: " << set.getSid().ownerSetId << std::endl; sif::info << "IMTQ raw MTM measurement X: " << set.mtmRawNt[0] << " nT" << std::endl; sif::info << "IMTQ raw MTM measurement Y: " << set.mtmRawNt[1] << " nT" << std::endl; sif::info << "IMTQ raw MTM measurement Z: " << set.mtmRawNt[2] << " nT" << std::endl; diff --git a/mission/devices/ImtqHandler.h b/mission/devices/ImtqHandler.h index 1d9c03df..d579f7f3 100644 --- a/mission/devices/ImtqHandler.h +++ b/mission/devices/ImtqHandler.h @@ -123,8 +123,8 @@ class ImtqHandler : public DeviceHandlerBase { power::Switch_t switcher = power::NO_SWITCH; - // uint8_t commandBuffer[imtq::MAX_COMMAND_SIZE]; ImtqRequest request{}; + DeviceCommandId_t expectedReply = imtq::cmdIds::REPLY_WITH_TORQUE; bool goToNormalMode = false; bool debugMode = false; bool specialRequestActive = false; diff --git a/mission/devices/devicedefinitions/imtqHelpers.h b/mission/devices/devicedefinitions/imtqHelpers.h index e961e636..a066216d 100644 --- a/mission/devices/devicedefinitions/imtqHelpers.h +++ b/mission/devices/devicedefinitions/imtqHelpers.h @@ -53,7 +53,8 @@ static const ReturnValue_t UNEXPECTED_SELF_TEST_REPLY = MAKE_RETURN_CODE(0xA7); namespace cmdIds { static constexpr DeviceCommandId_t REQUEST = 0x70; -static constexpr DeviceCommandId_t REPLY = 0x71; +static constexpr DeviceCommandId_t REPLY_NO_TORQUE = 0x71; +static constexpr DeviceCommandId_t REPLY_WITH_TORQUE = 0x72; static const DeviceCommandId_t START_ACTUATION_DIPOLE = 0x2; static const DeviceCommandId_t POS_X_SELF_TEST = 0x7; static const DeviceCommandId_t NEG_X_SELF_TEST = 0x8; diff --git a/tmtc b/tmtc index 94ae2d16..09c694cf 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 94ae2d16e21ade8f89723b2e62356967a67b171d +Subproject commit 09c694cf9c72762114de760f728a76051b3745d9 From 80115640bac3f48f5c3098091611d9d44adb6b2e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 4 Mar 2023 17:09:24 +0100 Subject: [PATCH 7/8] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index f5dcd6ef..8620f30f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -19,6 +19,8 @@ will consitute of a breaking change warranting a new major release: ## Fixed - Bumped FSFW: `Countdown` and `Stopwatch` use new monotonic clock API now. +- IMTQ: Various fixes, most notably missing buffer time after starting MGM measurement + and corrections for actuator commanding. # [v1.34.0] 2023-03-03 From f96fbf707deda1b7d5272d6f43928d603e2ab2a8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 4 Mar 2023 17:09:51 +0100 Subject: [PATCH 8/8] PR in changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8620f30f..05f621eb 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -21,6 +21,7 @@ will consitute of a breaking change warranting a new major release: - Bumped FSFW: `Countdown` and `Stopwatch` use new monotonic clock API now. - IMTQ: Various fixes, most notably missing buffer time after starting MGM measurement and corrections for actuator commanding. + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/430 # [v1.34.0] 2023-03-03