2426 lines
126 KiB
C++
2426 lines
126 KiB
C++
#include <commonConfig.h>
|
|
#include <fsfw/datapool/PoolEntry.h>
|
|
#include <fsfw/datapool/PoolReadGuard.h>
|
|
#include <fsfw/datapoollocal/LocalDataPoolManager.h>
|
|
#include <fsfw/datapoollocal/LocalPoolVariable.h>
|
|
#include <fsfw/datapoollocal/ProvidesDataPoolSubscriptionIF.h>
|
|
#include <fsfw/datapoollocal/localPoolDefinitions.h>
|
|
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
|
|
#include <fsfw/globalfunctions/arrayprinter.h>
|
|
#include <fsfw/ipc/MutexFactory.h>
|
|
#include <fsfw/ipc/MutexGuard.h>
|
|
#include <fsfw/ipc/messageQueueDefinitions.h>
|
|
#include <fsfw/modes/ModeMessage.h>
|
|
#include <fsfw/objectmanager/SystemObjectIF.h>
|
|
#include <fsfw/power/definitions.h>
|
|
#include <fsfw/returnvalues/returnvalue.h>
|
|
#include <fsfw/serialize/SerializeAdapter.h>
|
|
#include <fsfw/serialize/SerializeIF.h>
|
|
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
|
|
#include <fsfw/tasks/TaskFactory.h>
|
|
#include <fsfw/timemanager/Countdown.h>
|
|
#include <fsfw/timemanager/clockDefinitions.h>
|
|
#include <mission/acs/ImtqHandler.h>
|
|
|
|
#include <cmath>
|
|
#include <fsfw/datapoollocal/LocalPoolVariable.tpp>
|
|
|
|
#include "mission/config/torquer.h"
|
|
|
|
static constexpr bool ACTUATION_WIRETAPPING = false;
|
|
|
|
ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
|
|
power::Switch_t pwrSwitcher, bool enableHkSets)
|
|
: DeviceHandlerBase(objectId, comIF, comCookie),
|
|
enableHkSets(enableHkSets),
|
|
statusSet(this),
|
|
dipoleSet(*this),
|
|
rawMtmNoTorque(this),
|
|
hkDatasetNoTorque(this),
|
|
rawMtmWithTorque(this),
|
|
hkDatasetWithTorque(this),
|
|
calMtmMeasurementSet(this),
|
|
posXselfTestDataset(this),
|
|
negXselfTestDataset(this),
|
|
posYselfTestDataset(this),
|
|
negYselfTestDataset(this),
|
|
posZselfTestDataset(this),
|
|
negZselfTestDataset(this),
|
|
switcher(pwrSwitcher) {
|
|
if (comCookie == nullptr) {
|
|
sif::error << "IMTQHandler: Invalid com cookie" << std::endl;
|
|
}
|
|
}
|
|
|
|
ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) {
|
|
uint8_t dhbOpCode = DeviceHandlerIF::PERFORM_OPERATION;
|
|
auto actuateStep = [&]() {
|
|
if (ignoreActForRestOfComSteps) {
|
|
requestStep = imtq::RequestType::DO_NOTHING;
|
|
} else {
|
|
requestStep = imtq::RequestType::ACTUATE;
|
|
}
|
|
};
|
|
switch (static_cast<imtq::ComStep>(opCode)) {
|
|
case (imtq::ComStep::DHB_OP): {
|
|
break;
|
|
}
|
|
case (imtq::ComStep::START_MEASURE_SEND): {
|
|
requestStep = imtq::RequestType::MEASURE_NO_ACTUATION;
|
|
dhbOpCode = DeviceHandlerIF::SEND_WRITE;
|
|
break;
|
|
}
|
|
case (imtq::ComStep::START_MEASURE_GET): {
|
|
requestStep = imtq::RequestType::MEASURE_NO_ACTUATION;
|
|
dhbOpCode = DeviceHandlerIF::GET_WRITE;
|
|
break;
|
|
}
|
|
case (imtq::ComStep::READ_MEASURE_SEND): {
|
|
requestStep = imtq::RequestType::MEASURE_NO_ACTUATION;
|
|
dhbOpCode = DeviceHandlerIF::SEND_READ;
|
|
break;
|
|
}
|
|
case (imtq::ComStep::READ_MEASURE_GET): {
|
|
requestStep = imtq::RequestType::MEASURE_NO_ACTUATION;
|
|
dhbOpCode = DeviceHandlerIF::GET_READ;
|
|
break;
|
|
}
|
|
case (imtq::ComStep::START_ACTUATE_SEND): {
|
|
if (manualTorqueCmdActive) {
|
|
if (manuallyCommandedTorqueDuration.isBusy()) {
|
|
ignoreActForRestOfComSteps = true;
|
|
requestStep = imtq::RequestType::DO_NOTHING;
|
|
} else {
|
|
manualTorqueCmdActive = false;
|
|
PoolReadGuard pg(&dipoleSet);
|
|
dipoleSet.dipoles[0] = 0;
|
|
dipoleSet.dipoles[1] = 0;
|
|
dipoleSet.dipoles[2] = 0;
|
|
dipoleSet.currentTorqueDurationMs = 0;
|
|
requestStep = imtq::RequestType::ACTUATE;
|
|
ignoreActForRestOfComSteps = false;
|
|
}
|
|
} else {
|
|
requestStep = imtq::RequestType::ACTUATE;
|
|
}
|
|
dhbOpCode = DeviceHandlerIF::SEND_WRITE;
|
|
break;
|
|
}
|
|
case (imtq::ComStep::START_ACTUATE_GET): {
|
|
actuateStep();
|
|
dhbOpCode = DeviceHandlerIF::GET_WRITE;
|
|
break;
|
|
}
|
|
case (imtq::ComStep::READ_ACTUATE_SEND): {
|
|
actuateStep();
|
|
dhbOpCode = DeviceHandlerIF::SEND_READ;
|
|
break;
|
|
}
|
|
case (imtq::ComStep::READ_ACTUATE_GET): {
|
|
actuateStep();
|
|
dhbOpCode = DeviceHandlerIF::GET_READ;
|
|
break;
|
|
}
|
|
default: {
|
|
sif::error << "ImtqHandler: Unexpected COM step" << std::endl;
|
|
break;
|
|
}
|
|
}
|
|
return DeviceHandlerBase::performOperation(dhbOpCode);
|
|
}
|
|
|
|
ImtqHandler::~ImtqHandler() = default;
|
|
|
|
void ImtqHandler::doStartUp() {
|
|
if (internalState != InternalState::STARTUP) {
|
|
commandExecuted = false;
|
|
updatePeriodicReply(true, imtq::cmdIds::REPLY_NO_TORQUE);
|
|
updatePeriodicReply(true, imtq::cmdIds::REPLY_WITH_TORQUE);
|
|
internalState = InternalState::STARTUP;
|
|
}
|
|
if (internalState == InternalState::STARTUP) {
|
|
if (commandExecuted) {
|
|
if (goToNormalMode) {
|
|
setMode(MODE_NORMAL);
|
|
} else {
|
|
setMode(_MODE_TO_ON);
|
|
}
|
|
commandExecuted = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
void ImtqHandler::doShutDown() {
|
|
if (internalState != InternalState::SHUTDOWN) {
|
|
commandExecuted = false;
|
|
internalState = InternalState::SHUTDOWN;
|
|
}
|
|
if (internalState == InternalState::SHUTDOWN and commandExecuted) {
|
|
updatePeriodicReply(false, imtq::cmdIds::REPLY_NO_TORQUE);
|
|
updatePeriodicReply(false, imtq::cmdIds::REPLY_WITH_TORQUE);
|
|
specialRequestActive = false;
|
|
firstReplyCycle = true;
|
|
internalState = InternalState::NONE;
|
|
commandExecuted = false;
|
|
statusSet.setValidity(false, true);
|
|
rawMtmNoTorque.setValidity(false, true);
|
|
rawMtmWithTorque.setValidity(false, true);
|
|
hkDatasetNoTorque.setValidity(false, true);
|
|
hkDatasetWithTorque.setValidity(false, true);
|
|
calMtmMeasurementSet.setValidity(false, true);
|
|
setMode(_MODE_POWER_DOWN);
|
|
}
|
|
}
|
|
|
|
ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
|
|
switch (requestStep) {
|
|
case (imtq::RequestType::MEASURE_NO_ACTUATION): {
|
|
*id = imtq::cmdIds::REQUEST;
|
|
return buildCommandFromCommand(*id, nullptr, 0);
|
|
}
|
|
case (imtq::RequestType::ACTUATE): {
|
|
*id = imtq::cmdIds::START_ACTUATION_DIPOLE;
|
|
return buildCommandFromCommand(*id, nullptr, 0);
|
|
}
|
|
default: {
|
|
*id = imtq::cmdIds::REQUEST;
|
|
request.requestType = imtq::RequestType::DO_NOTHING;
|
|
request.specialRequest = imtq::SpecialRequest::NONE;
|
|
expectedReply = DeviceHandlerIF::NO_COMMAND_ID;
|
|
rawPacket = reinterpret_cast<uint8_t*>(&request);
|
|
rawPacketLen = sizeof(imtq::Request);
|
|
return returnvalue::OK;
|
|
}
|
|
}
|
|
return NOTHING_TO_SEND;
|
|
}
|
|
|
|
ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
|
|
if (internalState == InternalState::STARTUP or internalState == InternalState::SHUTDOWN) {
|
|
*id = imtq::cmdIds::REQUEST;
|
|
return buildCommandFromCommand(*id, nullptr, 0);
|
|
}
|
|
return NOTHING_TO_SEND;
|
|
}
|
|
|
|
ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
|
|
const uint8_t* commandData,
|
|
size_t commandDataLen) {
|
|
auto genericSpecialRequest = [&](imtq::SpecialRequest specialRequest) {
|
|
request.requestType = imtq::RequestType::MEASURE_NO_ACTUATION;
|
|
request.specialRequest = specialRequest;
|
|
expectedReply = imtq::cmdIds::REPLY_NO_TORQUE;
|
|
specialRequestActive = true;
|
|
rawPacket = reinterpret_cast<uint8_t*>(&request);
|
|
rawPacketLen = sizeof(imtq::Request);
|
|
};
|
|
switch (deviceCommand) {
|
|
case (imtq::cmdIds::POS_X_SELF_TEST): {
|
|
genericSpecialRequest(imtq::SpecialRequest::DO_SELF_TEST_POS_X);
|
|
return returnvalue::OK;
|
|
}
|
|
case (imtq::cmdIds::NEG_X_SELF_TEST): {
|
|
genericSpecialRequest(imtq::SpecialRequest::DO_SELF_TEST_NEG_X);
|
|
return returnvalue::OK;
|
|
}
|
|
case (imtq::cmdIds::POS_Y_SELF_TEST): {
|
|
genericSpecialRequest(imtq::SpecialRequest::DO_SELF_TEST_POS_Y);
|
|
return returnvalue::OK;
|
|
}
|
|
case (imtq::cmdIds::NEG_Y_SELF_TEST): {
|
|
genericSpecialRequest(imtq::SpecialRequest::DO_SELF_TEST_NEG_Y);
|
|
return returnvalue::OK;
|
|
}
|
|
case (imtq::cmdIds::POS_Z_SELF_TEST): {
|
|
genericSpecialRequest(imtq::SpecialRequest::DO_SELF_TEST_POS_Z);
|
|
return returnvalue::OK;
|
|
}
|
|
case (imtq::cmdIds::NEG_Z_SELF_TEST): {
|
|
genericSpecialRequest(imtq::SpecialRequest::DO_SELF_TEST_NEG_Z);
|
|
return returnvalue::OK;
|
|
}
|
|
case (imtq::cmdIds::GET_SELF_TEST_RESULT): {
|
|
genericSpecialRequest(imtq::SpecialRequest::GET_SELF_TEST_RESULT);
|
|
return returnvalue::OK;
|
|
}
|
|
case (imtq::cmdIds::REQUEST): {
|
|
request.requestType = imtq::RequestType::MEASURE_NO_ACTUATION;
|
|
request.specialRequest = imtq::SpecialRequest::NONE;
|
|
// 6 ms integration time instead of 10 ms.
|
|
request.integrationTimeSel = 2;
|
|
expectedReply = imtq::cmdIds::REPLY_NO_TORQUE;
|
|
if (internalState == InternalState::SHUTDOWN) {
|
|
request.mode = acs::SimpleSensorMode::OFF;
|
|
} else {
|
|
request.mode = acs::SimpleSensorMode::NORMAL;
|
|
}
|
|
rawPacket = reinterpret_cast<uint8_t*>(&request);
|
|
rawPacketLen = sizeof(imtq::Request);
|
|
return returnvalue::OK;
|
|
}
|
|
case (imtq::cmdIds::START_ACTUATION_DIPOLE): {
|
|
if (commandData != nullptr && commandDataLen < 8) {
|
|
return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
|
|
}
|
|
{
|
|
// Do this in any case to read values which might be commanded by the ACS controller.
|
|
PoolReadGuard pg(&dipoleSet);
|
|
// Commands override anything which was set in the software
|
|
if (commandData != nullptr) {
|
|
dipoleSet.setValidityBufferGeneration(false);
|
|
ReturnValue_t result = dipoleSet.deSerialize(&commandData, &commandDataLen,
|
|
SerializeIF::Endianness::NETWORK);
|
|
dipoleSet.setValidityBufferGeneration(true);
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
manualTorqueCmdActive = true;
|
|
manuallyCommandedTorqueDuration.setTimeout(dipoleSet.currentTorqueDurationMs.value);
|
|
}
|
|
}
|
|
|
|
expectedReply = imtq::cmdIds::REPLY_WITH_TORQUE;
|
|
request.requestType = imtq::RequestType::ACTUATE;
|
|
request.specialRequest = imtq::SpecialRequest::NONE;
|
|
std::memcpy(request.dipoles, dipoleSet.dipoles.value, sizeof(request.dipoles));
|
|
request.torqueDuration = dipoleSet.currentTorqueDurationMs.value;
|
|
if (ACTUATION_WIRETAPPING) {
|
|
sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.dipoles[0]
|
|
<< ", y = " << dipoleSet.dipoles[1] << ", z = " << dipoleSet.dipoles[2]
|
|
<< ", duration = " << dipoleSet.currentTorqueDurationMs.value << std::endl;
|
|
}
|
|
MutexGuard mg(torquer::lazyLock(), torquer::LOCK_TYPE, torquer::LOCK_TIMEOUT,
|
|
torquer::LOCK_CTX);
|
|
torquer::TORQUEING = true;
|
|
torquer::TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value);
|
|
rawPacket = reinterpret_cast<uint8_t*>(&request);
|
|
rawPacketLen = sizeof(imtq::Request);
|
|
return returnvalue::OK;
|
|
}
|
|
default:
|
|
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
|
|
}
|
|
return returnvalue::FAILED;
|
|
}
|
|
|
|
void ImtqHandler::fillCommandAndReplyMap() {
|
|
insertInCommandMap(imtq::cmdIds::REQUEST);
|
|
insertInCommandMap(imtq::cmdIds::START_ACTUATION_DIPOLE);
|
|
insertInReplyMap(imtq::cmdIds::REPLY_NO_TORQUE, 5, nullptr, 0, true);
|
|
insertInReplyMap(imtq::cmdIds::REPLY_WITH_TORQUE, 20, nullptr, 0, true);
|
|
insertInCommandMap(imtq::cmdIds::POS_X_SELF_TEST);
|
|
insertInCommandMap(imtq::cmdIds::NEG_X_SELF_TEST);
|
|
insertInCommandMap(imtq::cmdIds::POS_Y_SELF_TEST);
|
|
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,
|
|
DeviceCommandId_t* foundId, size_t* foundLen) {
|
|
if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON or getMode() == _MODE_POWER_DOWN) {
|
|
return IGNORE_FULL_PACKET;
|
|
}
|
|
if (internalState == InternalState::SHUTDOWN) {
|
|
commandExecuted = true;
|
|
}
|
|
if (remainingSize > 0) {
|
|
*foundLen = remainingSize;
|
|
*foundId = expectedReply;
|
|
return returnvalue::OK;
|
|
}
|
|
return returnvalue::FAILED;
|
|
}
|
|
|
|
ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
|
|
ReturnValue_t result;
|
|
ReturnValue_t status = returnvalue::OK;
|
|
if (getMode() != MODE_NORMAL) {
|
|
if (expectedReply == imtq::cmdIds::REPLY_NO_TORQUE) {
|
|
ImtqRepliesDefault replies(packet);
|
|
if (replies.devWasConfigured() and internalState == InternalState::STARTUP) {
|
|
commandExecuted = true;
|
|
}
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
if (expectedReply == imtq::cmdIds::REPLY_NO_TORQUE) {
|
|
// sif::debug << "handle measure" << std::endl;
|
|
ImtqRepliesDefault replies(packet);
|
|
if (replies.devWasConfigured() and internalState == InternalState::STARTUP) {
|
|
commandExecuted = true;
|
|
}
|
|
if (specialRequestActive) {
|
|
if (replies.wasSpecialRequestRead()) {
|
|
uint8_t* specialRequest = replies.getSpecialRequest();
|
|
imtq::CC::CC cc = static_cast<imtq::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;
|
|
}
|
|
specialRequestActive = false;
|
|
}
|
|
if (not replies.wasEngHkRead() and not firstReplyCycle) {
|
|
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 (not replies.wasGetSystemStateRead() and not firstReplyCycle) {
|
|
sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl;
|
|
}
|
|
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.wasGetRawMgmMeasurementRead() and not firstReplyCycle) {
|
|
sif::warning << "IMTQ: Possible timing issue, raw MGM measurement was not read" << std::endl;
|
|
}
|
|
uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement();
|
|
result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement);
|
|
if (result == returnvalue::OK) {
|
|
fillRawMtmDataset(rawMtmNoTorque, rawMgmMeasurement);
|
|
} else {
|
|
status = result;
|
|
}
|
|
|
|
if (not replies.wasCalibMgmMeasurementRead() and not firstReplyCycle) {
|
|
sif::warning << "IMTQ: Possible timing issue, calib MGM measurement was not read"
|
|
<< std::endl;
|
|
}
|
|
uint8_t* calibMgmMeasurement = replies.getCalibMgmMeasurement();
|
|
result = parseStatusByte(imtq::CC::GET_CAL_MTM_MEASUREMENT, calibMgmMeasurement);
|
|
if (result == returnvalue::OK) {
|
|
fillCalibratedMtmDataset(calibMgmMeasurement);
|
|
} else {
|
|
status = result;
|
|
}
|
|
} else if (expectedReply == imtq::cmdIds::REPLY_WITH_TORQUE) {
|
|
// sif::debug << "handle measure with torque" << std::endl;
|
|
ImtqRepliesWithTorque replies(packet);
|
|
if (replies.wasDipoleActuationRead()) {
|
|
parseStatusByte(imtq::CC::START_ACTUATION_DIPOLE, replies.getDipoleActuation());
|
|
} else if (not firstReplyCycle) {
|
|
sif::warning << "IMTQ: Possible timing issue, start actuation dipole status was not read"
|
|
<< std::endl;
|
|
}
|
|
|
|
if (not replies.wasGetRawMgmMeasurementRead() and not firstReplyCycle) {
|
|
sif::warning << "IMTQ: Possible timing issue, was MGM measurement with torque was not read"
|
|
<< std::endl;
|
|
}
|
|
uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement();
|
|
result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement);
|
|
if (result == returnvalue::OK) {
|
|
fillRawMtmDataset(rawMtmWithTorque, rawMgmMeasurement);
|
|
} else {
|
|
status = result;
|
|
}
|
|
|
|
if (not replies.wasEngHkRead() and not firstReplyCycle) {
|
|
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(hkDatasetWithTorque, engHkReply);
|
|
if (firstReplyCycle) {
|
|
firstReplyCycle = false;
|
|
}
|
|
}
|
|
return status;
|
|
}
|
|
|
|
LocalPoolDataSetBase* ImtqHandler::getDataSetHandle(sid_t sid) {
|
|
if (sid == hkDatasetNoTorque.getSid()) {
|
|
return &hkDatasetNoTorque;
|
|
} else if (sid == dipoleSet.getSid()) {
|
|
return &dipoleSet;
|
|
} else if (sid == statusSet.getSid()) {
|
|
return &statusSet;
|
|
} else if (sid == hkDatasetWithTorque.getSid()) {
|
|
return &hkDatasetWithTorque;
|
|
} else if (sid == rawMtmWithTorque.getSid()) {
|
|
return &rawMtmWithTorque;
|
|
} else if (sid == calMtmMeasurementSet.getSid()) {
|
|
return &calMtmMeasurementSet;
|
|
} else if (sid == rawMtmNoTorque.getSid()) {
|
|
return &rawMtmNoTorque;
|
|
} else if (sid == posXselfTestDataset.getSid()) {
|
|
return &posXselfTestDataset;
|
|
} else if (sid == negXselfTestDataset.getSid()) {
|
|
return &negXselfTestDataset;
|
|
} else if (sid == posYselfTestDataset.getSid()) {
|
|
return &posYselfTestDataset;
|
|
} else if (sid == negYselfTestDataset.getSid()) {
|
|
return &negYselfTestDataset;
|
|
} else if (sid == posZselfTestDataset.getSid()) {
|
|
return &posZselfTestDataset;
|
|
} else if (sid == negZselfTestDataset.getSid()) {
|
|
return &negZselfTestDataset;
|
|
} else {
|
|
sif::error << "ImtqHandler::getDataSetHandle: Invalid SID" << std::endl;
|
|
return nullptr;
|
|
}
|
|
}
|
|
|
|
uint32_t ImtqHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; }
|
|
|
|
ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
|
|
LocalDataPoolManager& poolManager) {
|
|
/** Entries of housekeeping dataset */
|
|
localDataPoolMap.emplace(imtq::STATUS_BYTE_MODE, &statusMode);
|
|
localDataPoolMap.emplace(imtq::STATUS_BYTE_CONF, &statusConfig);
|
|
localDataPoolMap.emplace(imtq::STATUS_BYTE_ERROR, &statusError);
|
|
localDataPoolMap.emplace(imtq::STATUS_BYTE_UPTIME, &statusUptime);
|
|
|
|
// ENG HK No Torque
|
|
localDataPoolMap.emplace(imtq::DIGITAL_VOLTAGE_MV, new PoolEntry<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::ANALOG_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::COIL_CURRENTS, &coilCurrentsMilliampsNoTorque);
|
|
localDataPoolMap.emplace(imtq::COIL_TEMPERATURES, &coilTempsNoTorque);
|
|
localDataPoolMap.emplace(imtq::MCU_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
|
|
// ENG HK With Torque
|
|
localDataPoolMap.emplace(imtq::DIGITAL_VOLTAGE_MV_WT, new PoolEntry<uint16_t>({0}));
|
|
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);
|
|
|
|
/** Entries of calibrated MTM measurement dataset */
|
|
localDataPoolMap.emplace(imtq::MGM_CAL_NT, &mgmCalEntry);
|
|
localDataPoolMap.emplace(imtq::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>({0}));
|
|
|
|
/** Entries of raw MTM measurement dataset */
|
|
localDataPoolMap.emplace(imtq::MTM_RAW, &mtmRawNoTorque);
|
|
localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS, &actStatusNoTorque);
|
|
|
|
localDataPoolMap.emplace(imtq::MTM_RAW_WT, &mtmRawWithTorque);
|
|
localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS_WT, &actStatusWithTorque);
|
|
|
|
/** INIT measurements for positive X axis test */
|
|
localDataPoolMap.emplace(imtq::INIT_POS_X_ERR, new PoolEntry<uint8_t>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_X_RAW_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_X_RAW_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_X_RAW_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_X_CAL_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_X_CAL_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_X_CAL_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_X_COIL_X_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_X_COIL_Y_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_X_COIL_Z_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_X_COIL_X_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_X_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_X_COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
|
|
/** INIT measurements for negative X axis test */
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_X_ERR, new PoolEntry<uint8_t>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_X_RAW_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_X_RAW_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_X_RAW_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_X_CAL_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_X_CAL_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_X_CAL_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_X_COIL_X_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_X_COIL_Y_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_X_COIL_Z_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_X_COIL_X_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_X_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_X_COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
|
|
/** INIT measurements for positive Y axis test */
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Y_ERR, new PoolEntry<uint8_t>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Y_RAW_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Y_RAW_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Y_RAW_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Y_CAL_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Y_CAL_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Y_CAL_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Y_COIL_X_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Y_COIL_Y_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Y_COIL_Z_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Y_COIL_X_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Y_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Y_COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
|
|
/** INIT measurements for negative Y axis test */
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Y_ERR, new PoolEntry<uint8_t>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Y_RAW_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Y_RAW_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Y_RAW_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Y_CAL_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Y_CAL_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Y_CAL_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Y_COIL_X_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Y_COIL_Y_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Y_COIL_Z_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Y_COIL_X_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Y_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Y_COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
|
|
/** INIT measurements for positive Z axis test */
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Z_ERR, new PoolEntry<uint8_t>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Z_RAW_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Z_RAW_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Z_RAW_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Z_CAL_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Z_CAL_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Z_CAL_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Z_COIL_X_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Z_COIL_Y_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Z_COIL_Z_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Z_COIL_X_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Z_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_POS_Z_COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
|
|
/** INIT measurements for negative Z axis test */
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Z_ERR, new PoolEntry<uint8_t>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Z_RAW_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Z_RAW_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Z_RAW_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Z_CAL_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Z_CAL_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Z_CAL_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Z_COIL_X_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Z_COIL_Y_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Z_COIL_Z_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Z_COIL_X_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Z_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::INIT_NEG_Z_COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
|
|
localDataPoolMap.emplace(imtq::POS_X_ERR, new PoolEntry<uint8_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_X_RAW_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_X_RAW_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_X_RAW_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_X_CAL_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_X_CAL_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_X_CAL_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_X_COIL_X_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_X_COIL_Y_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_X_COIL_Z_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_X_COIL_X_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_X_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_X_COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
|
|
localDataPoolMap.emplace(imtq::NEG_X_ERR, new PoolEntry<uint8_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_X_RAW_MAG_X, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_X_RAW_MAG_Y, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_X_RAW_MAG_Z, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_X_CAL_MAG_X, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_X_CAL_MAG_Y, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_X_CAL_MAG_Z, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_X_COIL_X_CURRENT, new PoolEntry<uint16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_X_COIL_Y_CURRENT, new PoolEntry<uint16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_X_COIL_Z_CURRENT, new PoolEntry<uint16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_X_COIL_X_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_X_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_X_COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
|
|
localDataPoolMap.emplace(imtq::POS_Y_ERR, new PoolEntry<uint8_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_Y_RAW_MAG_X, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_Y_RAW_MAG_Y, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_Y_RAW_MAG_Z, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_Y_CAL_MAG_X, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_Y_CAL_MAG_Y, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_Y_CAL_MAG_Z, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_Y_COIL_X_CURRENT, new PoolEntry<uint16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_Y_COIL_Y_CURRENT, new PoolEntry<uint16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_Y_COIL_Z_CURRENT, new PoolEntry<uint16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_Y_COIL_X_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_Y_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_Y_COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
|
|
localDataPoolMap.emplace(imtq::NEG_Y_ERR, new PoolEntry<uint8_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_Y_RAW_MAG_X, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_Y_RAW_MAG_Y, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_Y_RAW_MAG_Z, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_Y_CAL_MAG_X, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_Y_CAL_MAG_Y, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_Y_CAL_MAG_Z, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_Y_COIL_X_CURRENT, new PoolEntry<uint16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_Y_COIL_Y_CURRENT, new PoolEntry<uint16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_Y_COIL_Z_CURRENT, new PoolEntry<uint16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_Y_COIL_X_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_Y_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_Y_COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
|
|
localDataPoolMap.emplace(imtq::POS_Z_ERR, new PoolEntry<uint8_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_Z_RAW_MAG_X, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_Z_RAW_MAG_Y, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_Z_RAW_MAG_Z, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_Z_CAL_MAG_X, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_Z_CAL_MAG_Y, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_Z_CAL_MAG_Z, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_Z_COIL_X_CURRENT, new PoolEntry<uint16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_Z_COIL_Y_CURRENT, new PoolEntry<uint16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_Z_COIL_Z_CURRENT, new PoolEntry<uint16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_Z_COIL_X_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_Z_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::POS_Z_COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
|
|
localDataPoolMap.emplace(imtq::NEG_Z_ERR, new PoolEntry<uint8_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_Z_RAW_MAG_X, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_Z_RAW_MAG_Y, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_Z_RAW_MAG_Z, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_Z_CAL_MAG_X, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_Z_CAL_MAG_Y, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_Z_CAL_MAG_Z, new PoolEntry<uint32_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_Z_COIL_X_CURRENT, new PoolEntry<uint16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_Z_COIL_Y_CURRENT, new PoolEntry<uint16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_Z_COIL_Z_CURRENT, new PoolEntry<uint16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_Z_COIL_X_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_Z_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::NEG_Z_COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
|
|
/** FINA measurements for positive X axis test */
|
|
localDataPoolMap.emplace(imtq::FINA_POS_X_ERR, new PoolEntry<uint8_t>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_X_RAW_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_X_RAW_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_X_RAW_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_X_CAL_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_X_CAL_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_X_CAL_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_X_COIL_X_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_X_COIL_Y_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_X_COIL_Z_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_X_COIL_X_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_X_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_X_COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
|
|
/** FINA measurements for negative X axis test */
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_X_ERR, new PoolEntry<uint8_t>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_X_RAW_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_X_RAW_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_X_RAW_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_X_CAL_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_X_CAL_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_X_CAL_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_X_COIL_X_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_X_COIL_Y_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_X_COIL_Z_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_X_COIL_X_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_X_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_X_COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
|
|
/** FINA measurements for positive Y axis test */
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Y_ERR, new PoolEntry<uint8_t>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Y_RAW_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Y_RAW_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Y_RAW_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Y_CAL_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Y_CAL_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Y_CAL_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Y_COIL_X_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Y_COIL_Y_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Y_COIL_Z_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Y_COIL_X_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Y_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Y_COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
|
|
/** FINA measurements for negative Y axis test */
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Y_ERR, new PoolEntry<uint8_t>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Y_RAW_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Y_RAW_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Y_RAW_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Y_CAL_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Y_CAL_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Y_CAL_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Y_COIL_X_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Y_COIL_Y_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Y_COIL_Z_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Y_COIL_X_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Y_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Y_COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
|
|
/** FINA measurements for positive Z axis test */
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Z_ERR, new PoolEntry<uint8_t>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Z_RAW_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Z_RAW_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Z_RAW_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Z_CAL_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Z_CAL_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Z_CAL_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Z_COIL_X_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Z_COIL_Y_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Z_COIL_Z_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Z_COIL_X_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Z_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_POS_Z_COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
|
|
/** FINA measurements for negative Z axis test */
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Z_ERR, new PoolEntry<uint8_t>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Z_RAW_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Z_RAW_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Z_RAW_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Z_CAL_MAG_X, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Z_CAL_MAG_Y, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Z_CAL_MAG_Z, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Z_COIL_X_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Z_COIL_Y_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Z_COIL_Z_CURRENT, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Z_COIL_X_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Z_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(imtq::FINA_NEG_Z_COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
|
|
|
poolManager.subscribeForDiagPeriodicPacket(
|
|
subdp::DiagnosticsHkPeriodicParams(hkDatasetNoTorque.getSid(), enableHkSets, 60.0));
|
|
poolManager.subscribeForDiagPeriodicPacket(
|
|
subdp::DiagnosticsHkPeriodicParams(hkDatasetWithTorque.getSid(), enableHkSets, 60.0));
|
|
poolManager.subscribeForDiagPeriodicPacket(
|
|
subdp::DiagnosticsHkPeriodicParams(rawMtmNoTorque.getSid(), false, 10.0));
|
|
poolManager.subscribeForDiagPeriodicPacket(
|
|
subdp::DiagnosticsHkPeriodicParams(rawMtmWithTorque.getSid(), false, 10.0));
|
|
poolManager.subscribeForDiagPeriodicPacket(
|
|
subdp::DiagnosticsHkPeriodicParams(calMtmMeasurementSet.getSid(), false, 10.0));
|
|
poolManager.subscribeForRegularPeriodicPacket(
|
|
subdp::RegularHkPeriodicParams(statusSet.getSid(), false, 10.0));
|
|
poolManager.subscribeForDiagPeriodicPacket(
|
|
subdp::DiagnosticsHkPeriodicParams(dipoleSet.getSid(), false, 10.0));
|
|
return DeviceHandlerBase::initializeLocalDataPool(localDataPoolMap, poolManager);
|
|
}
|
|
|
|
ReturnValue_t ImtqHandler::getSelfTestCommandId(DeviceCommandId_t* id) {
|
|
DeviceCommandId_t commandId = getPendingCommand();
|
|
switch (commandId) {
|
|
case imtq::cmdIds::POS_X_SELF_TEST:
|
|
case imtq::cmdIds::NEG_X_SELF_TEST:
|
|
case imtq::cmdIds::POS_Y_SELF_TEST:
|
|
case imtq::cmdIds::NEG_Y_SELF_TEST:
|
|
case imtq::cmdIds::POS_Z_SELF_TEST:
|
|
case imtq::cmdIds::NEG_Z_SELF_TEST:
|
|
*id = commandId;
|
|
break;
|
|
default:
|
|
sif::error << "IMTQHandler::getSelfTestCommandId: Reply does not match to pending "
|
|
<< "command" << std::endl;
|
|
// return UNEXPECTED_SELF_TEST_REPLY;
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t ImtqHandler::parseStatusByte(imtq::CC::CC command, const uint8_t* packet) {
|
|
uint8_t cmdErrorField = packet[1] & 0xF;
|
|
if (cmdErrorField == 0) {
|
|
return returnvalue::OK;
|
|
}
|
|
sif::error << std::hex;
|
|
switch (cmdErrorField) {
|
|
case 1:
|
|
sif::error << "IMTQ::parseStatusByte: Command 0x" << std::setw(2) << command
|
|
<< " rejected without reason" << std::endl;
|
|
return imtq::REJECTED_WITHOUT_REASON;
|
|
case 2:
|
|
sif::error << "IMTQ::parseStatusByte: Command 0x" << std::setw(2) << command
|
|
<< " has invalid command code" << std::endl;
|
|
return imtq::INVALID_COMMAND_CODE;
|
|
case 3:
|
|
sif::error << "IMTQ::parseStatusByte: Command 0x" << std::setw(2) << command
|
|
<< " has missing parameter" << std::endl;
|
|
return imtq::PARAMETER_MISSING;
|
|
case 4:
|
|
sif::error << "IMTQ::parseStatusByte: Command 0x" << std::setw(2) << command
|
|
<< " has invalid parameter" << std::endl;
|
|
return imtq::PARAMETER_INVALID;
|
|
case 5:
|
|
sif::error << "IMTQ::parseStatusByte: CC 0x" << std::setw(2) << command << " unavailable"
|
|
<< std::endl;
|
|
return imtq::CC_UNAVAILABLE;
|
|
case 7:
|
|
sif::error << "IMTQ::parseStatusByte: Internal processing error for command 0x"
|
|
<< std::setw(2) << command << std::endl;
|
|
return imtq::INTERNAL_PROCESSING_ERROR;
|
|
default:
|
|
sif::error << "IMTQ::parseStatusByte: CMD error field for command 0x" << std::setw(2)
|
|
<< command << " contains unknown error code 0x" << static_cast<int>(cmdErrorField)
|
|
<< std::endl;
|
|
return imtq::CMD_ERR_UNKNOWN;
|
|
}
|
|
sif::error << std::dec;
|
|
}
|
|
|
|
void ImtqHandler::fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* packet) {
|
|
PoolReadGuard rg(&hkDataset);
|
|
uint8_t offset = 2;
|
|
hkDataset.digitalVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
hkDataset.analogVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
hkDataset.digitalCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
hkDataset.analogCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
hkDataset.coilCurrentsMilliamps[0] =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
hkDataset.coilCurrentsMilliamps[1] =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
hkDataset.coilCurrentsMilliamps[2] =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
hkDataset.coilTemperatures[0] = (*(packet + offset + 1) << 8 | *(packet + offset));
|
|
offset += 2;
|
|
hkDataset.coilTemperatures[1] = (*(packet + offset + 1) << 8 | *(packet + offset));
|
|
offset += 2;
|
|
hkDataset.coilTemperatures[2] = (*(packet + offset + 1) << 8 | *(packet + offset));
|
|
offset += 2;
|
|
size_t dummy = 2;
|
|
SerializeAdapter::deSerialize(&hkDataset.mcuTemperature.value, packet + offset, &dummy,
|
|
SerializeIF::Endianness::LITTLE);
|
|
|
|
hkDataset.setValidity(true, true);
|
|
|
|
if (debugMode) {
|
|
#if OBSW_VERBOSE_LEVEL >= 1
|
|
sif::info << "IMTQ digital voltage: " << hkDataset.digitalVoltageMv << " 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 analog current: " << hkDataset.analogCurrentmA << " mA" << std::endl;
|
|
sif::info << "IMTQ coil X current: " << hkDataset.coilCurrentsMilliamps[0] << " mA"
|
|
<< std::endl;
|
|
sif::info << "IMTQ coil Y current: " << hkDataset.coilCurrentsMilliamps[1] << " mA"
|
|
<< std::endl;
|
|
sif::info << "IMTQ coil Z current: " << hkDataset.coilCurrentsMilliamps[2] << " mA"
|
|
<< std::endl;
|
|
sif::info << "IMTQ coil X temperature: " << hkDataset.coilTemperatures[0] << " °C" << std::endl;
|
|
sif::info << "IMTQ coil Y temperature: " << hkDataset.coilTemperatures[1] << " °C" << std::endl;
|
|
sif::info << "IMTQ coil Z temperature: " << hkDataset.coilTemperatures[2] << " °C" << std::endl;
|
|
sif::info << "IMTQ coil MCU temperature: " << hkDataset.mcuTemperature << " °C" << std::endl;
|
|
#endif
|
|
}
|
|
}
|
|
|
|
void ImtqHandler::setToGoToNormal(bool enable) { this->goToNormalMode = enable; }
|
|
|
|
void ImtqHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
|
|
PoolReadGuard rg(&calMtmMeasurementSet);
|
|
calMtmMeasurementSet.setValidity(true, true);
|
|
int8_t offset = 2;
|
|
calMtmMeasurementSet.mgmXyz[0] = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
calMtmMeasurementSet.mgmXyz[1] = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
calMtmMeasurementSet.mgmXyz[2] = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
calMtmMeasurementSet.coilActuationStatus = (*(packet + offset + 3) << 24) |
|
|
(*(packet + offset + 2) << 16) |
|
|
(*(packet + offset + 1) << 8) | (*(packet + offset));
|
|
if (debugMode) {
|
|
#if OBSW_VERBOSE_LEVEL >= 1
|
|
sif::info << "IMTQ calibrated MTM measurement X: " << calMtmMeasurementSet.mgmXyz[0] << " nT"
|
|
<< std::endl;
|
|
sif::info << "IMTQ calibrated MTM measurement Y: " << calMtmMeasurementSet.mgmXyz[1] << " nT"
|
|
<< std::endl;
|
|
sif::info << "IMTQ calibrated MTM measurement Z: " << calMtmMeasurementSet.mgmXyz[2] << " nT"
|
|
<< std::endl;
|
|
sif::info << "IMTQ coil actuation status during MTM measurement: "
|
|
<< (unsigned int)calMtmMeasurementSet.coilActuationStatus.value << std::endl;
|
|
#endif
|
|
}
|
|
}
|
|
|
|
void ImtqHandler::fillRawMtmDataset(imtq::RawMtmMeasurementSet& set, const uint8_t* packet) {
|
|
PoolReadGuard rg(&set);
|
|
if (rg.getReadResult() != returnvalue::OK) {
|
|
sif::error << "ImtqHandler::fillRawMtmDataset: Read failure" << std::endl;
|
|
}
|
|
unsigned int offset = 2;
|
|
size_t deSerLen = 16;
|
|
const uint8_t* dataStart = packet + offset;
|
|
int32_t xRaw = 0;
|
|
int32_t yRaw = 0;
|
|
int32_t zRaw = 0;
|
|
uint32_t coilActStatus = 0;
|
|
auto res =
|
|
SerializeAdapter::deSerialize(&xRaw, &dataStart, &deSerLen, SerializeIF::Endianness::LITTLE);
|
|
if (res != returnvalue::OK) {
|
|
return;
|
|
}
|
|
res =
|
|
SerializeAdapter::deSerialize(&yRaw, &dataStart, &deSerLen, SerializeIF::Endianness::LITTLE);
|
|
if (res != returnvalue::OK) {
|
|
return;
|
|
}
|
|
res =
|
|
SerializeAdapter::deSerialize(&zRaw, &dataStart, &deSerLen, SerializeIF::Endianness::LITTLE);
|
|
if (res != returnvalue::OK) {
|
|
return;
|
|
}
|
|
res = SerializeAdapter::deSerialize(&coilActStatus, &dataStart, &deSerLen,
|
|
SerializeIF::Endianness::LITTLE);
|
|
if (res != returnvalue::OK) {
|
|
return;
|
|
}
|
|
set.mtmRawNt[0] = static_cast<float>(xRaw) * 7.5;
|
|
set.mtmRawNt[1] = static_cast<float>(yRaw) * 7.5;
|
|
set.mtmRawNt[2] = static_cast<float>(zRaw) * 7.5;
|
|
set.coilActuationStatus = static_cast<uint8_t>(coilActStatus);
|
|
set.setValidity(true, true);
|
|
if (debugMode) {
|
|
#if OBSW_VERBOSE_LEVEL >= 1
|
|
sif::info << "Set ID: " << set.getSid().ownerSetId << std::endl;
|
|
sif::info << "IMTQ raw MTM measurement X: " << set.mtmRawNt[0] << " nT" << std::endl;
|
|
sif::info << "IMTQ raw MTM measurement Y: " << set.mtmRawNt[1] << " nT" << std::endl;
|
|
sif::info << "IMTQ raw MTM measurement Z: " << set.mtmRawNt[2] << " nT" << std::endl;
|
|
sif::info << "IMTQ coil actuation status during MTM measurement: "
|
|
<< (unsigned int)set.coilActuationStatus.value << std::endl;
|
|
#endif
|
|
}
|
|
}
|
|
|
|
void ImtqHandler::handleSelfTestReply(const uint8_t* packet) {
|
|
uint16_t offset = 2;
|
|
checkErrorByte(*(packet + offset), *(packet + offset + 1));
|
|
|
|
switch (*(packet + imtq::MAIN_STEP_OFFSET)) {
|
|
case imtq::selfTest::step::X_POSITIVE: {
|
|
handlePositiveXSelfTestReply(packet);
|
|
break;
|
|
}
|
|
case imtq::selfTest::step::X_NEGATIVE: {
|
|
handleNegativeXSelfTestReply(packet);
|
|
break;
|
|
}
|
|
case imtq::selfTest::step::Y_POSITIVE: {
|
|
handlePositiveYSelfTestReply(packet);
|
|
break;
|
|
}
|
|
case imtq::selfTest::step::Y_NEGATIVE: {
|
|
handleNegativeYSelfTestReply(packet);
|
|
break;
|
|
}
|
|
case imtq::selfTest::step::Z_POSITIVE: {
|
|
handlePositiveZSelfTestReply(packet);
|
|
break;
|
|
}
|
|
case imtq::selfTest::step::Z_NEGATIVE: {
|
|
handleNegativeZSelfTestReply(packet);
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
void ImtqHandler::handlePositiveXSelfTestReply(const uint8_t* packet) {
|
|
PoolReadGuard rg(&posXselfTestDataset);
|
|
|
|
uint16_t offset = 2;
|
|
/** Init measurements */
|
|
posXselfTestDataset.initErr = *(packet + offset);
|
|
offset += 2; // STEP byte will not be stored
|
|
posXselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posXselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posXselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posXselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posXselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posXselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posXselfTestDataset.initCoilXCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posXselfTestDataset.initCoilYCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posXselfTestDataset.initCoilZCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posXselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
posXselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
posXselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
|
|
/** +X measurements */
|
|
checkErrorByte(*(packet + offset), *(packet + offset + 1));
|
|
posXselfTestDataset.err = *(packet + offset);
|
|
offset += 2; // STEP byte will not be stored
|
|
posXselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posXselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posXselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posXselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posXselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posXselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posXselfTestDataset.coilXCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posXselfTestDataset.coilYCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posXselfTestDataset.coilZCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posXselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
posXselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
posXselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
|
|
/** FINA measurements */
|
|
checkErrorByte(*(packet + offset), *(packet + offset + 1));
|
|
posXselfTestDataset.finaErr = *(packet + offset);
|
|
offset += 2; // STEP byte will not be stored
|
|
posXselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posXselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posXselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posXselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posXselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posXselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posXselfTestDataset.finaCoilXCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posXselfTestDataset.finaCoilYCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posXselfTestDataset.finaCoilZCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posXselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
posXselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
posXselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
|
|
if (debugMode) {
|
|
#if OBSW_VERBOSE_LEVEL >= 1
|
|
sif::info << "IMTQ self test (INIT) err: "
|
|
<< static_cast<unsigned int>(posXselfTestDataset.initErr.value) << std::endl;
|
|
sif::info << "IMTQ self test (INIT) raw magnetic field X: " << posXselfTestDataset.initRawMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << posXselfTestDataset.initRawMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << posXselfTestDataset.initRawMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) calibrated magnetic field X: "
|
|
<< posXselfTestDataset.initCalMagX << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: "
|
|
<< posXselfTestDataset.initCalMagY << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: "
|
|
<< posXselfTestDataset.initCalMagZ << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil X current: " << posXselfTestDataset.initCoilXCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Y current: " << posXselfTestDataset.initCoilYCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Z current: " << posXselfTestDataset.initCoilZCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil X temperature: "
|
|
<< posXselfTestDataset.initCoilXTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Y temperature: "
|
|
<< posXselfTestDataset.initCoilYTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Z temperature: "
|
|
<< posXselfTestDataset.initCoilZTemperature << " °C" << std::endl;
|
|
|
|
sif::info << "IMTQ self test (+X) err: "
|
|
<< static_cast<unsigned int>(posXselfTestDataset.err.value) << std::endl;
|
|
sif::info << "IMTQ self test (+X) raw magnetic field X: " << posXselfTestDataset.rawMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (+X) raw magnetic field Y: " << posXselfTestDataset.rawMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (+X) raw magnetic field Z: " << posXselfTestDataset.rawMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (+X) calibrated magnetic field X: " << posXselfTestDataset.calMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (+X) calibrated magnetic field Y: " << posXselfTestDataset.calMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (+X) calibrated magnetic field Z: " << posXselfTestDataset.calMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (+X) coil X current: " << posXselfTestDataset.coilXCurrent << " mA"
|
|
<< std::endl;
|
|
sif::info << "IMTQ self test (+X) coil Y current: " << posXselfTestDataset.coilYCurrent << " mA"
|
|
<< std::endl;
|
|
sif::info << "IMTQ self test (+X) coil Z current: " << posXselfTestDataset.coilZCurrent << " mA"
|
|
<< std::endl;
|
|
sif::info << "IMTQ self test (+X) coil X temperature: " << posXselfTestDataset.coilXTemperature
|
|
<< " °C" << std::endl;
|
|
sif::info << "IMTQ self test (+X) coil Y temperature: " << posXselfTestDataset.coilYTemperature
|
|
<< " °C" << std::endl;
|
|
sif::info << "IMTQ self test (+X) coil Z temperature: " << posXselfTestDataset.coilZTemperature
|
|
<< " °C" << std::endl;
|
|
|
|
sif::info << "IMTQ self test (FINA) err: "
|
|
<< static_cast<unsigned int>(posXselfTestDataset.finaErr.value) << std::endl;
|
|
sif::info << "IMTQ self test (FINA) raw magnetic field X: " << posXselfTestDataset.finaRawMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << posXselfTestDataset.finaRawMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << posXselfTestDataset.finaRawMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) calibrated magnetic field X: "
|
|
<< posXselfTestDataset.finaCalMagX << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: "
|
|
<< posXselfTestDataset.finaCalMagY << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: "
|
|
<< posXselfTestDataset.finaCalMagZ << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil X current: " << posXselfTestDataset.finaCoilXCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Y current: " << posXselfTestDataset.finaCoilYCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Z current: " << posXselfTestDataset.finaCoilZCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil X temperature: "
|
|
<< posXselfTestDataset.finaCoilXTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Y temperature: "
|
|
<< posXselfTestDataset.finaCoilYTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Z temperature: "
|
|
<< posXselfTestDataset.finaCoilZTemperature << " °C" << std::endl;
|
|
#endif
|
|
}
|
|
}
|
|
|
|
void ImtqHandler::handleNegativeXSelfTestReply(const uint8_t* packet) {
|
|
PoolReadGuard rg(&posXselfTestDataset);
|
|
|
|
uint16_t offset = 2;
|
|
/** Init measurements */
|
|
negXselfTestDataset.initErr = *(packet + offset);
|
|
offset += 2; // STEP byte will not be stored
|
|
negXselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negXselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negXselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negXselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negXselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negXselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negXselfTestDataset.initCoilXCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negXselfTestDataset.initCoilYCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negXselfTestDataset.initCoilZCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negXselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
negXselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
negXselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
|
|
/** +X measurements */
|
|
checkErrorByte(*(packet + offset), *(packet + offset + 1));
|
|
negXselfTestDataset.err = *(packet + offset);
|
|
offset += 2; // STEP byte will not be stored
|
|
negXselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negXselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negXselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negXselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negXselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negXselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negXselfTestDataset.coilXCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negXselfTestDataset.coilYCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negXselfTestDataset.coilZCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negXselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
negXselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
negXselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
|
|
/** FINA measurements */
|
|
checkErrorByte(*(packet + offset), *(packet + offset + 1));
|
|
negXselfTestDataset.finaErr = *(packet + offset);
|
|
offset += 2; // STEP byte will not be stored
|
|
negXselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negXselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negXselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negXselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negXselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negXselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negXselfTestDataset.finaCoilXCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negXselfTestDataset.finaCoilYCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negXselfTestDataset.finaCoilZCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negXselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
negXselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
negXselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
|
|
if (debugMode) {
|
|
#if OBSW_VERBOSE_LEVEL >= 1
|
|
sif::info << "IMTQ self test (INIT) err: "
|
|
<< static_cast<unsigned int>(negXselfTestDataset.initErr.value) << std::endl;
|
|
sif::info << "IMTQ self test (INIT) raw magnetic field X: " << negXselfTestDataset.initRawMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << negXselfTestDataset.initRawMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << negXselfTestDataset.initRawMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) calibrated magnetic field X: "
|
|
<< negXselfTestDataset.initCalMagX << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: "
|
|
<< negXselfTestDataset.initCalMagY << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: "
|
|
<< negXselfTestDataset.initCalMagZ << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil X current: " << negXselfTestDataset.initCoilXCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Y current: " << negXselfTestDataset.initCoilYCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Z current: " << negXselfTestDataset.initCoilZCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil X temperature: "
|
|
<< negXselfTestDataset.initCoilXTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Y temperature: "
|
|
<< negXselfTestDataset.initCoilYTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Z temperature: "
|
|
<< negXselfTestDataset.initCoilZTemperature << " °C" << std::endl;
|
|
|
|
sif::info << "IMTQ self test (-X) err: "
|
|
<< static_cast<unsigned int>(negXselfTestDataset.err.value) << std::endl;
|
|
sif::info << "IMTQ self test (-X) raw magnetic field X: " << negXselfTestDataset.rawMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (-X) raw magnetic field Y: " << negXselfTestDataset.rawMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (-X) raw magnetic field Z: " << negXselfTestDataset.rawMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (-X) calibrated magnetic field X: " << negXselfTestDataset.calMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (-X) calibrated magnetic field Y: " << negXselfTestDataset.calMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (-X) calibrated magnetic field Z: " << negXselfTestDataset.calMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (-X) coil X current: " << negXselfTestDataset.coilXCurrent << " mA"
|
|
<< std::endl;
|
|
sif::info << "IMTQ self test (-X) coil Y current: " << negXselfTestDataset.coilYCurrent << " mA"
|
|
<< std::endl;
|
|
sif::info << "IMTQ self test (-X) coil Z current: " << negXselfTestDataset.coilZCurrent << " mA"
|
|
<< std::endl;
|
|
sif::info << "IMTQ self test (-X) coil X temperature: " << negXselfTestDataset.coilXTemperature
|
|
<< " °C" << std::endl;
|
|
sif::info << "IMTQ self test (-X) coil Y temperature: " << negXselfTestDataset.coilYTemperature
|
|
<< " °C" << std::endl;
|
|
sif::info << "IMTQ self test (-X) coil Z temperature: " << negXselfTestDataset.coilZTemperature
|
|
<< " °C" << std::endl;
|
|
|
|
sif::info << "IMTQ self test (FINA) err: "
|
|
<< static_cast<unsigned int>(negXselfTestDataset.finaErr.value) << std::endl;
|
|
sif::info << "IMTQ self test (FINA) raw magnetic field X: " << negXselfTestDataset.finaRawMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << negXselfTestDataset.finaRawMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << negXselfTestDataset.finaRawMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) calibrated magnetic field X: "
|
|
<< negXselfTestDataset.finaCalMagX << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: "
|
|
<< negXselfTestDataset.finaCalMagY << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: "
|
|
<< negXselfTestDataset.finaCalMagZ << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil X current: " << negXselfTestDataset.finaCoilXCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Y current: " << negXselfTestDataset.finaCoilYCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Z current: " << negXselfTestDataset.finaCoilZCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil X temperature: "
|
|
<< negXselfTestDataset.finaCoilXTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Y temperature: "
|
|
<< negXselfTestDataset.finaCoilYTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Z temperature: "
|
|
<< negXselfTestDataset.finaCoilZTemperature << " °C" << std::endl;
|
|
#endif
|
|
}
|
|
}
|
|
|
|
void ImtqHandler::handlePositiveYSelfTestReply(const uint8_t* packet) {
|
|
PoolReadGuard rg(&posXselfTestDataset);
|
|
|
|
uint16_t offset = 2;
|
|
/** Init measurements */
|
|
posYselfTestDataset.initErr = *(packet + offset);
|
|
offset += 2; // STEP byte will not be stored
|
|
posYselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posYselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posYselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posYselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posYselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posYselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posYselfTestDataset.initCoilXCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posYselfTestDataset.initCoilYCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posYselfTestDataset.initCoilZCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posYselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
posYselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
posYselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
|
|
/** +X measurements */
|
|
checkErrorByte(*(packet + offset), *(packet + offset + 1));
|
|
posYselfTestDataset.err = *(packet + offset);
|
|
offset += 2; // STEP byte will not be stored
|
|
posYselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posYselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posYselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posYselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posYselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posYselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posYselfTestDataset.coilXCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posYselfTestDataset.coilYCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posYselfTestDataset.coilZCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posYselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
posYselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
posYselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
|
|
/** FINA measurements */
|
|
checkErrorByte(*(packet + offset), *(packet + offset + 1));
|
|
posYselfTestDataset.finaErr = *(packet + offset);
|
|
offset += 2; // STEP byte will not be stored
|
|
posYselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posYselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posYselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posYselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posYselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posYselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posYselfTestDataset.finaCoilXCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posYselfTestDataset.finaCoilYCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posYselfTestDataset.finaCoilZCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posYselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
posYselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
posYselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
|
|
if (debugMode) {
|
|
#if OBSW_VERBOSE_LEVEL >= 1
|
|
sif::info << "IMTQ self test (INIT) err: "
|
|
<< static_cast<unsigned int>(posYselfTestDataset.initErr.value) << std::endl;
|
|
sif::info << "IMTQ self test (INIT) raw magnetic field X: " << posYselfTestDataset.initRawMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << posYselfTestDataset.initRawMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << posYselfTestDataset.initRawMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) calibrated magnetic field X: "
|
|
<< posYselfTestDataset.initCalMagX << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: "
|
|
<< posYselfTestDataset.initCalMagY << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: "
|
|
<< posYselfTestDataset.initCalMagZ << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil X current: " << posYselfTestDataset.initCoilXCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Y current: " << posYselfTestDataset.initCoilYCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Z current: " << posYselfTestDataset.initCoilZCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil X temperature: "
|
|
<< posYselfTestDataset.initCoilXTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Y temperature: "
|
|
<< posYselfTestDataset.initCoilYTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Z temperature: "
|
|
<< posYselfTestDataset.initCoilZTemperature << " °C" << std::endl;
|
|
|
|
sif::info << "IMTQ self test (+Y) err: "
|
|
<< static_cast<unsigned int>(posYselfTestDataset.err.value) << std::endl;
|
|
sif::info << "IMTQ self test (+Y) raw magnetic field X: " << posYselfTestDataset.rawMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (+Y) raw magnetic field Y: " << posYselfTestDataset.rawMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (+Y) raw magnetic field Z: " << posYselfTestDataset.rawMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (+Y) calibrated magnetic field X: " << posYselfTestDataset.calMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (+Y) calibrated magnetic field Y: " << posYselfTestDataset.calMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (+Y) calibrated magnetic field Z: " << posYselfTestDataset.calMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (+Y) coil X current: " << posYselfTestDataset.coilXCurrent << " mA"
|
|
<< std::endl;
|
|
sif::info << "IMTQ self test (+Y) coil Y current: " << posYselfTestDataset.coilYCurrent << " mA"
|
|
<< std::endl;
|
|
sif::info << "IMTQ self test (+Y) coil Z current: " << posYselfTestDataset.coilZCurrent << " mA"
|
|
<< std::endl;
|
|
sif::info << "IMTQ self test (+Y) coil X temperature: " << posYselfTestDataset.coilXTemperature
|
|
<< " °C" << std::endl;
|
|
sif::info << "IMTQ self test (+Y) coil Y temperature: " << posYselfTestDataset.coilYTemperature
|
|
<< " °C" << std::endl;
|
|
sif::info << "IMTQ self test (+Y) coil Z temperature: " << posYselfTestDataset.coilZTemperature
|
|
<< " °C" << std::endl;
|
|
|
|
sif::info << "IMTQ self test (FINA) err: "
|
|
<< static_cast<unsigned int>(posYselfTestDataset.finaErr.value) << std::endl;
|
|
sif::info << "IMTQ self test (FINA) raw magnetic field X: " << posYselfTestDataset.finaRawMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << posYselfTestDataset.finaRawMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << posYselfTestDataset.finaRawMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) calibrated magnetic field X: "
|
|
<< posYselfTestDataset.finaCalMagX << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: "
|
|
<< posYselfTestDataset.finaCalMagY << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: "
|
|
<< posYselfTestDataset.finaCalMagZ << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil X current: " << posYselfTestDataset.finaCoilXCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Y current: " << posYselfTestDataset.finaCoilYCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Z current: " << posYselfTestDataset.finaCoilZCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil X temperature: "
|
|
<< posYselfTestDataset.finaCoilXTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Y temperature: "
|
|
<< posYselfTestDataset.finaCoilYTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Z temperature: "
|
|
<< posYselfTestDataset.finaCoilZTemperature << " °C" << std::endl;
|
|
#endif
|
|
}
|
|
}
|
|
|
|
void ImtqHandler::handleNegativeYSelfTestReply(const uint8_t* packet) {
|
|
PoolReadGuard rg(&posXselfTestDataset);
|
|
|
|
uint16_t offset = 2;
|
|
/** Init measurements */
|
|
posZselfTestDataset.initErr = *(packet + offset);
|
|
offset += 2; // STEP byte will not be stored
|
|
negYselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negYselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negYselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negYselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negYselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negYselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negYselfTestDataset.initCoilXCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negYselfTestDataset.initCoilYCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negYselfTestDataset.initCoilZCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negYselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
negYselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
negYselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
|
|
/** +X measurements */
|
|
checkErrorByte(*(packet + offset), *(packet + offset + 1));
|
|
negYselfTestDataset.err = *(packet + offset);
|
|
offset += 2; // STEP byte will not be stored
|
|
negYselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negYselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negYselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negYselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negYselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negYselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negYselfTestDataset.coilXCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negYselfTestDataset.coilYCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negYselfTestDataset.coilZCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negYselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
negYselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
negYselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
|
|
/** FINA measurements */
|
|
checkErrorByte(*(packet + offset), *(packet + offset + 1));
|
|
negYselfTestDataset.finaErr = *(packet + offset);
|
|
offset += 2; // STEP byte will not be stored
|
|
negYselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negYselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negYselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negYselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negYselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negYselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negYselfTestDataset.finaCoilXCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negYselfTestDataset.finaCoilYCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negYselfTestDataset.finaCoilZCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negYselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
negYselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
negYselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
|
|
if (debugMode) {
|
|
#if OBSW_VERBOSE_LEVEL >= 1
|
|
sif::info << "IMTQ self test (INIT) err: "
|
|
<< static_cast<unsigned int>(negYselfTestDataset.initErr.value) << std::endl;
|
|
sif::info << "IMTQ self test (INIT) raw magnetic field X: " << negYselfTestDataset.initRawMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << negYselfTestDataset.initRawMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << negYselfTestDataset.initRawMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) calibrated magnetic field X: "
|
|
<< negYselfTestDataset.initCalMagX << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: "
|
|
<< negYselfTestDataset.initCalMagY << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: "
|
|
<< negYselfTestDataset.initCalMagZ << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil X current: " << negYselfTestDataset.initCoilXCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Y current: " << negYselfTestDataset.initCoilYCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Z current: " << negYselfTestDataset.initCoilZCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil X temperature: "
|
|
<< negYselfTestDataset.initCoilXTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Y temperature: "
|
|
<< negYselfTestDataset.initCoilYTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Z temperature: "
|
|
<< negYselfTestDataset.initCoilZTemperature << " °C" << std::endl;
|
|
|
|
sif::info << "IMTQ self test (-Y) err: "
|
|
<< static_cast<unsigned int>(negYselfTestDataset.err.value) << std::endl;
|
|
sif::info << "IMTQ self test (-Y) raw magnetic field X: " << negYselfTestDataset.rawMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (-Y) raw magnetic field Y: " << negYselfTestDataset.rawMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (-Y) raw magnetic field Z: " << negYselfTestDataset.rawMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (-Y) calibrated magnetic field X: " << negYselfTestDataset.calMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (-Y) calibrated magnetic field Y: " << negYselfTestDataset.calMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (-Y) calibrated magnetic field Z: " << negYselfTestDataset.calMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (-Y) coil X current: " << negYselfTestDataset.coilXCurrent << " mA"
|
|
<< std::endl;
|
|
sif::info << "IMTQ self test (-Y) coil Y current: " << negYselfTestDataset.coilYCurrent << " mA"
|
|
<< std::endl;
|
|
sif::info << "IMTQ self test (-Y) coil Z current: " << negYselfTestDataset.coilZCurrent << " mA"
|
|
<< std::endl;
|
|
sif::info << "IMTQ self test (-Y) coil X temperature: " << negYselfTestDataset.coilXTemperature
|
|
<< " °C" << std::endl;
|
|
sif::info << "IMTQ self test (-Y) coil Y temperature: " << negYselfTestDataset.coilYTemperature
|
|
<< " °C" << std::endl;
|
|
sif::info << "IMTQ self test (-Y) coil Z temperature: " << negYselfTestDataset.coilZTemperature
|
|
<< " °C" << std::endl;
|
|
|
|
sif::info << "IMTQ self test (FINA) err: "
|
|
<< static_cast<unsigned int>(negYselfTestDataset.finaErr.value) << std::endl;
|
|
sif::info << "IMTQ self test (FINA) raw magnetic field X: " << negYselfTestDataset.finaRawMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << negYselfTestDataset.finaRawMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << negYselfTestDataset.finaRawMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) calibrated magnetic field X: "
|
|
<< negYselfTestDataset.finaCalMagX << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: "
|
|
<< negYselfTestDataset.finaCalMagY << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: "
|
|
<< negYselfTestDataset.finaCalMagZ << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil X current: " << negYselfTestDataset.finaCoilXCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Y current: " << negYselfTestDataset.finaCoilYCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Z current: " << negYselfTestDataset.finaCoilZCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil X temperature: "
|
|
<< negYselfTestDataset.finaCoilXTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Y temperature: "
|
|
<< negYselfTestDataset.finaCoilYTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Z temperature: "
|
|
<< negYselfTestDataset.finaCoilZTemperature << " °C" << std::endl;
|
|
#endif
|
|
}
|
|
}
|
|
|
|
void ImtqHandler::handlePositiveZSelfTestReply(const uint8_t* packet) {
|
|
PoolReadGuard rg(&posXselfTestDataset);
|
|
|
|
uint16_t offset = 2;
|
|
/** Init measurements */
|
|
posZselfTestDataset.initErr = *(packet + offset);
|
|
offset += 2; // STEP byte will not be stored
|
|
posZselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posZselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posZselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posZselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posZselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posZselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posZselfTestDataset.initCoilXCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posZselfTestDataset.initCoilYCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posZselfTestDataset.initCoilZCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posZselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
posZselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
posZselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
|
|
/** +X measurements */
|
|
checkErrorByte(*(packet + offset), *(packet + offset + 1));
|
|
posZselfTestDataset.err = *(packet + offset);
|
|
offset += 2; // STEP byte will not be stored
|
|
posZselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posZselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posZselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posZselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posZselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posZselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posZselfTestDataset.coilXCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posZselfTestDataset.coilYCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posZselfTestDataset.coilZCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posZselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
posZselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
posZselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
|
|
/** FINA measurements */
|
|
checkErrorByte(*(packet + offset), *(packet + offset + 1));
|
|
posZselfTestDataset.finaErr = *(packet + offset);
|
|
offset += 2; // STEP byte will not be stored
|
|
posZselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posZselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posZselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
posZselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posZselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posZselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
posZselfTestDataset.finaCoilXCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posZselfTestDataset.finaCoilYCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posZselfTestDataset.finaCoilZCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
posZselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
posZselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
posZselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
|
|
if (debugMode) {
|
|
#if OBSW_VERBOSE_LEVEL >= 1
|
|
sif::info << "IMTQ self test (INIT) err: "
|
|
<< static_cast<unsigned int>(posZselfTestDataset.initErr.value) << std::endl;
|
|
sif::info << "IMTQ self test (INIT) raw magnetic field X: " << posZselfTestDataset.initRawMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << posZselfTestDataset.initRawMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << posZselfTestDataset.initRawMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) calibrated magnetic field X: "
|
|
<< posZselfTestDataset.initCalMagX << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: "
|
|
<< posZselfTestDataset.initCalMagY << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: "
|
|
<< posZselfTestDataset.initCalMagZ << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil X current: " << posZselfTestDataset.initCoilXCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Y current: " << posZselfTestDataset.initCoilYCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Z current: " << posZselfTestDataset.initCoilZCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil X temperature: "
|
|
<< posZselfTestDataset.initCoilXTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Y temperature: "
|
|
<< posZselfTestDataset.initCoilYTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Z temperature: "
|
|
<< posZselfTestDataset.initCoilZTemperature << " °C" << std::endl;
|
|
|
|
sif::info << "IMTQ self test (+Z) err: "
|
|
<< static_cast<unsigned int>(posZselfTestDataset.err.value) << std::endl;
|
|
sif::info << "IMTQ self test (+Z) raw magnetic field X: " << posZselfTestDataset.rawMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (+Z) raw magnetic field Y: " << posZselfTestDataset.rawMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (+Z) raw magnetic field Z: " << posZselfTestDataset.rawMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (+Z) calibrated magnetic field X: " << posZselfTestDataset.calMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (+Z) calibrated magnetic field Y: " << posZselfTestDataset.calMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (+Z) calibrated magnetic field Z: " << posZselfTestDataset.calMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (+Z) coil X current: " << posZselfTestDataset.coilXCurrent << " mA"
|
|
<< std::endl;
|
|
sif::info << "IMTQ self test (+Z) coil Y current: " << posZselfTestDataset.coilYCurrent << " mA"
|
|
<< std::endl;
|
|
sif::info << "IMTQ self test (+Z) coil Z current: " << posZselfTestDataset.coilZCurrent << " mA"
|
|
<< std::endl;
|
|
sif::info << "IMTQ self test (+Z) coil X temperature: " << posZselfTestDataset.coilXTemperature
|
|
<< " °C" << std::endl;
|
|
sif::info << "IMTQ self test (+Z) coil Y temperature: " << posZselfTestDataset.coilYTemperature
|
|
<< " °C" << std::endl;
|
|
sif::info << "IMTQ self test (+Z) coil Z temperature: " << negYselfTestDataset.coilZTemperature
|
|
<< " °C" << std::endl;
|
|
|
|
sif::info << "IMTQ self test (FINA) err: "
|
|
<< static_cast<unsigned int>(posZselfTestDataset.finaErr.value) << std::endl;
|
|
sif::info << "IMTQ self test (FINA) raw magnetic field X: " << posZselfTestDataset.finaRawMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << posZselfTestDataset.finaRawMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << posZselfTestDataset.finaRawMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) calibrated magnetic field X: "
|
|
<< posZselfTestDataset.finaCalMagX << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: "
|
|
<< posZselfTestDataset.finaCalMagY << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: "
|
|
<< posZselfTestDataset.finaCalMagZ << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil X current: " << posZselfTestDataset.finaCoilXCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Y current: " << posZselfTestDataset.finaCoilYCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Z current: " << posZselfTestDataset.finaCoilZCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil X temperature: "
|
|
<< posZselfTestDataset.finaCoilXTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Y temperature: "
|
|
<< posZselfTestDataset.finaCoilYTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Z temperature: "
|
|
<< posZselfTestDataset.finaCoilZTemperature << " °C" << std::endl;
|
|
#endif
|
|
}
|
|
}
|
|
|
|
void ImtqHandler::handleNegativeZSelfTestReply(const uint8_t* packet) {
|
|
PoolReadGuard rg(&posXselfTestDataset);
|
|
|
|
uint16_t offset = 2;
|
|
/** Init measurements */
|
|
negZselfTestDataset.initErr = *(packet + offset);
|
|
offset += 2; // STEP byte will not be stored
|
|
negZselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negZselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negZselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negZselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negZselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negZselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negZselfTestDataset.initCoilXCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negZselfTestDataset.initCoilYCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negZselfTestDataset.initCoilZCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negZselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
negZselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
negZselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
|
|
/** +X measurements */
|
|
checkErrorByte(*(packet + offset), *(packet + offset + 1));
|
|
negZselfTestDataset.err = *(packet + offset);
|
|
offset += 2; // STEP byte will not be stored
|
|
negZselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negZselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negZselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negZselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negZselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negZselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negZselfTestDataset.coilXCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negZselfTestDataset.coilYCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negZselfTestDataset.coilZCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negZselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
negZselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
negZselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
|
|
/** FINA measurements */
|
|
checkErrorByte(*(packet + offset), *(packet + offset + 1));
|
|
negZselfTestDataset.finaErr = *(packet + offset);
|
|
offset += 2; // STEP byte will not be stored
|
|
negZselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negZselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negZselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset)) *
|
|
7.5;
|
|
offset += 4;
|
|
negZselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negZselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negZselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
|
|
*(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
negZselfTestDataset.finaCoilXCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negZselfTestDataset.finaCoilYCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negZselfTestDataset.finaCoilZCurrent =
|
|
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
|
offset += 2;
|
|
negZselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
negZselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 2;
|
|
negZselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
|
|
if (debugMode) {
|
|
#if OBSW_VERBOSE_LEVEL >= 1
|
|
sif::info << "IMTQ self test (INIT) err: "
|
|
<< static_cast<unsigned int>(negZselfTestDataset.initErr.value) << std::endl;
|
|
sif::info << "IMTQ self test (INIT) raw magnetic field X: " << negZselfTestDataset.initRawMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << negZselfTestDataset.initRawMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << negZselfTestDataset.initRawMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) calibrated magnetic field X: "
|
|
<< negZselfTestDataset.initCalMagX << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: "
|
|
<< negZselfTestDataset.initCalMagY << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: "
|
|
<< negZselfTestDataset.initCalMagZ << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil X current: " << negZselfTestDataset.initCoilXCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Y current: " << negZselfTestDataset.initCoilYCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Z current: " << negZselfTestDataset.initCoilZCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil X temperature: "
|
|
<< negZselfTestDataset.initCoilXTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Y temperature: "
|
|
<< negZselfTestDataset.initCoilYTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Z temperature: "
|
|
<< negZselfTestDataset.initCoilZTemperature << " °C" << std::endl;
|
|
|
|
sif::info << "IMTQ self test (-Z) err: "
|
|
<< static_cast<unsigned int>(negZselfTestDataset.err.value) << std::endl;
|
|
sif::info << "IMTQ self test (-Z) raw magnetic field X: " << negZselfTestDataset.rawMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (-Z) raw magnetic field Y: " << negZselfTestDataset.rawMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (-Z) raw magnetic field Z: " << negZselfTestDataset.rawMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (-Z) calibrated magnetic field X: " << negZselfTestDataset.calMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (-Z) calibrated magnetic field Y: " << negZselfTestDataset.calMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (-Z) calibrated magnetic field Z: " << negZselfTestDataset.calMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (-Z) coil X current: " << negZselfTestDataset.coilXCurrent << " mA"
|
|
<< std::endl;
|
|
sif::info << "IMTQ self test (-Z) coil Y current: " << negZselfTestDataset.coilYCurrent << " mA"
|
|
<< std::endl;
|
|
sif::info << "IMTQ self test (-Z) coil Z current: " << negZselfTestDataset.coilZCurrent << " mA"
|
|
<< std::endl;
|
|
sif::info << "IMTQ self test (-Z) coil X temperature: " << negZselfTestDataset.coilXTemperature
|
|
<< " °C" << std::endl;
|
|
sif::info << "IMTQ self test (-Z) coil Y temperature: " << negZselfTestDataset.coilYTemperature
|
|
<< " °C" << std::endl;
|
|
sif::info << "IMTQ self test (-Z) coil Z temperature: " << negYselfTestDataset.coilZTemperature
|
|
<< " °C" << std::endl;
|
|
|
|
sif::info << "IMTQ self test (FINA) err: "
|
|
<< static_cast<unsigned int>(negZselfTestDataset.finaErr.value) << std::endl;
|
|
sif::info << "IMTQ self test (FINA) raw magnetic field X: " << negZselfTestDataset.finaRawMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << negZselfTestDataset.finaRawMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << negZselfTestDataset.finaRawMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) calibrated magnetic field X: "
|
|
<< negZselfTestDataset.finaCalMagX << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: "
|
|
<< negZselfTestDataset.finaCalMagY << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: "
|
|
<< negZselfTestDataset.finaCalMagZ << " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil X current: " << negZselfTestDataset.finaCoilXCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Y current: " << negZselfTestDataset.finaCoilYCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Z current: " << negZselfTestDataset.finaCoilZCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil X temperature: "
|
|
<< negZselfTestDataset.finaCoilXTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Y temperature: "
|
|
<< negZselfTestDataset.finaCoilYTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Z temperature: "
|
|
<< negZselfTestDataset.finaCoilZTemperature << " °C" << std::endl;
|
|
#endif
|
|
}
|
|
}
|
|
|
|
void ImtqHandler::setDebugMode(bool enable) { this->debugMode = enable; }
|
|
|
|
void ImtqHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) {
|
|
std::string stepString("");
|
|
if (step < 8) {
|
|
stepString = makeStepString(step);
|
|
} else {
|
|
/** This should normally never happen */
|
|
sif::debug << "IMTQHandler::checkErrorByte: Invalid step" << std::endl;
|
|
return;
|
|
}
|
|
|
|
if (errorByte == 0) {
|
|
return;
|
|
}
|
|
|
|
if (errorByte & imtq::I2C_FAILURE_MASK) {
|
|
triggerEvent(SELF_TEST_I2C_FAILURE, step);
|
|
sif::error << "IMTQHandler::checkErrorByte: Self test I2C failure for step " << stepString
|
|
<< std::endl;
|
|
}
|
|
if (errorByte & imtq::SPI_FAILURE_MASK) {
|
|
triggerEvent(SELF_TEST_SPI_FAILURE, step);
|
|
sif::error << "IMTQHandler::checkErrorByte: Self test SPI failure for step " << stepString
|
|
<< std::endl;
|
|
}
|
|
if (errorByte & imtq::ADC_FAILURE_MASK) {
|
|
triggerEvent(SELF_TEST_ADC_FAILURE, step);
|
|
sif::error << "IMTQHandler::checkErrorByte: Self test ADC failure for step " << stepString
|
|
<< std::endl;
|
|
}
|
|
if (errorByte & imtq::PWM_FAILURE_MASK) {
|
|
triggerEvent(SELF_TEST_PWM_FAILURE, step);
|
|
sif::error << "IMTQHandler::checkErrorByte: Self test PWM failure for step " << stepString
|
|
<< std::endl;
|
|
}
|
|
if (errorByte & imtq::TC_FAILURE_MASK) {
|
|
triggerEvent(SELF_TEST_TC_FAILURE, step);
|
|
sif::error << "IMTQHandler::checkErrorByte: Self test TC failure (system failure) for step "
|
|
<< stepString << std::endl;
|
|
}
|
|
if (errorByte & imtq::MTM_RANGE_FAILURE_MASK) {
|
|
triggerEvent(SELF_TEST_TC_FAILURE, step);
|
|
sif::error << "IMTQHandler::checkErrorByte: Self test MTM range failure for step " << stepString
|
|
<< std::endl;
|
|
}
|
|
if (errorByte & imtq::COIL_CURRENT_FAILURE_MASK) {
|
|
triggerEvent(SELF_TEST_COIL_CURRENT_FAILURE, step);
|
|
sif::error << "IMTQHandler::checkErrorByte: Self test coil current outside of expected "
|
|
"range for step "
|
|
<< stepString << std::endl;
|
|
}
|
|
|
|
if (errorByte & imtq::INVALID_ERROR_BYTE) {
|
|
triggerEvent(INVALID_ERROR_BYTE, step);
|
|
sif::error << "IMTQHandler::checkErrorByte: Self test result of step " << stepString
|
|
<< " has invalid error byte" << std::endl;
|
|
}
|
|
}
|
|
|
|
void ImtqHandler::fillSystemStateIntoDataset(const uint8_t* packet) {
|
|
PoolReadGuard pg(&statusSet);
|
|
statusSet.statusByteMode.value = packet[2];
|
|
statusSet.statusByteError.value = packet[3];
|
|
statusSet.statusByteConfig.value = packet[4];
|
|
size_t dummy = 0;
|
|
SerializeAdapter::deSerialize<uint32_t>(&statusSet.statusByteUptime.value, packet + 5, &dummy,
|
|
SerializeIF::Endianness::LITTLE);
|
|
statusSet.setValidity(true, true);
|
|
}
|
|
|
|
std::string ImtqHandler::makeStepString(const uint8_t step) {
|
|
std::string stepString("");
|
|
switch (step) {
|
|
case imtq::selfTest::step::INIT:
|
|
stepString = std::string("INIT");
|
|
break;
|
|
case imtq::selfTest::step::X_POSITIVE:
|
|
stepString = std::string("+X");
|
|
break;
|
|
case imtq::selfTest::step::X_NEGATIVE:
|
|
stepString = std::string("-X");
|
|
break;
|
|
case imtq::selfTest::step::Y_POSITIVE:
|
|
stepString = std::string("+Y");
|
|
break;
|
|
case imtq::selfTest::step::Y_NEGATIVE:
|
|
stepString = std::string("-Y");
|
|
break;
|
|
case imtq::selfTest::step::Z_POSITIVE:
|
|
stepString = std::string("+Z");
|
|
break;
|
|
case imtq::selfTest::step::Z_NEGATIVE:
|
|
stepString = std::string("-Z");
|
|
break;
|
|
case imtq::selfTest::step::FINA:
|
|
stepString = std::string("FINA");
|
|
break;
|
|
default:
|
|
sif::error << "IMTQHandler::checkErrorByte: Received packet with invalid step information"
|
|
<< std::endl;
|
|
break;
|
|
}
|
|
return stepString;
|
|
}
|
|
|
|
ReturnValue_t ImtqHandler::getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) {
|
|
if (switcher != power::NO_SWITCH) {
|
|
*numberOfSwitches = 1;
|
|
*switches = &switcher;
|
|
return returnvalue::OK;
|
|
}
|
|
return DeviceHandlerBase::NO_SWITCH;
|
|
}
|