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

Refactor IMTQ handling #384

Merged
muellerr merged 30 commits from refactor_imtq_handling into develop 2023-02-21 11:04:29 +01:00
Conversation 0 Commits 30 Files Changed 34 +293 -225
5 changed files with 293 additions and 225 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 2bacf1efa0 - Show all commits

261
linux/devices/ImtqPollingTask.cpp
View File

@ -18,8 +18,6 @@ ImtqPollingTask::ImtqPollingTask(object_id_t imtqPollingTask) : SystemObject(imt
}
ReturnValue_t ImtqPollingTask::performOperation(uint8_t operationCode) {
size_t replyLen = 0;
uint8_t* replyPtr;
while (true) {
ipcLock->lockMutex();
state = InternalState::IDLE;
@ -28,91 +26,13 @@ ReturnValue_t ImtqPollingTask::performOperation(uint8_t operationCode) {
comStatus = returnvalue::OK;
// Stopwatch watch;
auto i2cCmdExecDefault = [&](imtq::CC::CC cc, ReturnValue_t comErrIfFails =
imtq::MGM_MEASUREMENT_LOW_LEVEL_ERROR) {
ReturnValue_t res = performI2cFullRequest(replyPtr + 1, replyLen);
if (res != returnvalue::OK) {
sif::error << "IMTQ: I2C transaction for command 0x" << std::hex << std::setw(2) << cc
<< " failed" << std::dec << std::endl;
comStatus = comErrIfFails;
return returnvalue::FAILED;
}
if (replyPtr[1] != cc) {
sif::warning << "IMTQ: Unexpected CC 0x" << std::hex << std::setw(2)
<< static_cast<int>(replyPtr[1]) << " for command 0x" << cc << std::dec
<< std::endl;
comStatus = comErrIfFails;
return returnvalue::FAILED;
}
replyPtr[0] = true;
return returnvalue::OK;
};
switch (currentRequest) {
case imtq::RequestType::MEASURE: {
ImtqRepliesDefault replies(replyBuf.data());
auto i2cCmdExecMeasure = [&](imtq::CC::CC cc) {
ccToReplyPtrMeasure(replies, cc, &replyPtr, replyLen);
return i2cCmdExecDefault(cc);
};
cmdLen = 1;
cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT;
if (i2cCmdExecMeasure(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) {
break;
}
cmdBuf[0] = imtq::CC::GET_SYSTEM_STATE;
if (i2cCmdExecMeasure(imtq::CC::GET_SYSTEM_STATE) != returnvalue::OK) {
break;
}
// Takes a bit of time to take measurements. Subtract a bit because of the delay of previous
// commands.
TaskFactory::delayTask(currentIntegrationTimeMs - 1);
cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
break;
}
cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA;
if (i2cCmdExecMeasure(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) {
break;
}
cmdBuf[0] = imtq::CC::GET_CAL_MTM_MEASUREMENT;
if (i2cCmdExecMeasure(imtq::CC::GET_CAL_MTM_MEASUREMENT) != returnvalue::OK) {
break;
}
case imtq::RequestType::MEASURE_WITH_ACTUATION: {
handleMeasureStep();
break;
}
case imtq::RequestType::ACTUATE: {
ImtqRepliesWithTorque replies(replyBufActuation.data());
auto i2cCmdExecActuate = [&](imtq::CC::CC cc) {
ccToReplyPtrActuate(replies, cc, &replyPtr, replyLen);
return i2cCmdExecDefault(cc, imtq::ACTUATE_CMD_LOW_LEVEL_ERROR);
};
buildDipoleCommand();
if (i2cCmdExecActuate(imtq::CC::START_ACTUATION_DIPOLE) != returnvalue::OK) {
break;
}
cmdLen = 1;
cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT;
if (i2cCmdExecActuate(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) {
break;
}
TaskFactory::delayTask(currentIntegrationTimeMs);
cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
break;
}
cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA;
if (i2cCmdExecActuate(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) {
break;
}
handleActuateStep();
break;
}
};
@ -120,6 +40,141 @@ ReturnValue_t ImtqPollingTask::performOperation(uint8_t operationCode) {
return returnvalue::OK;
}
void ImtqPollingTask::handleMeasureStep() {
size_t replyLen = 0;
uint8_t* replyPtr;
ImtqRepliesDefault replies(replyBuf.data());
// Can be used later to verify correct timing (e.g. all data has been read)
clearReadFlagsDefault(replies);
auto i2cCmdExecMeasure = [&](imtq::CC::CC cc) {
ccToReplyPtrMeasure(replies, cc, &replyPtr, replyLen);
return i2cCmdExecDefault(cc, replyPtr, replyLen, imtq::MGM_MEASUREMENT_LOW_LEVEL_ERROR);
};
cmdLen = 1;
cmdBuf[0] = imtq::CC::GET_SYSTEM_STATE;
if (i2cCmdExecMeasure(imtq::CC::GET_SYSTEM_STATE) != returnvalue::OK) {
return;
}
ignoreNextActuateRequest =
(replies.getSystemState()[2] == static_cast<uint8_t>(imtq::mode::SELF_TEST));
if (ignoreNextActuateRequest) {
// Do not command anything until self-test is done.
return;
}
if (specialRequest != imtq::SpecialRequest::NONE) {
auto executeSelfTest = [&](imtq::selfTest::Axis axis) {
cmdBuf[0] = imtq::CC::SELF_TEST_CMD;
cmdBuf[1] = axis;
return i2cCmdExecMeasure(imtq::CC::SELF_TEST_CMD);
};
// If a self-test is already ongoing, ignore the request.
if (replies.getSystemState()[2] != static_cast<uint8_t>(imtq::mode::SELF_TEST)) {
switch (specialRequest) {
case (imtq::SpecialRequest::DO_SELF_TEST_POS_X): {
executeSelfTest(imtq::selfTest::Axis::X_POSITIVE);
break;
}
case (imtq::SpecialRequest::DO_SELF_TEST_NEG_X): {
executeSelfTest(imtq::selfTest::Axis::X_NEGATIVE);
break;
}
case (imtq::SpecialRequest::DO_SELF_TEST_POS_Y): {
executeSelfTest(imtq::selfTest::Axis::Y_POSITIVE);
break;
}
case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Y): {
executeSelfTest(imtq::selfTest::Axis::Y_NEGATIVE);
break;
}
case (imtq::SpecialRequest::DO_SELF_TEST_POS_Z): {
executeSelfTest(imtq::selfTest::Axis::Z_POSITIVE);
break;
}
case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Z): {
executeSelfTest(imtq::selfTest::Axis::Z_NEGATIVE);
break;
}
case (imtq::SpecialRequest::GET_SELF_TEST_RESULT): {
cmdBuf[0] = imtq::CC::GET_SELF_TEST_RESULT;
i2cCmdExecMeasure(imtq::CC::GET_SELF_TEST_RESULT);
break;
}
default: {
// Should never happen
break;
}
}
// We are done. Only request self test or results here.
return;
}
}
cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT;
if (i2cCmdExecMeasure(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) {
return;
}
// Takes a bit of time to take measurements. Subtract a bit because of the delay of previous
// commands.
TaskFactory::delayTask(currentIntegrationTimeMs - 1);
cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
return;
}
cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA;
if (i2cCmdExecMeasure(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) {
return;
}
cmdBuf[0] = imtq::CC::GET_CAL_MTM_MEASUREMENT;
if (i2cCmdExecMeasure(imtq::CC::GET_CAL_MTM_MEASUREMENT) != returnvalue::OK) {
return;
}
return;
}
void ImtqPollingTask::handleActuateStep() {
uint8_t* replyPtr = nullptr;
size_t replyLen = 0;
// No point when self-test mode is active.
if (ignoreNextActuateRequest) {
return;
}
ImtqRepliesWithTorque replies(replyBufActuation.data());
// Can be used later to verify correct timing (e.g. all data has been read)
clearReadFlagsWithTorque(replies);
auto i2cCmdExecActuate = [&](imtq::CC::CC cc) {
ccToReplyPtrActuate(replies, cc, &replyPtr, replyLen);
return i2cCmdExecDefault(cc, replyPtr, replyLen, imtq::ACTUATE_CMD_LOW_LEVEL_ERROR);
};
buildDipoleCommand();
if (i2cCmdExecActuate(imtq::CC::START_ACTUATION_DIPOLE) != returnvalue::OK) {
return;
}
cmdLen = 1;
cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT;
if (i2cCmdExecActuate(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) {
return;
}
TaskFactory::delayTask(currentIntegrationTimeMs);
cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
return;
}
cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA;
if (i2cCmdExecActuate(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) {
return;
}
return;
}
ReturnValue_t ImtqPollingTask::initialize() { return returnvalue::OK; }
ReturnValue_t ImtqPollingTask::initializeInterface(CookieIF* cookie) {
@ -226,9 +281,12 @@ size_t ImtqPollingTask::getExchangeBufLen(imtq::SpecialRequest specialRequest) {
size_t baseLen = ImtqRepliesDefault::BASE_LEN;
switch (specialRequest) {
case (imtq::SpecialRequest::NONE):
case (imtq::SpecialRequest::DO_SELF_TEST_X):
case (imtq::SpecialRequest::DO_SELF_TEST_Y):
case (imtq::SpecialRequest::DO_SELF_TEST_Z): {
case (imtq::SpecialRequest::DO_SELF_TEST_POS_X):
case (imtq::SpecialRequest::DO_SELF_TEST_NEG_X):
case (imtq::SpecialRequest::DO_SELF_TEST_POS_Y):
case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Y):
case (imtq::SpecialRequest::DO_SELF_TEST_POS_Z):
case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Z): {
break;
}
case (imtq::SpecialRequest::GET_SELF_TEST_RESULT): {
@ -262,7 +320,7 @@ ReturnValue_t ImtqPollingTask::readReceivedMessage(CookieIF* cookie, uint8_t** b
size_t replyLen = 0;
MutexGuard mg(bufLock);
if (currentRequest == imtq::RequestType::MEASURE) {
if (currentRequest == imtq::RequestType::MEASURE_WITH_ACTUATION) {
replyLen = getExchangeBufLen(specialRequest);
memcpy(exchangeBuf.data(), replyBuf.data(), replyLen);
} else {
@ -274,7 +332,7 @@ ReturnValue_t ImtqPollingTask::readReceivedMessage(CookieIF* cookie, uint8_t** b
return comStatus;
}
void ImtqPollingTask::clearReadFlags(ImtqRepliesDefault& replies) {
void ImtqPollingTask::clearReadFlagsDefault(ImtqRepliesDefault& replies) {
replies.calibMgmMeasurement[0] = false;
replies.rawMgmMeasurement[0] = false;
replies.systemState[0] = false;
@ -282,6 +340,33 @@ void ImtqPollingTask::clearReadFlags(ImtqRepliesDefault& replies) {
replies.engHk[0] = false;
}
ReturnValue_t ImtqPollingTask::i2cCmdExecDefault(imtq::CC::CC cc, uint8_t* replyPtr,
size_t replyLen, ReturnValue_t comErrIfFails) {
ReturnValue_t res = performI2cFullRequest(replyPtr + 1, replyLen);
if (res != returnvalue::OK) {
sif::error << "IMTQ: I2C transaction for command 0x" << std::hex << std::setw(2) << cc
<< " failed" << std::dec << std::endl;
comStatus = comErrIfFails;
return returnvalue::FAILED;
}
if (replyPtr[1] != cc) {
sif::warning << "IMTQ: Unexpected CC 0x" << std::hex << std::setw(2)
<< static_cast<int>(replyPtr[1]) << " for command 0x" << cc << std::dec
<< std::endl;
comStatus = comErrIfFails;
return returnvalue::FAILED;
}
replyPtr[0] = true;
return returnvalue::OK;
}
void ImtqPollingTask::clearReadFlagsWithTorque(ImtqRepliesWithTorque& replies) {
replies.dipoleActuation[0] = false;
replies.engHk[0] = false;
replies.rawMgmMeasurement[0] = false;
replies.startMtmMeasurement[0] = false;
}
ReturnValue_t ImtqPollingTask::performI2cFullRequest(uint8_t* reply, size_t replyLen) {
int fd = 0;
if (cmdLen == 0 or reply == nullptr) {

10
linux/devices/ImtqPollingTask.h
View File

@ -22,7 +22,7 @@ class ImtqPollingTask : public SystemObject,
static constexpr ReturnValue_t NO_REPLY_AVAILABLE = returnvalue::makeCode(2, 0);
enum class InternalState { IDLE, BUSY } state = InternalState::IDLE;
imtq::RequestType currentRequest = imtq::RequestType::MEASURE;
imtq::RequestType currentRequest = imtq::RequestType::MEASURE_WITH_ACTUATION;
SemaphoreIF* semaphore;
ReturnValue_t comStatus = returnvalue::OK;
@ -32,6 +32,7 @@ class ImtqPollingTask : public SystemObject,
const char* i2cDev = nullptr;
address_t i2cAddr = 0;
uint32_t currentIntegrationTimeMs = 10;
bool ignoreNextActuateRequest = false;
imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE;
int16_t dipoles[3] = {};
@ -55,9 +56,14 @@ class ImtqPollingTask : public SystemObject,
size_t& replyLen);
void ccToReplyPtrActuate(ImtqRepliesWithTorque& replies, imtq::CC::CC cc, uint8_t** replyBuf,
size_t& replyLen);
void clearReadFlags(ImtqRepliesDefault& replies);
void clearReadFlagsDefault(ImtqRepliesDefault& replies);
void clearReadFlagsWithTorque(ImtqRepliesWithTorque& replies);
size_t getExchangeBufLen(imtq::SpecialRequest specialRequest);
void buildDipoleCommand();
void handleMeasureStep();
void handleActuateStep();
ReturnValue_t i2cCmdExecDefault(imtq::CC::CC cc, uint8_t* replyPtr, size_t replyLen,
ReturnValue_t comErrIfFails);
ReturnValue_t performI2cFullRequest(uint8_t* reply, size_t replyLen);
};

198
mission/devices/ImtqHandler.cpp
View File

@ -69,7 +69,7 @@ void ImtqHandler::doShutDown() {
ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
switch (requestStep) {
case (imtq::RequestType::MEASURE): {
case (imtq::RequestType::MEASURE_WITH_ACTUATION): {
*id = imtq::cmdIds::REQUEST;
return buildCommandFromCommand(*id, nullptr, 0);
}
@ -88,40 +88,43 @@ ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
const uint8_t* commandData,
size_t commandDataLen) {
auto genericMeasureRequest = [&](imtq::SpecialRequest specialRequest) {
ImtqRequest request(commandBuffer, sizeof(commandBuffer));
request.setMeasureRequest(specialRequest);
rawPacket = commandBuffer;
rawPacketLen = ImtqRequest::REQUEST_LEN;
};
switch (deviceCommand) {
case (imtq::cmdIds::POS_X_SELF_TEST): {
// TODO: special request
genericMeasureRequest(imtq::SpecialRequest::DO_SELF_TEST_POS_X);
return returnvalue::OK;
}
case (imtq::cmdIds::NEG_X_SELF_TEST): {
// TODO: special request
genericMeasureRequest(imtq::SpecialRequest::DO_SELF_TEST_NEG_X);
return returnvalue::OK;
}
case (imtq::cmdIds::POS_Y_SELF_TEST): {
// TODO: special request
genericMeasureRequest(imtq::SpecialRequest::DO_SELF_TEST_POS_Y);
return returnvalue::OK;
}
case (imtq::cmdIds::NEG_Y_SELF_TEST): {
// TODO: special request
genericMeasureRequest(imtq::SpecialRequest::DO_SELF_TEST_NEG_Y);
return returnvalue::OK;
}
case (imtq::cmdIds::POS_Z_SELF_TEST): {
// TODO: special request
genericMeasureRequest(imtq::SpecialRequest::DO_SELF_TEST_POS_Z);
return returnvalue::OK;
}
case (imtq::cmdIds::NEG_Z_SELF_TEST): {
// TODO: special request
genericMeasureRequest(imtq::SpecialRequest::DO_SELF_TEST_NEG_Z);
return returnvalue::OK;
}
case (imtq::cmdIds::GET_SELF_TEST_RESULT): {
// TODO: special request
genericMeasureRequest(imtq::SpecialRequest::GET_SELF_TEST_RESULT);
return returnvalue::OK;
}
case (imtq::cmdIds::REQUEST): {
ImtqRequest request(commandBuffer, sizeof(commandBuffer));
request.setMeasureRequest(imtq::SpecialRequest::NONE);
rawPacket = commandBuffer;
rawPacketLen = ImtqRequest::REQUEST_LEN;
genericMeasureRequest(imtq::SpecialRequest::NONE);
return returnvalue::OK;
}
case (imtq::cmdIds::START_ACTUATION_DIPOLE): {
@ -168,6 +171,13 @@ void ImtqHandler::fillCommandAndReplyMap() {
insertInCommandMap(imtq::cmdIds::REQUEST);
insertInCommandMap(imtq::cmdIds::START_ACTUATION_DIPOLE);
insertInReplyMap(imtq::cmdIds::REPLY, 5, nullptr, 0, true);
insertInCommandMap(imtq::cmdIds::POS_X_SELF_TEST);
insertInCommandMap(imtq::cmdIds::NEG_X_SELF_TEST);
insertInCommandMap(imtq::cmdIds::POS_Y_SELF_TEST);
insertInCommandMap(imtq::cmdIds::NEG_Y_SELF_TEST);
insertInCommandMap(imtq::cmdIds::POS_Z_SELF_TEST);
insertInCommandMap(imtq::cmdIds::NEG_Z_SELF_TEST);
insertInCommandMap(imtq::cmdIds::GET_SELF_TEST_RESULT);
}
ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSize,
@ -187,72 +197,102 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
ReturnValue_t result;
ReturnValue_t status = returnvalue::OK;
// arrayprinter::print(packet, ImtqReplies::BASE_LEN);
if (requestStep == imtq::RequestType::MEASURE) {
if (requestStep == imtq::RequestType::MEASURE_WITH_ACTUATION) {
requestStep = imtq::RequestType::ACTUATE;
ImtqRepliesDefault replies(packet);
if (replies.wasEngHkRead()) {
uint8_t* engHkReply = replies.getEngHk();
result = parseStatusByte(imtq::CC::GET_ENG_HK_DATA, engHkReply);
if (result == returnvalue::OK) {
fillEngHkDataset(hkDatasetNoTorque, engHkReply);
} else {
if (replies.wasSpecialRequestRead()) {
uint8_t* specialRequest = replies.getSpecialRequest();
imtq::CC::CC cc = specialRequest[0];
result = parseStatusByte(cc, packet);
if (result != returnvalue::OK) {
status = result;
}
if (cc == imtq::CC::CC::GET_SELF_TEST_RESULT) {
handleSelfTestReply(specialRequest);
}
// For a special request, the other stuff was not read, so return here.
return status;
} else {
sif::warning << "IMTQ: Possible timing issue, special request was not read" << std::endl;
}
if (not replies.wasEngHkRead()) {
sif::warning << "IMTQ: Possible timing issue, ENG HK was not read" << std::endl;
}
// Still read it, even if it is old. Better than nothing
uint8_t* engHkReply = replies.getEngHk();
result = parseStatusByte(imtq::CC::GET_ENG_HK_DATA, engHkReply);
if (result == returnvalue::OK) {
fillEngHkDataset(hkDatasetNoTorque, engHkReply);
} else {
status = result;
}
if (replies.wasGetSystemStateRead()) {
uint8_t* sysStateReply = replies.getSystemState();
result = parseStatusByte(imtq::CC::GET_SYSTEM_STATE, sysStateReply);
if (result == returnvalue::OK) {
fillSystemStateIntoDataset(sysStateReply);
} else {
status = result;
}
if (not replies.wasGetSystemStateRead()) {
sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl;
}
if (replies.wasGetRawMgmMeasurementRead()) {
uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement();
result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement);
if (result == returnvalue::OK) {
fillRawMtmDataset(rawMgmMeasurement);
} else {
status = result;
}
uint8_t* sysStateReply = replies.getSystemState();
result = parseStatusByte(imtq::CC::GET_SYSTEM_STATE, sysStateReply);
if (result == returnvalue::OK) {
fillSystemStateIntoDataset(sysStateReply);
} else {
status = result;
}
if (replies.wasCalibMgmMeasurementRead()) {
uint8_t* calibMgmMeasurement = replies.getCalibMgmMeasurement();
result = parseStatusByte(imtq::CC::GET_CAL_MTM_MEASUREMENT, calibMgmMeasurement);
if (result == returnvalue::OK) {
fillRawMtmDataset(calibMgmMeasurement);
} else {
status = result;
}
if (not replies.wasGetRawMgmMeasurementRead()) {
sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl;
}
uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement();
result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement);
if (result == returnvalue::OK) {
fillRawMtmDataset(rawMgmMeasurement);
} else {
status = result;
}
if (not replies.wasCalibMgmMeasurementRead()) {
sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl;
}
uint8_t* calibMgmMeasurement = replies.getCalibMgmMeasurement();
result = parseStatusByte(imtq::CC::GET_CAL_MTM_MEASUREMENT, calibMgmMeasurement);
if (result == returnvalue::OK) {
fillRawMtmDataset(calibMgmMeasurement);
} else {
status = result;
}
} else {
requestStep = imtq::RequestType::MEASURE;
requestStep = imtq::RequestType::MEASURE_WITH_ACTUATION;
ImtqRepliesWithTorque replies(packet);
if (replies.wasDipoleActuationRead()) {
parseStatusByte(imtq::CC::START_ACTUATION_DIPOLE, replies.getDipoleActuation());
} else {
sif::warning << "IMTQ: Possible timing issue, start actuation dipole status was not read"
<< std::endl;
}
if (replies.wasGetRawMgmMeasurementRead()) {
uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement();
result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement);
if (result == returnvalue::OK) {
fillRawMtmDataset(rawMgmMeasurement);
} else {
status = result;
}
if (not replies.wasGetRawMgmMeasurementRead()) {
sif::warning << "IMTQ: Possible timing issue, was MGM measurement with torque was not read"
<< std::endl;
}
if (replies.wasEngHkRead()) {
uint8_t* engHkReply = replies.getEngHk();
result = parseStatusByte(imtq::CC::GET_ENG_HK_DATA, engHkReply);
if (result != returnvalue::OK) {
return result;
} else {
status = result;
}
fillEngHkDataset(hkDatasetNoTorque, engHkReply);
uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement();
result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement);
if (result == returnvalue::OK) {
fillRawMtmDataset(rawMgmMeasurement);
} else {
status = result;
}
if (not replies.wasEngHkRead()) {
sif::warning << "IMTQ: Possible timing issue, engineering HK with torque was not read"
<< std::endl;
}
uint8_t* engHkReply = replies.getEngHk();
result = parseStatusByte(imtq::CC::GET_ENG_HK_DATA, engHkReply);
if (result != returnvalue::OK) {
return result;
} else {
status = result;
}
fillEngHkDataset(hkDatasetNoTorque, engHkReply);
}
return status;
}
@ -705,42 +745,6 @@ void ImtqHandler::fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* pa
void ImtqHandler::setToGoToNormal(bool enable) { this->goToNormalMode = enable; }
void ImtqHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) {
if (wiretappingMode == RAW) {
/* Data already sent in doGetRead() */
return;
}
DeviceReplyMap::iterator iter = deviceReplyMap.find(replyId);
if (iter == deviceReplyMap.end()) {
sif::debug << "IMTQHandler::handleDeviceTM: Unknown reply id" << std::endl;
return;
}
MessageQueueId_t queueId = iter->second.command->second.sendReplyTo;
if (queueId == NO_COMMANDER) {
return;
}
ReturnValue_t result = actionHelper.reportData(queueId, replyId, data, dataSize);
if (result != returnvalue::OK) {
sif::debug << "IMTQHandler::handleDeviceTM: Failed to report data" << std::endl;
return;
}
}
void ImtqHandler::handleGetCommandedDipoleReply(const uint8_t* packet) {
// uint8_t tmData[6];
// /* Switching endianess of received dipole values */
// tmData[0] = *(packet + 3);
// tmData[1] = *(packet + 2);
// tmData[2] = *(packet + 5);
// tmData[3] = *(packet + 4);
// tmData[4] = *(packet + 7);
// tmData[5] = *(packet + 6);
// handleDeviceTM(tmData, sizeof(tmData), imtq::cmdIds::GET_COMMANDED_DIPOLE);
}
void ImtqHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
PoolReadGuard rg(&calMtmMeasurementSet);
calMtmMeasurementSet.setValidity(true, true);

22
mission/devices/ImtqHandler.h
View File

@ -116,11 +116,7 @@ class ImtqHandler : public DeviceHandlerBase {
bool goToNormalMode = false;
bool debugMode = false;
imtq::RequestType requestStep = imtq::RequestType::MEASURE;
// enum class StartupStep { NONE, COMMAND_SELF_TEST, GET_SELF_TEST_RESULT };
// StartupStep startupStep = StartupStep::COMMAND_SELF_TEST;
imtq::RequestType requestStep = imtq::RequestType::MEASURE_WITH_ACTUATION;
/**
* @brief In case of a status reply to a single axis self test command, this function
@ -148,22 +144,6 @@ class ImtqHandler : public DeviceHandlerBase {
void fillSystemStateIntoDataset(const uint8_t* packet);
/**
* @brief This function sends a command reply to the requesting queue.
*
* @param data Pointer to the data to send.
* @param dataSize Size of the data to send.
* @param relplyId Reply id which will be inserted at the beginning of the action message.
*/
void handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId);
/**
* @brief This function handles the reply containing the commanded dipole.
*
* @param packet Pointer to the reply data.
*/
void handleGetCommandedDipoleReply(const uint8_t* packet);
/**
* @brief This function parses the reply containing the calibrated MTM measurement and writes
* the values to the appropriate dataset.

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

@ -10,14 +10,17 @@ class ImtqHandler;
namespace imtq {
enum class RequestType : uint8_t { MEASURE, ACTUATE };
enum class RequestType : uint8_t { MEASURE_WITH_ACTUATION, ACTUATE };
enum class SpecialRequest : uint8_t {
NONE = 0,
DO_SELF_TEST_X = 1,
DO_SELF_TEST_Y = 2,
DO_SELF_TEST_Z = 3,
GET_SELF_TEST_RESULT = 4
DO_SELF_TEST_POS_X = 1,
DO_SELF_TEST_NEG_X = 2,
DO_SELF_TEST_POS_Y = 3,
DO_SELF_TEST_NEG_Y = 4,
DO_SELF_TEST_POS_Z = 5,
DO_SELF_TEST_NEG_Z = 6,
GET_SELF_TEST_RESULT = 7
};
static const uint8_t INTERFACE_ID = CLASS_ID::IMTQ_HANDLER;
@ -50,16 +53,6 @@ static const DeviceCommandId_t GET_SELF_TEST_RESULT = 0xD;
} // namespace cmdIds
// static const DeviceCommandId_t NONE = 0x0;
// static const DeviceCommandId_t GET_ENG_HK_DATA = 0x1;
// static const DeviceCommandId_t GET_COMMANDED_DIPOLE = 0x3;
///** Generates new measurement of the magnetic field */
// static const DeviceCommandId_t START_MTM_MEASUREMENT = 0x4;
///** Requests the calibrated magnetometer measurement */
// static const DeviceCommandId_t GET_CAL_MTM_MEASUREMENT = 0x5;
///** Requests the raw values measured by the built-in MTM XEN1210 */
// static const DeviceCommandId_t GET_RAW_MTM_MEASUREMENT = 0x6;
static const uint8_t POINTER_REG_SIZE = 1;
enum SetIds : uint32_t {
@ -1105,7 +1098,7 @@ struct ImtqRequest {
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);
rawData[0] = static_cast<uint8_t>(imtq::RequestType::MEASURE_WITH_ACTUATION);
rawData[1] = static_cast<uint8_t>(specialRequest);
}
@ -1148,7 +1141,7 @@ struct ImtqRequest {
private:
ImtqRequest(uint8_t* rawData, size_t maxLen) : rawData(rawData) {
if (rawData != nullptr) {
rawData[0] = static_cast<uint8_t>(imtq::RequestType::MEASURE);
rawData[0] = static_cast<uint8_t>(imtq::RequestType::MEASURE_WITH_ACTUATION);
}
}
uint8_t* rawData;