diff --git a/bsp_rpi/ObjectFactory.cpp b/bsp_rpi/ObjectFactory.cpp index ff0970a4..8979e701 100644 --- a/bsp_rpi/ObjectFactory.cpp +++ b/bsp_rpi/ObjectFactory.cpp @@ -99,12 +99,12 @@ void ObjectFactory::produce(){ gpioIF->addGpios(gpioCookieAcsBoard); std::string spiDev = "/dev/spidev0.0"; - SpiCookie* spiCookie = new SpiCookie(addresses::MGM_0_LIS3, - gpioIds::MGM_0_LIS3_CS, spiDev, 24, spi::SpiMode::MODE_3, 3'900'000); + SpiCookie* spiCookie = new SpiCookie(addresses::MGM_0_LIS3, gpioIds::MGM_0_LIS3_CS, spiDev, 24, + spi::SpiMode::MODE_3, 3'900'000); auto mgmLis3Handler = new MGMHandlerLIS3MDL(objects::MGM_0_LIS3_HANDLER, objects::SPI_COM_IF, spiCookie); mgmLis3Handler->setStartUpImmediately(); - spiCookie = new SpiCookie(addresses::MGM_1_RM3100, gpioIds::MGM_1_RM3100_CS, spiDev, 32, + spiCookie = new SpiCookie(addresses::MGM_1_RM3100, gpioIds::MGM_1_RM3100_CS, spiDev, 16, spi::SpiMode::MODE_3, 976'000); auto mgmRm3100Handler = new MGMHandlerRM3100(objects::MGM_1_RM3100_HANDLER, objects::SPI_COM_IF, spiCookie); diff --git a/bsp_rpi/boardconfig/rpi_config.h b/bsp_rpi/boardconfig/rpi_config.h index be8dac4e..d1acacb6 100644 --- a/bsp_rpi/boardconfig/rpi_config.h +++ b/bsp_rpi/boardconfig/rpi_config.h @@ -7,8 +7,8 @@ #define RPI_LOOPBACK_TEST_GPIO 0 /* Only one of those 2 should be enabled! */ -#define RPI_ADD_SPI_TEST 1 -#define RPI_TEST_ACS_BOARD 0 +#define RPI_ADD_SPI_TEST 0 +#define RPI_TEST_ACS_BOARD 1 /* Adapt these values accordingly */ namespace gpio { diff --git a/fsfw b/fsfw index 17b8d3fe..778ef4ef 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 17b8d3fed05cae6e208dc3f14b6ba0d44c9ec5ef +Subproject commit 778ef4ef230019ca4b155a77518156793cf5d308 diff --git a/fsfwconfig/OBSWConfig.h b/fsfwconfig/OBSWConfig.h index a7b078a2..ab1526bd 100644 --- a/fsfwconfig/OBSWConfig.h +++ b/fsfwconfig/OBSWConfig.h @@ -26,7 +26,8 @@ debugging. */ #define PDU2_DEBUG 0 #define ACU_DEBUG 1 -#define FSFW_LINUX_SPI_WIRETAPPING 1 +/* Can be used for low-level debugging of the SPI bus */ +#define FSFW_LINUX_SPI_WIRETAPPING 0 #ifdef __cplusplus diff --git a/mission/devices/MGMHandlerRM3100.cpp b/mission/devices/MGMHandlerRM3100.cpp index bec149e7..154f3574 100644 --- a/mission/devices/MGMHandlerRM3100.cpp +++ b/mission/devices/MGMHandlerRM3100.cpp @@ -1,6 +1,7 @@ #include #include "MGMHandlerRM3100.h" +#include #include #include #include @@ -8,10 +9,10 @@ MGMHandlerRM3100::MGMHandlerRM3100(object_id_t objectId, object_id_t deviceCommunication, CookieIF* comCookie): - DeviceHandlerBase(objectId, deviceCommunication, comCookie), - primaryDataset(this) { + DeviceHandlerBase(objectId, deviceCommunication, comCookie), + primaryDataset(this) { #if OBSW_VERBOSE_LEVEL >= 1 - debugDivider = new PeriodicOperationDivider(10); + debugDivider = new PeriodicOperationDivider(10); #endif } @@ -52,307 +53,303 @@ void MGMHandlerRM3100::doStartUp() { } void MGMHandlerRM3100::doShutDown() { - setMode(_MODE_POWER_DOWN); + setMode(_MODE_POWER_DOWN); } ReturnValue_t MGMHandlerRM3100::buildTransitionDeviceCommand( - DeviceCommandId_t *id) { - switch(internalState) { - case(InternalState::STATE_NONE): - case(InternalState::STATE_NORMAL): { - return HasReturnvaluesIF::RETURN_OK; - } - case(InternalState::STATE_CONFIGURE_CMM): { - *id = RM3100::CONFIGURE_CMM; - break; - } - case(InternalState::STATE_READ_CMM): { - *id = RM3100::READ_CMM; - break; - } - case(InternalState::STATE_CONFIGURE_TMRC): { - *id = RM3100::CONFIGURE_TMRC; - break; - } - case(InternalState::STATE_READ_TMRC): { - *id = RM3100::READ_TMRC; - break; - } - default: - // might be a configuration error. - sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown " - << "internal state!" << std::endl; - return HasReturnvaluesIF::RETURN_OK; - } + DeviceCommandId_t *id) { + switch(internalState) { + case(InternalState::STATE_NONE): + case(InternalState::STATE_NORMAL): { + return HasReturnvaluesIF::RETURN_OK; + } + case(InternalState::STATE_CONFIGURE_CMM): { + *id = RM3100::CONFIGURE_CMM; + break; + } + case(InternalState::STATE_READ_CMM): { + *id = RM3100::READ_CMM; + break; + } + case(InternalState::STATE_CONFIGURE_TMRC): { + *id = RM3100::CONFIGURE_TMRC; + break; + } + case(InternalState::STATE_READ_TMRC): { + *id = RM3100::READ_TMRC; + break; + } + default: + // might be a configuration error. + sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown " + << "internal state!" << std::endl; + return HasReturnvaluesIF::RETURN_OK; + } - return buildCommandFromCommand(*id, nullptr, 0); + return buildCommandFromCommand(*id, nullptr, 0); } ReturnValue_t MGMHandlerRM3100::buildCommandFromCommand( - DeviceCommandId_t deviceCommand, const uint8_t *commandData, - size_t commandDataLen) { - switch(deviceCommand) { - case(RM3100::CONFIGURE_CMM): { - commandBuffer[0] = RM3100::CMM_REGISTER; - commandBuffer[1] = RM3100::CMM_VALUE; - rawPacket = commandBuffer; - rawPacketLen = 2; - break; - } - case(RM3100::READ_CMM): { - commandBuffer[0] = RM3100::CMM_REGISTER | RM3100::READ_MASK; - commandBuffer[1] = 0; - rawPacket = commandBuffer; - rawPacketLen = 2; - break; - } - case(RM3100::CONFIGURE_TMRC): { - return handleTmrcConfigCommand(deviceCommand, commandData, - commandDataLen); - } - case(RM3100::READ_TMRC): { - commandBuffer[0] = RM3100::TMRC_REGISTER | RM3100::READ_MASK; - commandBuffer[1] = 0; - rawPacket = commandBuffer; - rawPacketLen = 2; - break; - } - case(RM3100::CONFIGURE_CYCLE_COUNT): { - return handleCycleCountConfigCommand(deviceCommand, commandData, - commandDataLen); - } - case(RM3100::READ_CYCLE_COUNT): { - commandBuffer[0] = RM3100::CYCLE_COUNT_START_REGISTER | - RM3100::READ_MASK; - std::memset(commandBuffer + 1, 0, 6); - rawPacket = commandBuffer; - rawPacketLen = 7; - break; - } - case(RM3100::READ_DATA): { - commandBuffer[0] = RM3100::MEASUREMENT_REG_START | RM3100::READ_MASK; - std::memset(commandBuffer + 1, 0, 9); - rawPacketLen = 10; - break; - } - default: - return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; - } - return RETURN_OK; + DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) { + switch(deviceCommand) { + case(RM3100::CONFIGURE_CMM): { + commandBuffer[0] = RM3100::CMM_REGISTER; + commandBuffer[1] = RM3100::CMM_VALUE; + rawPacket = commandBuffer; + rawPacketLen = 2; + break; + } + case(RM3100::READ_CMM): { + commandBuffer[0] = RM3100::CMM_REGISTER | RM3100::READ_MASK; + commandBuffer[1] = 0; + rawPacket = commandBuffer; + rawPacketLen = 2; + break; + } + case(RM3100::CONFIGURE_TMRC): { + return handleTmrcConfigCommand(deviceCommand, commandData, + commandDataLen); + } + case(RM3100::READ_TMRC): { + commandBuffer[0] = RM3100::TMRC_REGISTER | RM3100::READ_MASK; + commandBuffer[1] = 0; + rawPacket = commandBuffer; + rawPacketLen = 2; + break; + } + case(RM3100::CONFIGURE_CYCLE_COUNT): { + return handleCycleCountConfigCommand(deviceCommand, commandData, + commandDataLen); + } + case(RM3100::READ_CYCLE_COUNT): { + commandBuffer[0] = RM3100::CYCLE_COUNT_START_REGISTER | RM3100::READ_MASK; + std::memset(commandBuffer + 1, 0, 6); + rawPacket = commandBuffer; + rawPacketLen = 7; + break; + } + case(RM3100::READ_DATA): { + commandBuffer[0] = RM3100::MEASUREMENT_REG_START | RM3100::READ_MASK; + std::memset(commandBuffer + 1, 0, 9); + rawPacketLen = 10; + break; + } + default: + return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; + } + return RETURN_OK; } ReturnValue_t MGMHandlerRM3100::buildNormalDeviceCommand( - DeviceCommandId_t *id) { - *id = RM3100::READ_DATA; - return buildCommandFromCommand(*id, nullptr, 0); + DeviceCommandId_t *id) { + *id = RM3100::READ_DATA; + return buildCommandFromCommand(*id, nullptr, 0); } ReturnValue_t MGMHandlerRM3100::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, - size_t *foundLen) { + size_t *foundLen) { - /* For SPI, ID will always be the one of the last sent command. */ - *foundId = this->getPendingCommand(); - *foundLen = len; - return HasReturnvaluesIF::RETURN_OK; + /* For SPI, ID will always be the one of the last sent command. */ + *foundId = this->getPendingCommand(); + *foundLen = len; + return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t MGMHandlerRM3100::interpretDeviceReply( - DeviceCommandId_t id, const uint8_t *packet) { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; - switch(id) { - case(RM3100::CONFIGURE_CMM): - case(RM3100::CONFIGURE_CYCLE_COUNT): - case(RM3100::CONFIGURE_TMRC): { - /* We can only check whether write was successful with read operation. */ - break; - } - case(RM3100::READ_CMM): { - if(packet[1] == cmmRegValue) { - commandExecuted = true; - } - else { - /* Attempt reconfiguration. */ - internalState = InternalState::STATE_CONFIGURE_CMM; - return DeviceHandlerIF::DEVICE_REPLY_INVALID; - } - break; - } - case(RM3100::READ_TMRC): { - if(packet[1] == tmrcRegValue) { - commandExecuted = true; - /* Reading TMRC was commanded. Trigger event to inform ground. */ - if(mode != _MODE_START_UP) { - triggerEvent(tmrcSet, tmrcRegValue, 0); - } - } - else { - /* Attempt reconfiguration. */ - internalState = InternalState::STATE_CONFIGURE_TMRC; - return DeviceHandlerIF::DEVICE_REPLY_INVALID; - } - break; - } - case(RM3100::READ_CYCLE_COUNT): { - uint16_t cycleCountX = packet[1] << 8 | packet[2]; - uint16_t cycleCountY = packet[3] << 8 | packet[4]; - uint16_t cycleCountZ = packet[5] << 8 | packet[6]; - if(cycleCountX != cycleCountRegValueX or - cycleCountY != cycleCountRegValueY or - cycleCountZ != cycleCountRegValueZ) { - return DeviceHandlerIF::DEVICE_REPLY_INVALID; - } - /* Reading TMRC was commanded. Trigger event to inform ground. */ - if(mode != _MODE_START_UP) { - uint32_t eventParam1 = cycleCountX << 16 | cycleCountY; - triggerEvent(cycleCountersSet, eventParam1, cycleCountZ); - } - break; - } - case(RM3100::READ_DATA): { - result = handleDataReadout(packet); - break; - } - default: - return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; - } + DeviceCommandId_t id, const uint8_t *packet) { + ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + switch(id) { + case(RM3100::CONFIGURE_CMM): + case(RM3100::CONFIGURE_CYCLE_COUNT): + case(RM3100::CONFIGURE_TMRC): { + /* We can only check whether write was successful with read operation. */ + break; + } + case(RM3100::READ_CMM): { + uint8_t cmmValue = packet[1]; + /* We clear the seventh bit in any case + * because this one is zero sometimes for some reason */ + bitutil::bitClear(&cmmValue, 6); + if(cmmValue == cmmRegValue) { + commandExecuted = true; + } + else { + /* Attempt reconfiguration. */ + internalState = InternalState::STATE_CONFIGURE_CMM; + return DeviceHandlerIF::DEVICE_REPLY_INVALID; + } + break; + } + case(RM3100::READ_TMRC): { + if(packet[1] == tmrcRegValue) { + commandExecuted = true; + /* Reading TMRC was commanded. Trigger event to inform ground. */ + if(mode != _MODE_START_UP) { + triggerEvent(tmrcSet, tmrcRegValue, 0); + } + } + else { + /* Attempt reconfiguration. */ + internalState = InternalState::STATE_CONFIGURE_TMRC; + return DeviceHandlerIF::DEVICE_REPLY_INVALID; + } + break; + } + case(RM3100::READ_CYCLE_COUNT): { + uint16_t cycleCountX = packet[1] << 8 | packet[2]; + uint16_t cycleCountY = packet[3] << 8 | packet[4]; + uint16_t cycleCountZ = packet[5] << 8 | packet[6]; + if(cycleCountX != cycleCountRegValueX or cycleCountY != cycleCountRegValueY or + cycleCountZ != cycleCountRegValueZ) { + return DeviceHandlerIF::DEVICE_REPLY_INVALID; + } + /* Reading TMRC was commanded. Trigger event to inform ground. */ + if(mode != _MODE_START_UP) { + uint32_t eventParam1 = (cycleCountX << 16) | cycleCountY; + triggerEvent(cycleCountersSet, eventParam1, cycleCountZ); + } + break; + } + case(RM3100::READ_DATA): { + result = handleDataReadout(packet); + break; + } + default: + return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; + } - return result; + return result; } -ReturnValue_t MGMHandlerRM3100::handleCycleCountConfigCommand( - DeviceCommandId_t deviceCommand, const uint8_t *commandData, - size_t commandDataLen) { - if(commandData == nullptr) { - return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; - } +ReturnValue_t MGMHandlerRM3100::handleCycleCountConfigCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, size_t commandDataLen) { + if(commandData == nullptr) { + return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; + } - // Set cycle count - if(commandDataLen == 2) { - handleCycleCommand(true, commandData, commandDataLen); - } - else if(commandDataLen == 6) { - handleCycleCommand(false, commandData, commandDataLen); - } - else { - return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; - } + // Set cycle count + if(commandDataLen == 2) { + handleCycleCommand(true, commandData, commandDataLen); + } + else if(commandDataLen == 6) { + handleCycleCommand(false, commandData, commandDataLen); + } + else { + return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; + } - commandBuffer[0] = RM3100::CYCLE_COUNT_VALUE; - std::memcpy(commandBuffer + 1, &cycleCountRegValueX, 2); - std::memcpy(commandBuffer + 3, &cycleCountRegValueY, 2); - std::memcpy(commandBuffer + 5, &cycleCountRegValueZ, 2); - rawPacketLen = 7; - rawPacket = commandBuffer; - return HasReturnvaluesIF::RETURN_OK; + commandBuffer[0] = RM3100::CYCLE_COUNT_VALUE; + std::memcpy(commandBuffer + 1, &cycleCountRegValueX, 2); + std::memcpy(commandBuffer + 3, &cycleCountRegValueY, 2); + std::memcpy(commandBuffer + 5, &cycleCountRegValueZ, 2); + rawPacketLen = 7; + rawPacket = commandBuffer; + return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t MGMHandlerRM3100::handleCycleCommand(bool oneCycleValue, - const uint8_t *commandData, size_t commandDataLen) { - RM3100::CycleCountCommand command(oneCycleValue); - ReturnValue_t result = command.deSerialize(&commandData, &commandDataLen, - SerializeIF::Endianness::BIG); - if(result != HasReturnvaluesIF::RETURN_OK) { - return result; - } + const uint8_t *commandData, size_t commandDataLen) { + RM3100::CycleCountCommand command(oneCycleValue); + ReturnValue_t result = command.deSerialize(&commandData, &commandDataLen, + SerializeIF::Endianness::BIG); + if(result != HasReturnvaluesIF::RETURN_OK) { + return result; + } - // Data sheet p.30 - // "while noise limits the useful upper range to ~400 cycle counts." - if(command.cycleCountX > 450 ) { - return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; - } + /* Data sheet p.30 "while noise limits the useful upper range to ~400 cycle counts." */ + if(command.cycleCountX > 450 ) { + return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; + } - if(not oneCycleValue and - (command.cycleCountY > 450 or command.cycleCountZ > 450)) { - return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; - } + if(not oneCycleValue and (command.cycleCountY > 450 or command.cycleCountZ > 450)) { + return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; + } - cycleCountRegValueX = command.cycleCountX; - cycleCountRegValueY = command.cycleCountY; - cycleCountRegValueZ = command.cycleCountZ; - return HasReturnvaluesIF::RETURN_OK; + cycleCountRegValueX = command.cycleCountX; + cycleCountRegValueY = command.cycleCountY; + cycleCountRegValueZ = command.cycleCountZ; + return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t MGMHandlerRM3100::handleTmrcConfigCommand( - DeviceCommandId_t deviceCommand, const uint8_t *commandData, - size_t commandDataLen) { - if(commandData == nullptr) { - return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; - } + DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) { + if(commandData == nullptr) { + return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; + } - if(commandDataLen != 1) { - return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; - } + if(commandDataLen != 1) { + return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; + } - commandBuffer[0] = RM3100::TMRC_REGISTER; - commandBuffer[1] = commandData[1]; - rawPacketLen = 2; - rawPacket = commandBuffer; - return HasReturnvaluesIF::RETURN_OK; + commandBuffer[0] = RM3100::TMRC_REGISTER; + commandBuffer[1] = commandData[1]; + rawPacketLen = 2; + rawPacket = commandBuffer; + return HasReturnvaluesIF::RETURN_OK; } void MGMHandlerRM3100::fillCommandAndReplyMap() { - insertInCommandAndReplyMap(RM3100::CONFIGURE_CMM, 1); - insertInCommandAndReplyMap(RM3100::READ_CMM, 1); + insertInCommandAndReplyMap(RM3100::CONFIGURE_CMM, 1); + insertInCommandAndReplyMap(RM3100::READ_CMM, 1); - insertInCommandAndReplyMap(RM3100::CONFIGURE_TMRC, 1); - insertInCommandAndReplyMap(RM3100::READ_TMRC, 1); + insertInCommandAndReplyMap(RM3100::CONFIGURE_TMRC, 1); + insertInCommandAndReplyMap(RM3100::READ_TMRC, 1); - insertInCommandAndReplyMap(RM3100::CONFIGURE_CYCLE_COUNT, 1); - insertInCommandAndReplyMap(RM3100::READ_CYCLE_COUNT, 1); + insertInCommandAndReplyMap(RM3100::CONFIGURE_CYCLE_COUNT, 1); + insertInCommandAndReplyMap(RM3100::READ_CYCLE_COUNT, 1); - insertInCommandAndReplyMap(RM3100::READ_DATA, 1, &primaryDataset); + insertInCommandAndReplyMap(RM3100::READ_DATA, 1, &primaryDataset); } void MGMHandlerRM3100::modeChanged(void) { - internalState = InternalState::STATE_NONE; + internalState = InternalState::STATE_NONE; } ReturnValue_t MGMHandlerRM3100::initializeLocalDataPool( - localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(RM3100::FIELD_STRENGTH_X, - new PoolEntry({0.0})); - localDataPoolMap.emplace(RM3100::FIELD_STRENGTH_Y, - new PoolEntry({0.0})); - localDataPoolMap.emplace(RM3100::FIELD_STRENGTH_Z, - new PoolEntry({0.0})); - return HasReturnvaluesIF::RETURN_OK; + localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(RM3100::FIELD_STRENGTH_X, new PoolEntry({0.0})); + localDataPoolMap.emplace(RM3100::FIELD_STRENGTH_Y, new PoolEntry({0.0})); + localDataPoolMap.emplace(RM3100::FIELD_STRENGTH_Z, new PoolEntry({0.0})); + return HasReturnvaluesIF::RETURN_OK; } uint32_t MGMHandlerRM3100::getTransitionDelayMs(Mode_t from, Mode_t to) { - return 60000; + return 60000; } ReturnValue_t MGMHandlerRM3100::handleDataReadout(const uint8_t *packet) { - /* Analyze data here. The sensor generates 24 bit signed values so we need to do some bitshift - * trickery here to calculate the raw values first */ - int32_t fieldStrengthRawX = ((packet[1] << 24) | (packet[2] << 16) | (packet[3] << 8)) >> 8; - int32_t fieldStrengthRawY = ((packet[4] << 24) | (packet[5] << 16) | (packet[6] << 8)) >> 8; - int32_t fieldStrengthRawZ = ((packet[7] << 24) | (packet[8] << 16) | (packet[3] << 8)) >> 8; + /* Analyze data here. The sensor generates 24 bit signed values so we need to do some bitshift + * trickery here to calculate the raw values first */ + int32_t fieldStrengthRawX = ((packet[1] << 24) | (packet[2] << 16) | (packet[3] << 8)) >> 8; + int32_t fieldStrengthRawY = ((packet[4] << 24) | (packet[5] << 16) | (packet[6] << 8)) >> 8; + int32_t fieldStrengthRawZ = ((packet[7] << 24) | (packet[8] << 16) | (packet[3] << 8)) >> 8; - /* Now scale to physical value in microtesla */ - float fieldStrengthX = fieldStrengthRawX * scaleFactorX; + /* Now scale to physical value in microtesla */ + float fieldStrengthX = fieldStrengthRawX * scaleFactorX; float fieldStrengthY = fieldStrengthRawY * scaleFactorX; float fieldStrengthZ = fieldStrengthRawZ * scaleFactorX; #if OBSW_VERBOSE_LEVEL >= 1 - if(debugDivider->checkAndIncrement()) { - sif::info << "MGMHandlerLIS3: Magnetic field strength in" - " microtesla:" << std::endl; - /* Set terminal to utf-8 if there is an issue with micro printout. */ - sif::info << "X: " << fieldStrengthX << " \xC2\xB5T" << std::endl; - sif::info << "Y: " << fieldStrengthY << " \xC2\xB5T" << std::endl; - sif::info << "Z: " << fieldStrengthZ << " \xC2\xB5T" << std::endl; - } + if(debugDivider->checkAndIncrement()) { + sif::info << "MGMHandlerLIS3: Magnetic field strength in" + " microtesla:" << std::endl; + /* Set terminal to utf-8 if there is an issue with micro printout. */ + sif::info << "X: " << fieldStrengthX << " \xC2\xB5T" << std::endl; + sif::info << "Y: " << fieldStrengthY << " \xC2\xB5T" << std::endl; + sif::info << "Z: " << fieldStrengthZ << " \xC2\xB5T" << std::endl; + } #endif - /* TODO: Sanity check on values */ - PoolReadGuard readGuard(&primaryDataset); - if(readGuard.getReadResult() == HasReturnvaluesIF::RETURN_OK) { - primaryDataset.fieldStrengthX = fieldStrengthX; - primaryDataset.fieldStrengthY = fieldStrengthY; - primaryDataset.fieldStrengthZ = fieldStrengthZ; - primaryDataset.setValidity(true, true); - } - return RETURN_OK; + /* TODO: Sanity check on values */ + PoolReadGuard readGuard(&primaryDataset); + if(readGuard.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + primaryDataset.fieldStrengthX = fieldStrengthX; + primaryDataset.fieldStrengthY = fieldStrengthY; + primaryDataset.fieldStrengthZ = fieldStrengthZ; + primaryDataset.setValidity(true, true); + } + return RETURN_OK; }