Refactor IMTQ handling #384

Merged
muellerr merged 30 commits from refactor_imtq_handling into develop 2023-02-21 11:04:29 +01:00
3 changed files with 34 additions and 23 deletions
Showing only changes of commit 2b38fe19af - Show all commits

View File

@ -118,7 +118,7 @@ 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 - 1); TaskFactory::delayTask(currentIntegrationTimeMs);
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) {

View File

@ -64,6 +64,8 @@ void ImtqHandler::doStartUp() {
void ImtqHandler::doShutDown() { void ImtqHandler::doShutDown() {
updatePeriodicReply(false, imtq::cmdIds::REPLY); updatePeriodicReply(false, imtq::cmdIds::REPLY);
specialRequestActive = false;
firstReplyCycle = true;
setMode(_MODE_POWER_DOWN); setMode(_MODE_POWER_DOWN);
} }
@ -91,6 +93,7 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
auto genericMeasureRequest = [&](imtq::SpecialRequest specialRequest) { auto genericMeasureRequest = [&](imtq::SpecialRequest specialRequest) {
ImtqRequest request(commandBuffer, sizeof(commandBuffer)); ImtqRequest request(commandBuffer, sizeof(commandBuffer));
request.setMeasureRequest(specialRequest); request.setMeasureRequest(specialRequest);
specialRequestActive = true;
rawPacket = commandBuffer; rawPacket = commandBuffer;
rawPacketLen = ImtqRequest::REQUEST_LEN; rawPacketLen = ImtqRequest::REQUEST_LEN;
}; };
@ -200,9 +203,10 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
if (requestStep == imtq::RequestType::MEASURE_WITH_ACTUATION) { if (requestStep == imtq::RequestType::MEASURE_WITH_ACTUATION) {
requestStep = imtq::RequestType::ACTUATE; requestStep = imtq::RequestType::ACTUATE;
ImtqRepliesDefault replies(packet); ImtqRepliesDefault replies(packet);
if (specialRequestActive) {
if (replies.wasSpecialRequestRead()) { if (replies.wasSpecialRequestRead()) {
uint8_t* specialRequest = replies.getSpecialRequest(); uint8_t* specialRequest = replies.getSpecialRequest();
imtq::CC::CC cc = specialRequest[0]; imtq::CC::CC cc = static_cast<imtq::CC::CC>(specialRequest[0]);
result = parseStatusByte(cc, packet); result = parseStatusByte(cc, packet);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
status = result; status = result;
@ -215,7 +219,9 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
} else { } else {
sif::warning << "IMTQ: Possible timing issue, special request was not read" << std::endl; sif::warning << "IMTQ: Possible timing issue, special request was not read" << std::endl;
} }
if (not replies.wasEngHkRead()) { specialRequestActive = false;
}
if (not replies.wasEngHkRead() and not firstReplyCycle) {
sif::warning << "IMTQ: Possible timing issue, ENG HK was not read" << std::endl; sif::warning << "IMTQ: Possible timing issue, ENG HK was not read" << std::endl;
} }
// Still read it, even if it is old. Better than nothing // Still read it, even if it is old. Better than nothing
@ -227,7 +233,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
status = result; status = result;
} }
if (not replies.wasGetSystemStateRead()) { if (not replies.wasGetSystemStateRead() and not firstReplyCycle) {
sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl; sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl;
} }
uint8_t* sysStateReply = replies.getSystemState(); uint8_t* sysStateReply = replies.getSystemState();
@ -238,7 +244,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
status = result; status = result;
} }
if (not replies.wasGetRawMgmMeasurementRead()) { if (not replies.wasGetRawMgmMeasurementRead() and not firstReplyCycle) {
sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl; sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl;
} }
uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement(); uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement();
@ -249,7 +255,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
status = result; status = result;
} }
if (not replies.wasCalibMgmMeasurementRead()) { if (not replies.wasCalibMgmMeasurementRead() and not firstReplyCycle) {
sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl; sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl;
} }
uint8_t* calibMgmMeasurement = replies.getCalibMgmMeasurement(); uint8_t* calibMgmMeasurement = replies.getCalibMgmMeasurement();
@ -264,15 +270,15 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
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());
} else { } else if (not firstReplyCycle) {
sif::warning << "IMTQ: Possible timing issue, start actuation dipole status was not read" sif::warning << "IMTQ: Possible timing issue, start actuation dipole status was not read"
<< std::endl; << std::endl;
} }
if (not replies.wasGetRawMgmMeasurementRead()) {
if (not replies.wasGetRawMgmMeasurementRead() and not firstReplyCycle) {
sif::warning << "IMTQ: Possible timing issue, was MGM measurement with torque was not read" sif::warning << "IMTQ: Possible timing issue, was MGM measurement with torque was not read"
<< std::endl; << std::endl;
} }
uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement(); uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement();
result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement); result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement);
if (result == returnvalue::OK) { if (result == returnvalue::OK) {
@ -280,11 +286,11 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
} else { } else {
status = result; status = result;
} }
if (not replies.wasEngHkRead()) {
if (not replies.wasEngHkRead() and not firstReplyCycle) {
sif::warning << "IMTQ: Possible timing issue, engineering HK with torque was not read" sif::warning << "IMTQ: Possible timing issue, engineering HK with torque was not read"
<< std::endl; << std::endl;
} }
uint8_t* engHkReply = replies.getEngHk(); uint8_t* engHkReply = replies.getEngHk();
result = parseStatusByte(imtq::CC::GET_ENG_HK_DATA, engHkReply); result = parseStatusByte(imtq::CC::GET_ENG_HK_DATA, engHkReply);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
@ -294,6 +300,9 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
} }
fillEngHkDataset(hkDatasetNoTorque, engHkReply); fillEngHkDataset(hkDatasetNoTorque, engHkReply);
} }
if (firstReplyCycle) {
firstReplyCycle = false;
}
return status; return status;
} }

View File

@ -115,6 +115,8 @@ class ImtqHandler : public DeviceHandlerBase {
uint8_t commandBuffer[imtq::MAX_COMMAND_SIZE]; uint8_t commandBuffer[imtq::MAX_COMMAND_SIZE];
bool goToNormalMode = false; bool goToNormalMode = false;
bool debugMode = false; bool debugMode = false;
bool specialRequestActive = false;
bool firstReplyCycle = true;
imtq::RequestType requestStep = imtq::RequestType::MEASURE_WITH_ACTUATION; imtq::RequestType requestStep = imtq::RequestType::MEASURE_WITH_ACTUATION;