added back self-test support
This commit is contained in:
@ -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);
|
||||
|
||||
Reference in New Issue
Block a user