eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

IMTQ Commanding Fix #430

Merged
muellerr merged 9 commits from imtq-cmd-fix into develop 2023-03-04 17:07:07 +01:00
Conversation 0 Commits 9 Files Changed 11 +289 -203
11 changed files with 289 additions and 203 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
CHANGELOG.md
View File

@ -16,6 +16,13 @@ will consitute of a breaking change warranting a new major release:
# [unreleased] # [unreleased]
## 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.
PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/430
# [v1.34.0] 2023-03-03 # [v1.34.0] 2023-03-03
eive-tmtc: v2.16.3 eive-tmtc: v2.16.3

2
bsp_q7s/fs/SdCardManager.h
View File

@ -225,7 +225,7 @@ class SdCardManager : public SystemObject, public SdCardMountedIF {
bool markedUnusable = false; bool markedUnusable = false;
MutexIF* sdLock = nullptr; MutexIF* sdLock = nullptr;
static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING; 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"; static constexpr char LOCK_CTX[] = "SdCardManager";
SdCardManager(); SdCardManager();

2
common/config/eive/definitions.h
View File

@ -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_3_READ_IMTQ_MGM_MS = 42;
static constexpr uint32_t SCHED_BLOCK_4_ACS_CTRL_MS = 45; 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_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_RTD = 150;
static constexpr uint32_t SCHED_BLOCK_7_RW_READ_MS = 300; static constexpr uint32_t SCHED_BLOCK_7_RW_READ_MS = 300;
static constexpr uint32_t SCHED_BLOCK_8_PLPCDU_MS = 320; static constexpr uint32_t SCHED_BLOCK_8_PLPCDU_MS = 320;

2
fsfw

@ -1 +1 @@
Subproject commit 33de15205b4560c54a108e35609536374b026c22 Subproject commit 2c5af91db170ffd19f5cc6726642593fa9e3a059

53
linux/devices/ImtqPollingTask.cpp
View File

@ -35,6 +35,9 @@ ReturnValue_t ImtqPollingTask::performOperation(uint8_t operationCode) {
handleActuateStep(); handleActuateStep();
break; break;
} }
default: {
break;
}
}; };
} }
return returnvalue::OK; return returnvalue::OK;
@ -118,12 +121,24 @@ void ImtqPollingTask::handleMeasureStep() {
} }
// Takes a bit of time to take measurements. Subtract a bit because of the delay of previous // Takes a bit of time to take measurements. Subtract a bit because of the delay of previous
// commands. // commands.
TaskFactory::delayTask(currentIntegrationTimeMs); TaskFactory::delayTask(currentIntegrationTimeMs + MGM_READ_BUFFER_TIME_MS);
cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT; cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) { if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
return; 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; cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA;
if (i2cCmdExecMeasure(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) { if (i2cCmdExecMeasure(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) {
@ -157,18 +172,34 @@ void ImtqPollingTask::handleActuateStep() {
return; return;
} }
TaskFactory::delayTask(10);
cmdLen = 1; cmdLen = 1;
cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT; cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT;
if (i2cCmdExecActuate(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) { if (i2cCmdExecActuate(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) {
return; return;
} }
TaskFactory::delayTask(currentIntegrationTimeMs); TaskFactory::delayTask(currentIntegrationTimeMs + MGM_READ_BUFFER_TIME_MS);
cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT; cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) { if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
return; 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; cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA;
if (i2cCmdExecActuate(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) { if (i2cCmdExecActuate(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) {
return; return;
@ -192,15 +223,15 @@ ReturnValue_t ImtqPollingTask::initializeInterface(CookieIF* cookie) {
ReturnValue_t ImtqPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendData, ReturnValue_t ImtqPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendData,
size_t sendLen) { size_t sendLen) {
ImtqRequest request(sendData, sendLen); const auto* imtqReq = reinterpret_cast<const ImtqRequest*>(sendData);
{ {
MutexGuard mg(ipcLock); MutexGuard mg(ipcLock);
currentRequest = request.getRequestType(); if (imtqReq->request == imtq::RequestType::ACTUATE) {
if (currentRequest == imtq::RequestType::ACTUATE) { std::memcpy(dipoles, imtqReq->dipoles, sizeof(dipoles));
std::memcpy(dipoles, request.getDipoles(), 6); torqueDuration = imtqReq->torqueDuration;
torqueDuration = request.getTorqueDuration();
} }
specialRequest = request.getSpecialRequest(); currentRequest = imtqReq->request;
specialRequest = imtqReq->specialRequest;
if (state != InternalState::IDLE) { if (state != InternalState::IDLE) {
return returnvalue::FAILED; return returnvalue::FAILED;
} }
@ -309,6 +340,8 @@ void ImtqPollingTask::buildDipoleCommand() {
} }
SerializeAdapter::serialize(&torqueDuration, &serPtr, &serLen, cmdBuf.size(), SerializeAdapter::serialize(&torqueDuration, &serPtr, &serLen, cmdBuf.size(),
SerializeIF::Endianness::LITTLE); SerializeIF::Endianness::LITTLE);
// sif::debug << "Dipole X: " << dipoles[0] << std::endl;
// sif::debug << "Torqeu Dur: " << torqueDuration << std::endl;
cmdLen = 1 + serLen; cmdLen = 1 + serLen;
} }
@ -325,9 +358,11 @@ ReturnValue_t ImtqPollingTask::readReceivedMessage(CookieIF* cookie, uint8_t** b
if (currentRequest == imtq::RequestType::MEASURE_NO_ACTUATION) { if (currentRequest == imtq::RequestType::MEASURE_NO_ACTUATION) {
replyLen = getExchangeBufLen(specialRequest); replyLen = getExchangeBufLen(specialRequest);
memcpy(exchangeBuf.data(), replyBuf.data(), replyLen); memcpy(exchangeBuf.data(), replyBuf.data(), replyLen);
} else { } else if (currentRequest == imtq::RequestType::ACTUATE) {
replyLen = ImtqRepliesWithTorque::BASE_LEN; replyLen = ImtqRepliesWithTorque::BASE_LEN;
memcpy(exchangeBuf.data(), replyBufActuation.data(), replyLen); memcpy(exchangeBuf.data(), replyBufActuation.data(), replyLen);
} else {
*size = 0;
} }
*buffer = exchangeBuf.data(); *buffer = exchangeBuf.data();
*size = replyLen; *size = replyLen;

2
linux/devices/ImtqPollingTask.h
View File

@ -32,6 +32,8 @@ class ImtqPollingTask : public SystemObject,
const char* i2cDev = nullptr; const char* i2cDev = nullptr;
address_t i2cAddr = 0; address_t i2cAddr = 0;
uint32_t currentIntegrationTimeMs = 10; 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; bool ignoreNextActuateRequest = false;
imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE; imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE;

6
mission/controller/AcsController.cpp
View File

@ -198,9 +198,9 @@ void AcsController::performSafe() {
updateCtrlValData(errAng); updateCtrlValData(errAng);
updateActuatorCmdData(cmdDipolMtqs); updateActuatorCmdData(cmdDipolMtqs);
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], //commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2]/*500, 500, 500*/,
// acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
// acsParameters.rwHandlingParameters.rampTime); // acsParameters.rwHandlingParameters.rampTime);
} }
void AcsController::performDetumble() { void AcsController::performDetumble() {

196
mission/devices/ImtqHandler.cpp
View File

@ -53,6 +53,13 @@ ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comC
ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) { ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) {
uint8_t dhbOpCode = DeviceHandlerIF::PERFORM_OPERATION; uint8_t dhbOpCode = DeviceHandlerIF::PERFORM_OPERATION;
auto actuateStep = [&]() {
if (ignoreActForRestOfComSteps) {
requestStep = imtq::RequestType::DO_NOTHING;
} else {
requestStep = imtq::RequestType::ACTUATE;
}
};
switch (static_cast<imtq::ComStep>(opCode)) { switch (static_cast<imtq::ComStep>(opCode)) {
case (imtq::ComStep::DHB_OP): { case (imtq::ComStep::DHB_OP): {
break; break;
@ -78,22 +85,38 @@ ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) {
break; break;
} }
case (imtq::ComStep::START_ACTUATE_SEND): { 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; dhbOpCode = DeviceHandlerIF::SEND_WRITE;
break; break;
} }
case (imtq::ComStep::START_ACTUATE_GET): { case (imtq::ComStep::START_ACTUATE_GET): {
requestStep = imtq::RequestType::ACTUATE; actuateStep();
dhbOpCode = DeviceHandlerIF::GET_WRITE; dhbOpCode = DeviceHandlerIF::GET_WRITE;
break; break;
} }
case (imtq::ComStep::READ_ACTUATE_SEND): { case (imtq::ComStep::READ_ACTUATE_SEND): {
requestStep = imtq::RequestType::ACTUATE; actuateStep();
dhbOpCode = DeviceHandlerIF::SEND_READ; dhbOpCode = DeviceHandlerIF::SEND_READ;
break; break;
} }
case (imtq::ComStep::READ_ACTUATE_GET): { case (imtq::ComStep::READ_ACTUATE_GET): {
requestStep = imtq::RequestType::ACTUATE; actuateStep();
dhbOpCode = DeviceHandlerIF::GET_READ; dhbOpCode = DeviceHandlerIF::GET_READ;
break; break;
} }
@ -108,7 +131,8 @@ ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) {
ImtqHandler::~ImtqHandler() = default; ImtqHandler::~ImtqHandler() = default;
void ImtqHandler::doStartUp() { void ImtqHandler::doStartUp() {
updatePeriodicReply(true, imtq::cmdIds::REPLY); updatePeriodicReply(true, imtq::cmdIds::REPLY_NO_TORQUE);
updatePeriodicReply(true, imtq::cmdIds::REPLY_WITH_TORQUE);
if (goToNormalMode) { if (goToNormalMode) {
setMode(MODE_NORMAL); setMode(MODE_NORMAL);
} else { } else {
@ -117,7 +141,8 @@ void ImtqHandler::doStartUp() {
} }
void ImtqHandler::doShutDown() { void ImtqHandler::doShutDown() {
updatePeriodicReply(false, imtq::cmdIds::REPLY); updatePeriodicReply(false, imtq::cmdIds::REPLY_NO_TORQUE);
updatePeriodicReply(false, imtq::cmdIds::REPLY_WITH_TORQUE);
specialRequestActive = false; specialRequestActive = false;
firstReplyCycle = true; firstReplyCycle = true;
setMode(_MODE_POWER_DOWN); setMode(_MODE_POWER_DOWN);
@ -133,6 +158,14 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
*id = imtq::cmdIds::START_ACTUATION_DIPOLE; *id = imtq::cmdIds::START_ACTUATION_DIPOLE;
return buildCommandFromCommand(*id, nullptr, 0); return buildCommandFromCommand(*id, nullptr, 0);
} }
default: {
*id = imtq::cmdIds::REQUEST;
request.request = imtq::RequestType::DO_NOTHING;
request.specialRequest = imtq::SpecialRequest::NONE;
rawPacket = reinterpret_cast<uint8_t*>(&request);
rawPacketLen = sizeof(ImtqRequest);
return returnvalue::OK;
}
} }
return NOTHING_TO_SEND; return NOTHING_TO_SEND;
} }
@ -145,11 +178,12 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
const uint8_t* commandData, const uint8_t* commandData,
size_t commandDataLen) { size_t commandDataLen) {
auto genericSpecialRequest = [&](imtq::SpecialRequest specialRequest) { auto genericSpecialRequest = [&](imtq::SpecialRequest specialRequest) {
ImtqRequest request(commandBuffer, sizeof(commandBuffer)); request.request = imtq::RequestType::MEASURE_NO_ACTUATION;
request.setMeasureRequest(specialRequest); request.specialRequest = specialRequest;
expectedReply = imtq::cmdIds::REPLY_NO_TORQUE;
specialRequestActive = true; specialRequestActive = true;
rawPacket = commandBuffer; rawPacket = reinterpret_cast<uint8_t*>(&request);
rawPacketLen = ImtqRequest::REQUEST_LEN; rawPacketLen = sizeof(ImtqRequest);
}; };
switch (deviceCommand) { switch (deviceCommand) {
case (imtq::cmdIds::POS_X_SELF_TEST): { case (imtq::cmdIds::POS_X_SELF_TEST): {
@ -181,45 +215,50 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
return returnvalue::OK; return returnvalue::OK;
} }
case (imtq::cmdIds::REQUEST): { case (imtq::cmdIds::REQUEST): {
ImtqRequest request(commandBuffer, sizeof(commandBuffer)); request.request = imtq::RequestType::MEASURE_NO_ACTUATION;
request.setMeasureRequest(imtq::SpecialRequest::NONE); request.specialRequest = imtq::SpecialRequest::NONE;
rawPacket = commandBuffer; expectedReply = imtq::cmdIds::REPLY_NO_TORQUE;
rawPacketLen = ImtqRequest::REQUEST_LEN; rawPacket = reinterpret_cast<uint8_t*>(&request);
rawPacketLen = sizeof(ImtqRequest);
return returnvalue::OK; return returnvalue::OK;
} }
case (imtq::cmdIds::START_ACTUATION_DIPOLE): { case (imtq::cmdIds::START_ACTUATION_DIPOLE): {
/* IMTQ expects low byte first */
// commandBuffer[0] = imtq::CC::START_ACTUATION_DIPOLE;
if (commandData != nullptr && commandDataLen < 8) { if (commandData != nullptr && commandDataLen < 8) {
return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
} }
ImtqRequest request(commandBuffer, sizeof(commandBuffer)); {
// Commands override anything which was set in the software // Do this in any case to read values which might be commanded by the ACS controller.
if (commandData != nullptr) {
// Read set dipole values from local pool
PoolReadGuard pg(&dipoleSet); PoolReadGuard pg(&dipoleSet);
// Commands override anything which was set in the software
int16_t xDipole = 0, yDipole = 0, zDipole = 0; if (commandData != nullptr) {
uint16_t torqueDuration = 0; dipoleSet.setValidityBufferGeneration(false);
dipoleSet.xDipole = xDipole; ReturnValue_t result = dipoleSet.deSerialize(&commandData, &commandDataLen,
dipoleSet.yDipole = yDipole; SerializeIF::Endianness::NETWORK);
dipoleSet.zDipole = zDipole; dipoleSet.setValidityBufferGeneration(true);
dipoleSet.currentTorqueDurationMs = torqueDuration; if (result != returnvalue::OK) {
return result;
}
manualTorqueCmdActive = true;
manuallyCommandedTorqueDuration.setTimeout(dipoleSet.currentTorqueDurationMs.value);
}
} }
request.setActuateRequest(dipoleSet.xDipole.value, dipoleSet.yDipole.value, expectedReply = imtq::cmdIds::REPLY_WITH_TORQUE;
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) { if (ACTUATION_WIRETAPPING) {
sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.xDipole.value sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.dipoles[0]
<< ", y = " << dipoleSet.yDipole.value << ", z = " << dipoleSet.zDipole.value << ", y = " << dipoleSet.dipoles[1] << ", z = " << dipoleSet.dipoles[2]
<< ", duration = " << dipoleSet.currentTorqueDurationMs.value << std::endl; << ", duration = " << dipoleSet.currentTorqueDurationMs.value << std::endl;
} }
MutexGuard mg(torquer::lazyLock(), torquer::LOCK_TYPE, torquer::LOCK_TIMEOUT, MutexGuard mg(torquer::lazyLock(), torquer::LOCK_TYPE, torquer::LOCK_TIMEOUT,
torquer::LOCK_CTX); torquer::LOCK_CTX);
torquer::TORQUEING = true; torquer::TORQUEING = true;
torquer::TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value); torquer::TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value);
rawPacket = commandBuffer; rawPacket = reinterpret_cast<uint8_t*>(&request);
rawPacketLen = ImtqRequest::REQUEST_LEN; rawPacketLen = sizeof(ImtqRequest);
return returnvalue::OK; return returnvalue::OK;
} }
default: default:
@ -231,7 +270,8 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
void ImtqHandler::fillCommandAndReplyMap() { void ImtqHandler::fillCommandAndReplyMap() {
insertInCommandMap(imtq::cmdIds::REQUEST); insertInCommandMap(imtq::cmdIds::REQUEST);
insertInCommandMap(imtq::cmdIds::START_ACTUATION_DIPOLE); 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::POS_X_SELF_TEST);
insertInCommandMap(imtq::cmdIds::NEG_X_SELF_TEST); insertInCommandMap(imtq::cmdIds::NEG_X_SELF_TEST);
insertInCommandMap(imtq::cmdIds::POS_Y_SELF_TEST); insertInCommandMap(imtq::cmdIds::POS_Y_SELF_TEST);
@ -248,7 +288,7 @@ ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSi
} }
if (remainingSize > 0) { if (remainingSize > 0) {
*foundLen = remainingSize; *foundLen = remainingSize;
*foundId = imtq::cmdIds::REPLY; *foundId = expectedReply;
return returnvalue::OK; return returnvalue::OK;
} }
return returnvalue::FAILED; return returnvalue::FAILED;
@ -262,8 +302,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
return returnvalue::OK; return returnvalue::OK;
} }
// arrayprinter::print(packet, ImtqReplies::BASE_LEN); // arrayprinter::print(packet, ImtqReplies::BASE_LEN);
if (requestStep == imtq::RequestType::MEASURE_NO_ACTUATION) { if (expectedReply == imtq::cmdIds::REPLY_NO_TORQUE) {
requestStep = imtq::RequestType::ACTUATE;
// sif::debug << "handle measure" << std::endl; // sif::debug << "handle measure" << std::endl;
ImtqRepliesDefault replies(packet); ImtqRepliesDefault replies(packet);
if (specialRequestActive) { if (specialRequestActive) {
@ -328,9 +367,8 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
} else { } else {
status = result; status = result;
} }
} else { } else if (expectedReply == imtq::cmdIds::REPLY_WITH_TORQUE) {
// sif::debug << "handle measure with torque" << std::endl; // sif::debug << "handle measure with torque" << std::endl;
requestStep = imtq::RequestType::MEASURE_NO_ACTUATION;
ImtqRepliesWithTorque replies(packet); ImtqRepliesWithTorque replies(packet);
if (replies.wasDipoleActuationRead()) { if (replies.wasDipoleActuationRead()) {
parseStatusByte(imtq::CC::START_ACTUATION_DIPOLE, replies.getDipoleActuation()); parseStatusByte(imtq::CC::START_ACTUATION_DIPOLE, replies.getDipoleActuation());
@ -375,6 +413,8 @@ LocalPoolDataSetBase* ImtqHandler::getDataSetHandle(sid_t sid) {
return &hkDatasetNoTorque; return &hkDatasetNoTorque;
} else if (sid == dipoleSet.getSid()) { } else if (sid == dipoleSet.getSid()) {
return &dipoleSet; return &dipoleSet;
} else if (sid == statusSet.getSid()) {
return &statusSet;
} else if (sid == hkDatasetWithTorque.getSid()) { } else if (sid == hkDatasetWithTorque.getSid()) {
return &hkDatasetWithTorque; return &hkDatasetWithTorque;
} else if (sid == rawMtmWithTorque.getSid()) { } else if (sid == rawMtmWithTorque.getSid()) {
@ -410,21 +450,26 @@ ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDat
localDataPoolMap.emplace(imtq::STATUS_BYTE_CONF, &statusConfig); localDataPoolMap.emplace(imtq::STATUS_BYTE_CONF, &statusConfig);
localDataPoolMap.emplace(imtq::STATUS_BYTE_ERROR, &statusError); localDataPoolMap.emplace(imtq::STATUS_BYTE_ERROR, &statusError);
localDataPoolMap.emplace(imtq::STATUS_BYTE_UPTIME, &statusUptime); localDataPoolMap.emplace(imtq::STATUS_BYTE_UPTIME, &statusUptime);
// ENG HK No Torque
localDataPoolMap.emplace(imtq::DIGITAL_VOLTAGE_MV, new PoolEntry<uint16_t>({0})); localDataPoolMap.emplace(imtq::DIGITAL_VOLTAGE_MV, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(imtq::ANALOG_VOLTAGE_MV, new PoolEntry<uint16_t>({0})); localDataPoolMap.emplace(imtq::ANALOG_VOLTAGE_MV, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(imtq::DIGITAL_CURRENT, new PoolEntry<float>({0})); localDataPoolMap.emplace(imtq::DIGITAL_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(imtq::ANALOG_CURRENT, new PoolEntry<float>({0})); localDataPoolMap.emplace(imtq::ANALOG_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(imtq::COIL_X_CURRENT, new PoolEntry<float>({0})); localDataPoolMap.emplace(imtq::COIL_CURRENTS, &coilCurrentsMilliampsNoTorque);
localDataPoolMap.emplace(imtq::COIL_Y_CURRENT, new PoolEntry<float>({0})); localDataPoolMap.emplace(imtq::COIL_TEMPERATURES, &coilTempsNoTorque);
localDataPoolMap.emplace(imtq::COIL_Z_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(imtq::COIL_X_TEMPERATURE, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(imtq::COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(imtq::COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(imtq::MCU_TEMPERATURE, new PoolEntry<int16_t>({0})); localDataPoolMap.emplace(imtq::MCU_TEMPERATURE, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(imtq::DIPOLES_X, &dipoleXEntry); // ENG HK With Torque
localDataPoolMap.emplace(imtq::DIPOLES_Y, &dipoleYEntry); localDataPoolMap.emplace(imtq::DIGITAL_VOLTAGE_MV_WT, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(imtq::DIPOLES_Z, &dipoleZEntry); localDataPoolMap.emplace(imtq::ANALOG_VOLTAGE_MV_WT, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(imtq::DIGITAL_CURRENT_WT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(imtq::ANALOG_CURRENT_WT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(imtq::COIL_CURRENTS_WT, &coilCurrentsMilliampsWithTorque);
localDataPoolMap.emplace(imtq::COIL_TEMPERATURES_WT, &coilTempsWithTorque);
localDataPoolMap.emplace(imtq::MCU_TEMPERATURE_WT, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(imtq::DIPOLES_ID, &dipolesPoolEntry);
localDataPoolMap.emplace(imtq::CURRENT_TORQUE_DURATION, &torqueDurationEntry); localDataPoolMap.emplace(imtq::CURRENT_TORQUE_DURATION, &torqueDurationEntry);
/** Entries of calibrated MTM measurement dataset */ /** Entries of calibrated MTM measurement dataset */
@ -432,8 +477,11 @@ ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDat
localDataPoolMap.emplace(imtq::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>({0})); localDataPoolMap.emplace(imtq::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>({0}));
/** Entries of raw MTM measurement dataset */ /** Entries of raw MTM measurement dataset */
localDataPoolMap.emplace(imtq::MTM_RAW, new PoolEntry<float>(3)); localDataPoolMap.emplace(imtq::MTM_RAW, &mtmRawNoTorque);
localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS, new PoolEntry<uint8_t>({0})); 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 */ /** INIT measurements for positive X axis test */
localDataPoolMap.emplace(imtq::INIT_POS_X_ERR, new PoolEntry<uint8_t>({0})); localDataPoolMap.emplace(imtq::INIT_POS_X_ERR, new PoolEntry<uint8_t>({0}));
@ -709,6 +757,10 @@ ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDat
subdp::DiagnosticsHkPeriodicParams(rawMtmWithTorque.getSid(), false, 10.0)); subdp::DiagnosticsHkPeriodicParams(rawMtmWithTorque.getSid(), false, 10.0));
poolManager.subscribeForDiagPeriodicPacket( poolManager.subscribeForDiagPeriodicPacket(
subdp::DiagnosticsHkPeriodicParams(calMtmMeasurementSet.getSid(), false, 10.0)); 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); return DeviceHandlerBase::initializeLocalDataPool(localDataPoolMap, poolManager);
} }
@ -732,7 +784,7 @@ ReturnValue_t ImtqHandler::getSelfTestCommandId(DeviceCommandId_t* id) {
} }
ReturnValue_t ImtqHandler::parseStatusByte(imtq::CC::CC command, const uint8_t* packet) { 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) { if (cmdErrorField == 0) {
return returnvalue::OK; return returnvalue::OK;
} }
@ -755,16 +807,20 @@ ReturnValue_t ImtqHandler::parseStatusByte(imtq::CC::CC command, const uint8_t*
<< " has invalid parameter" << std::endl; << " has invalid parameter" << std::endl;
return imtq::PARAMETER_INVALID; return imtq::PARAMETER_INVALID;
case 5: 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; return imtq::CC_UNAVAILABLE;
case 7: 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; return imtq::INTERNAL_PROCESSING_ERROR;
default: default:
sif::error << "IMTQ::parseStatusByte: CMD Error field contains unknown error code 0x" sif::error << "IMTQ::parseStatusByte: CMD error field for command 0x" << std::setw(2)
<< static_cast<int>(cmdErrorField) << std::endl; << command << " contains unknown error code 0x" << static_cast<int>(cmdErrorField)
<< std::endl;
return imtq::CMD_ERR_UNKNOWN; return imtq::CMD_ERR_UNKNOWN;
} }
sif::error << std::dec;
} }
void ImtqHandler::fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* packet) { void ImtqHandler::fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* packet) {
@ -778,20 +834,20 @@ void ImtqHandler::fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* pa
offset += 2; offset += 2;
hkDataset.analogCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; hkDataset.analogCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2; offset += 2;
hkDataset.coilXCurrentmA = hkDataset.coilCurrentsMilliamps[0] =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2; offset += 2;
hkDataset.coilYCurrentmA = hkDataset.coilCurrentsMilliamps[1] =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2; offset += 2;
hkDataset.coilZCurrentmA = hkDataset.coilCurrentsMilliamps[2] =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2; offset += 2;
hkDataset.coilXTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); hkDataset.coilTemperatures[0] = (*(packet + offset + 1) << 8 | *(packet + offset));
offset += 2; offset += 2;
hkDataset.coilYTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); hkDataset.coilTemperatures[1] = (*(packet + offset + 1) << 8 | *(packet + offset));
offset += 2; offset += 2;
hkDataset.coilZTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); hkDataset.coilTemperatures[2] = (*(packet + offset + 1) << 8 | *(packet + offset));
offset += 2; offset += 2;
size_t dummy = 2; size_t dummy = 2;
SerializeAdapter::deSerialize(&hkDataset.mcuTemperature.value, packet + offset, &dummy, SerializeAdapter::deSerialize(&hkDataset.mcuTemperature.value, packet + offset, &dummy,
@ -805,12 +861,15 @@ void ImtqHandler::fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* pa
sif::info << "IMTQ analog voltage: " << hkDataset.analogVoltageMv << " mV" << std::endl; sif::info << "IMTQ analog voltage: " << hkDataset.analogVoltageMv << " mV" << std::endl;
sif::info << "IMTQ digital current: " << hkDataset.digitalCurrentmA << " mA" << 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 analog current: " << hkDataset.analogCurrentmA << " mA" << std::endl;
sif::info << "IMTQ coil X current: " << hkDataset.coilXCurrentmA << " mA" << std::endl; sif::info << "IMTQ coil X current: " << hkDataset.coilCurrentsMilliamps[0] << " mA"
sif::info << "IMTQ coil Y current: " << hkDataset.coilYCurrentmA << " mA" << std::endl; << std::endl;
sif::info << "IMTQ coil Z current: " << hkDataset.coilZCurrentmA << " mA" << std::endl; sif::info << "IMTQ coil Y current: " << hkDataset.coilCurrentsMilliamps[1] << " mA"
sif::info << "IMTQ coil X temperature: " << hkDataset.coilXTemperature << " °C" << std::endl; << std::endl;
sif::info << "IMTQ coil Y temperature: " << hkDataset.coilYTemperature << " °C" << std::endl; sif::info << "IMTQ coil Z current: " << hkDataset.coilCurrentsMilliamps[2] << " mA"
sif::info << "IMTQ coil Z temperature: " << hkDataset.coilZTemperature << " °C" << std::endl; << 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; sif::info << "IMTQ coil MCU temperature: " << hkDataset.mcuTemperature << " °C" << std::endl;
#endif #endif
} }
@ -851,7 +910,7 @@ void ImtqHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
void ImtqHandler::fillRawMtmDataset(imtq::RawMtmMeasurementSet& set, const uint8_t* packet) { void ImtqHandler::fillRawMtmDataset(imtq::RawMtmMeasurementSet& set, const uint8_t* packet) {
PoolReadGuard rg(&set); PoolReadGuard rg(&set);
if (rg.getReadResult() != returnvalue::OK) { 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; unsigned int offset = 2;
size_t deSerLen = 16; size_t deSerLen = 16;
@ -887,6 +946,7 @@ void ImtqHandler::fillRawMtmDataset(imtq::RawMtmMeasurementSet& set, const uint8
set.setValidity(true, true); set.setValidity(true, true);
if (debugMode) { if (debugMode) {
#if OBSW_VERBOSE_LEVEL >= 1 #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 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 Y: " << set.mtmRawNt[1] << " nT" << std::endl;
sif::info << "IMTQ raw MTM measurement Z: " << set.mtmRawNt[2] << " nT" << std::endl; sif::info << "IMTQ raw MTM measurement Z: " << set.mtmRawNt[2] << " nT" << std::endl;

21
mission/devices/ImtqHandler.h
View File

@ -98,6 +98,9 @@ class ImtqHandler : public DeviceHandlerBase {
imtq::NegYSelfTestSet negYselfTestDataset; imtq::NegYSelfTestSet negYselfTestDataset;
imtq::PosZSelfTestSet posZselfTestDataset; imtq::PosZSelfTestSet posZselfTestDataset;
imtq::NegZSelfTestSet negZselfTestDataset; imtq::NegZSelfTestSet negZselfTestDataset;
bool manualTorqueCmdActive = false;
bool ignoreActForRestOfComSteps = false;
Countdown manuallyCommandedTorqueDuration = Countdown();
NormalPollingMode pollingMode = NormalPollingMode::UNCALIBRATED; NormalPollingMode pollingMode = NormalPollingMode::UNCALIBRATED;
@ -107,13 +110,21 @@ class ImtqHandler : public DeviceHandlerBase {
PoolEntry<uint32_t> statusUptime = PoolEntry<uint32_t>({0}); PoolEntry<uint32_t> statusUptime = PoolEntry<uint32_t>({0});
PoolEntry<int32_t> mgmCalEntry = PoolEntry<int32_t>(3); PoolEntry<int32_t> mgmCalEntry = PoolEntry<int32_t>(3);
PoolEntry<int16_t> dipoleXEntry = PoolEntry<int16_t>(0, false); PoolEntry<int16_t> dipolesPoolEntry = PoolEntry<int16_t>({0, 0, 0}, false);
PoolEntry<int16_t> dipoleYEntry = PoolEntry<int16_t>(0, false); PoolEntry<uint16_t> torqueDurationEntry = PoolEntry<uint16_t>({0}, false);
PoolEntry<int16_t> dipoleZEntry = PoolEntry<int16_t>(0, false); PoolEntry<float> coilCurrentsMilliampsNoTorque = PoolEntry<float>(3);
PoolEntry<uint16_t> torqueDurationEntry = PoolEntry<uint16_t>(0, false); PoolEntry<float> coilCurrentsMilliampsWithTorque = PoolEntry<float>(3);
PoolEntry<int16_t> coilTempsNoTorque = PoolEntry<int16_t>(3);
PoolEntry<int16_t> coilTempsWithTorque = PoolEntry<int16_t>(3);
PoolEntry<float> mtmRawNoTorque = PoolEntry<float>(3);
PoolEntry<uint8_t> actStatusNoTorque = PoolEntry<uint8_t>(1);
PoolEntry<float> mtmRawWithTorque = PoolEntry<float>(3);
PoolEntry<uint8_t> actStatusWithTorque = PoolEntry<uint8_t>(1);
power::Switch_t switcher = power::NO_SWITCH; 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 goToNormalMode = false;
bool debugMode = false; bool debugMode = false;
bool specialRequestActive = false; bool specialRequestActive = false;

199
mission/devices/devicedefinitions/imtqHelpers.h
View File

@ -22,7 +22,7 @@ enum ComStep : uint8_t {
READ_ACTUATE_GET = 8, 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 { enum class SpecialRequest : uint8_t {
NONE = 0, NONE = 0,
@ -53,7 +53,8 @@ static const ReturnValue_t UNEXPECTED_SELF_TEST_REPLY = MAKE_RETURN_CODE(0xA7);
namespace cmdIds { namespace cmdIds {
static constexpr DeviceCommandId_t REQUEST = 0x70; 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 START_ACTUATION_DIPOLE = 0x2;
static const DeviceCommandId_t POS_X_SELF_TEST = 0x7; static const DeviceCommandId_t POS_X_SELF_TEST = 0x7;
static const DeviceCommandId_t NEG_X_SELF_TEST = 0x8; static const DeviceCommandId_t NEG_X_SELF_TEST = 0x8;
@ -195,20 +196,28 @@ enum PoolIds : lp_id_t {
ANALOG_VOLTAGE_MV, ANALOG_VOLTAGE_MV,
DIGITAL_CURRENT, DIGITAL_CURRENT,
ANALOG_CURRENT, ANALOG_CURRENT,
COIL_X_CURRENT, COIL_CURRENTS,
COIL_Y_CURRENT, COIL_TEMPERATURES,
COIL_Z_CURRENT,
COIL_X_TEMPERATURE,
COIL_Y_TEMPERATURE,
COIL_Z_TEMPERATURE,
MCU_TEMPERATURE, 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, MGM_CAL_NT,
ACTUATION_CAL_STATUS, ACTUATION_CAL_STATUS,
MTM_RAW, MTM_RAW,
ACTUATION_RAW_STATUS, ACTUATION_RAW_STATUS,
DIPOLES_X,
DIPOLES_Y, MTM_RAW_WT,
DIPOLES_Z, ACTUATION_RAW_STATUS_WT,
DIPOLES_ID,
CURRENT_TORQUE_DURATION, CURRENT_TORQUE_DURATION,
INIT_POS_X_ERR, INIT_POS_X_ERR,
@ -476,34 +485,56 @@ class StatusDataset : public StaticLocalDataSet<4> {
class HkDataset : public StaticLocalDataSet<HK_SET_POOL_ENTRIES> { class HkDataset : public StaticLocalDataSet<HK_SET_POOL_ENTRIES> {
public: public:
HkDataset(HasLocalDataPoolIF* owner, uint32_t setId) : StaticLocalDataSet(owner, setId) {} HkDataset(HasLocalDataPoolIF* owner, uint32_t setId, std::array<lp_id_t, 7> 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<lp_id_t, 7> 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 // Engineering HK variables
lp_var_t<uint16_t> digitalVoltageMv = lp_var_t<uint16_t>(sid.objectId, DIGITAL_VOLTAGE_MV, this); lp_var_t<uint16_t> digitalVoltageMv;
lp_var_t<uint16_t> analogVoltageMv = lp_var_t<uint16_t>(sid.objectId, ANALOG_VOLTAGE_MV, this); lp_var_t<uint16_t> analogVoltageMv;
lp_var_t<float> digitalCurrentmA = lp_var_t<float>(sid.objectId, DIGITAL_CURRENT, this); lp_var_t<float> digitalCurrentmA;
lp_var_t<float> analogCurrentmA = lp_var_t<float>(sid.objectId, ANALOG_CURRENT, this); lp_var_t<float> analogCurrentmA;
lp_var_t<float> coilXCurrentmA = lp_var_t<float>(sid.objectId, COIL_X_CURRENT, this); lp_vec_t<float, 3> coilCurrentsMilliamps;
lp_var_t<float> coilYCurrentmA = lp_var_t<float>(sid.objectId, COIL_Y_CURRENT, this); /** All temperatures in [C] for X, Y, Z */
lp_var_t<float> coilZCurrentmA = lp_var_t<float>(sid.objectId, COIL_Z_CURRENT, this); lp_vec_t<int16_t, 3> coilTemperatures;
/** All temperatures in [<5B>C] */ lp_var_t<int16_t> mcuTemperature;
lp_var_t<int16_t> coilXTemperature = lp_var_t<int16_t>(sid.objectId, COIL_X_TEMPERATURE, this);
lp_var_t<int16_t> coilYTemperature = lp_var_t<int16_t>(sid.objectId, COIL_Y_TEMPERATURE, this); private:
lp_var_t<int16_t> coilZTemperature = lp_var_t<int16_t>(sid.objectId, COIL_Z_TEMPERATURE, this);
lp_var_t<int16_t> mcuTemperature = lp_var_t<int16_t>(sid.objectId, MCU_TEMPERATURE, this);
}; };
class HkDatasetNoTorque : public HkDataset { class HkDatasetNoTorque : public HkDataset {
public: 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 { class HkDatasetWithTorque : public HkDataset {
public: public:
HkDatasetWithTorque(HasLocalDataPoolIF* owner) 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 +560,39 @@ class CalibratedMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRI
*/ */
class RawMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRIES> { class RawMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRIES> {
public: public:
RawMtmMeasurementSet(HasLocalDataPoolIF* owner, uint32_t setId) RawMtmMeasurementSet(object_id_t objectId, uint32_t setId, std::array<lp_id_t, 2> pids)
: StaticLocalDataSet(owner, setId) {} : 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) RawMtmMeasurementSet(HasLocalDataPoolIF* owner, uint32_t setId, std::array<lp_id_t, 2> pids)
: StaticLocalDataSet(sid_t(objectId, setId)) {} : StaticLocalDataSet(owner, setId),
mtmRawNt(sid.objectId, pids.at(0), this),
coilActuationStatus(sid.objectId, pids.at(1), this) {}
/** The unit of all measurements is nT */ /** The unit of all measurements is nT */
lp_vec_t<float, 3> mtmRawNt = lp_vec_t<float, 3>(sid.objectId, MTM_RAW, this); lp_vec_t<float, 3> mtmRawNt;
/** 1 if coils were actuating during measurement otherwise 0 */ /** 1 if coils were actuating during measurement otherwise 0 */
lp_var_t<uint8_t> coilActuationStatus = lp_var_t<uint8_t> coilActuationStatus;
lp_var_t<uint8_t>(sid.objectId, ACTUATION_RAW_STATUS, this);
}; };
class RawMtmMeasurementNoTorque : public RawMtmMeasurementSet { class RawMtmMeasurementNoTorque : public RawMtmMeasurementSet {
public: public:
RawMtmMeasurementNoTorque(HasLocalDataPoolIF* owner) 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) 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 { class RawMtmMeasurementWithTorque : public RawMtmMeasurementSet {
public: public:
RawMtmMeasurementWithTorque(HasLocalDataPoolIF* owner) 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) 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 +646,16 @@ class DipoleActuationSet : public StaticLocalDataSet<4> {
void setDipoles(int16_t xDipole_, int16_t yDipole_, int16_t zDipole_, void setDipoles(int16_t xDipole_, int16_t yDipole_, int16_t zDipole_,
uint16_t currentTorqueDurationMs_) { uint16_t currentTorqueDurationMs_) {
if (xDipole.value != xDipole_) { dipoles[0] = xDipole_;
xDipole = xDipole_; dipoles[1] = yDipole_;
} dipoles[2] = zDipole_;
if (yDipole.value != yDipole_) {
yDipole = yDipole_;
}
if (zDipole.value != zDipole_) {
zDipole = zDipole_;
}
currentTorqueDurationMs = currentTorqueDurationMs_; currentTorqueDurationMs = currentTorqueDurationMs_;
} }
void getDipoles(int16_t& xDipole_, int16_t& yDipole_, int16_t& zDipole_) { const int16_t* getDipoles() const { return dipoles.value; }
xDipole_ = xDipole.value;
yDipole_ = yDipole.value;
zDipole_ = zDipole.value;
}
private: private:
lp_var_t<int16_t> xDipole = lp_var_t<int16_t>(sid.objectId, DIPOLES_X, this); lp_vec_t<int16_t, 3> dipoles = lp_vec_t<int16_t, 3>(sid.objectId, DIPOLES_ID, this);
lp_var_t<int16_t> yDipole = lp_var_t<int16_t>(sid.objectId, DIPOLES_Y, this);
lp_var_t<int16_t> zDipole = lp_var_t<int16_t>(sid.objectId, DIPOLES_Z, this);
lp_var_t<uint16_t> currentTorqueDurationMs = lp_var_t<uint16_t> currentTorqueDurationMs =
lp_var_t<uint16_t>(sid.objectId, CURRENT_TORQUE_DURATION, this); lp_var_t<uint16_t>(sid.objectId, CURRENT_TORQUE_DURATION, this);
}; };
@ -1099,65 +1125,10 @@ class NegZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
} // namespace imtq } // namespace imtq
struct ImtqRequest { struct ImtqRequest {
friend class ImtqHandler; imtq::RequestType request = imtq::RequestType::MEASURE_NO_ACTUATION;
imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE;
public: int16_t dipoles[3]{};
static constexpr size_t REQUEST_LEN = 10; uint16_t torqueDuration = 0;
ImtqRequest(const uint8_t* data, size_t maxSize)
: rawData(const_cast<uint8_t*>(data)), maxSize(maxSize) {}
imtq::RequestType getRequestType() const { return static_cast<imtq::RequestType>(rawData[0]); }
void setMeasureRequest(imtq::SpecialRequest specialRequest) {
rawData[0] = static_cast<uint8_t>(imtq::RequestType::MEASURE_NO_ACTUATION);
rawData[1] = static_cast<uint8_t>(specialRequest);
}
void setActuateRequest(int16_t dipoleX, int16_t dipoleY, int16_t dipoleZ,
uint16_t torqueDuration) {
size_t dummy = 0;
rawData[0] = static_cast<uint8_t>(imtq::RequestType::ACTUATE);
rawData[1] = static_cast<uint8_t>(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<int16_t*>(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<uint8_t>(specialRequest);
}
imtq::SpecialRequest getSpecialRequest() const {
return static_cast<imtq::SpecialRequest>(rawData[1]);
}
private:
ImtqRequest(uint8_t* rawData, size_t maxLen) : rawData(rawData) {
if (rawData != nullptr) {
rawData[0] = static_cast<uint8_t>(imtq::RequestType::MEASURE_NO_ACTUATION);
}
}
uint8_t* rawData;
size_t maxSize = 0;
}; };
struct ImtqRepliesDefault { struct ImtqRepliesDefault {

2
tmtc

@ -1 +1 @@
Subproject commit 94ae2d16e21ade8f89723b2e62356967a67b171d Subproject commit 09c694cf9c72762114de760f728a76051b3745d9