From 9cdf5f0f67f4f90b6a0f0a186eb1b51613c1e38c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 6 Mar 2021 20:21:23 +0100 Subject: [PATCH] simple rm3100 test working now --- linux/boardtest/SpiTestClass.cpp | 64 +++++++++++++++++++++++++--- linux/boardtest/SpiTestClass.h | 2 +- mission/devices/MGMHandlerRM3100.cpp | 32 ++++++++------ 3 files changed, 77 insertions(+), 21 deletions(-) diff --git a/linux/boardtest/SpiTestClass.cpp b/linux/boardtest/SpiTestClass.cpp index ed73b518..a1e2cd8e 100644 --- a/linux/boardtest/SpiTestClass.cpp +++ b/linux/boardtest/SpiTestClass.cpp @@ -1,16 +1,18 @@ #include "SpiTestClass.h" #include + #include #include - -#include -#include #include #include + #include #include #include + +#include +#include #include #include #include @@ -69,7 +71,7 @@ void SpiTestClass::performRm3100Test(uint8_t mgmId) { else { currentGpioId = gpioIds::MGM_3_RM3100_CS; } - uint32_t rm3100speed = 195'300; + uint32_t rm3100speed = 976'000; uint8_t rm3100revidReg = 0x36; spi::SpiMode rm3100mode = spi::SpiMode::MODE_3; @@ -96,7 +98,6 @@ void SpiTestClass::performRm3100Test(uint8_t mgmId) { /* Write configuration to CMM register */ writeRegister(fileDescriptor, currentGpioId, 0x01, 0x75); - // TaskFactory::delayTask(10); uint8_t cmmRegister = readRm3100Register(fileDescriptor , currentGpioId, 0x01); sif::info << "SpiTestClass::performRm3100Test: CMM register value: " << std::hex << "0x" << static_cast(cmmRegister) << std::dec << std::endl; @@ -104,7 +105,54 @@ void SpiTestClass::performRm3100Test(uint8_t mgmId) { /* Read the cycle count registers */ uint8_t cycleCountsRaw[6]; readMultipleRegisters(fileDescriptor, currentGpioId, 0x04, cycleCountsRaw, 6); - arrayprinter::print(cycleCountsRaw, 6); + + uint16_t cycleCountX = cycleCountsRaw[0] << 8 | cycleCountsRaw[1]; + uint16_t cycleCountY = cycleCountsRaw[2] << 8 | cycleCountsRaw[3]; + uint16_t cycleCountZ = cycleCountsRaw[4] << 8 | cycleCountsRaw[5]; + + sif::info << "Cycle count X: " << cycleCountX << std::endl; + sif::info << "Cycle count Y: " << cycleCountY << std::endl; + sif::info << "Cycle count z: " << cycleCountZ << std::endl; + + writeRegister(fileDescriptor, currentGpioId, 0x0B, 0x95); + uint8_t tmrcReg = readRm3100Register(fileDescriptor, currentGpioId, 0x0B); + sif::info << "SpiTestClass::performRm3100Test: TMRC register value: " << + std::hex << "0x" << static_cast(tmrcReg) << std::dec << std::endl; + + TaskFactory::delayTask(10); + uint8_t statusReg = readRm3100Register(fileDescriptor, currentGpioId, 0x34); + sif::info << "SpiTestClass::performRm3100Test: Status Register 0b" << + std::bitset<8>(statusReg) << std::endl; + /* This means that data is not ready */ + if((statusReg & 0b1000'0000) == 0) { + sif::warning << "SpiTestClass::performRm3100Test: Data not ready!" << std::endl; + TaskFactory::delayTask(10); + uint8_t statusReg = readRm3100Register(fileDescriptor, currentGpioId, 0x34); + if((statusReg & 0b1000'0000) == 0) { + return; + } + } + + uint32_t rm3100DefaultCycleCout = 0xC8; + /* Gain scales lineary with cycle count and is 38 for cycle count 100 */ + float rm3100Gain = rm3100DefaultCycleCout / 100.0 * 38.0; + float scaleFactor = 1 / rm3100Gain; + uint8_t rawValues[9]; + readMultipleRegisters(fileDescriptor, currentGpioId, 0x24, rawValues, 9); + + /* The sensor generates 24 bit signed values */ + int32_t rawX = ((rawValues[0] << 24) | (rawValues[1] << 16) | (rawValues[2] << 8)) >> 8; + int32_t rawY = ((rawValues[3] << 24) | (rawValues[4] << 16) | (rawValues[5] << 8)) >> 8; + int32_t rawZ = ((rawValues[6] << 24) | (rawValues[7] << 16) | (rawValues[8] << 8)) >> 8; + + float fieldStrengthX = rawX * scaleFactor; + float fieldStrengthY = rawY * scaleFactor; + float fieldStrengthZ = rawZ * scaleFactor; + + sif::info << "RM3100 measured field strenghts in microtesla:" << std::endl; + sif::info << "Field Strength X: " << fieldStrengthX << " \xC2\xB5T" << std::endl; + sif::info << "Field Strength Y: " << fieldStrengthY << " \xC2\xB5T" << std::endl; + sif::info << "Field Strength Z: " << fieldStrengthZ << " \xC2\xB5T" << std::endl; } void SpiTestClass::performLis3MdlTest(uint8_t lis3Id) { @@ -242,10 +290,14 @@ void SpiTestClass::readMultipleRegisters(int fd, gpioId_t chipSelect, uint8_t re spiTransferStruct.len = len + 1; sendBuffer[0] = reg | STM_READ_MASK; + for(uint8_t idx = 0; idx < len ; idx ++) { sendBuffer[idx + 1] = 0; } + if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) { + gpioIF->pullLow(chipSelect); + } int retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct); if(retval < 0) { utility::handleIoctlError("SpiTestClass::readRegister: Read failed"); diff --git a/linux/boardtest/SpiTestClass.h b/linux/boardtest/SpiTestClass.h index 6b7bbbd2..190bc504 100644 --- a/linux/boardtest/SpiTestClass.h +++ b/linux/boardtest/SpiTestClass.h @@ -28,7 +28,7 @@ private: std::array recvBuffer; std::array sendBuffer; - struct spi_ioc_transfer spiTransferStruct; + struct spi_ioc_transfer spiTransferStruct = {}; void performRm3100Test(uint8_t mgmId); void performLis3MdlTest(uint8_t lis3Id); diff --git a/mission/devices/MGMHandlerRM3100.cpp b/mission/devices/MGMHandlerRM3100.cpp index 1b0db34d..bec149e7 100644 --- a/mission/devices/MGMHandlerRM3100.cpp +++ b/mission/devices/MGMHandlerRM3100.cpp @@ -292,16 +292,16 @@ ReturnValue_t MGMHandlerRM3100::handleTmrcConfigCommand( } void MGMHandlerRM3100::fillCommandAndReplyMap() { - insertInCommandAndReplyMap(RM3100::CONFIGURE_CMM, 2); - insertInCommandAndReplyMap(RM3100::READ_CMM, 2); + insertInCommandAndReplyMap(RM3100::CONFIGURE_CMM, 1); + insertInCommandAndReplyMap(RM3100::READ_CMM, 1); - insertInCommandAndReplyMap(RM3100::CONFIGURE_TMRC, 2); - insertInCommandAndReplyMap(RM3100::READ_TMRC, 2); + insertInCommandAndReplyMap(RM3100::CONFIGURE_TMRC, 1); + insertInCommandAndReplyMap(RM3100::READ_TMRC, 1); - insertInCommandAndReplyMap(RM3100::CONFIGURE_CYCLE_COUNT, 2); - insertInCommandAndReplyMap(RM3100::READ_CYCLE_COUNT, 2); + insertInCommandAndReplyMap(RM3100::CONFIGURE_CYCLE_COUNT, 1); + insertInCommandAndReplyMap(RM3100::READ_CYCLE_COUNT, 1); - insertInCommandAndReplyMap(RM3100::READ_DATA, 2, &primaryDataset); + insertInCommandAndReplyMap(RM3100::READ_DATA, 1, &primaryDataset); } void MGMHandlerRM3100::modeChanged(void) { @@ -324,13 +324,16 @@ uint32_t MGMHandlerRM3100::getTransitionDelayMs(Mode_t from, Mode_t to) { } ReturnValue_t MGMHandlerRM3100::handleDataReadout(const uint8_t *packet) { - /* Analyze data here. Field strengths in microtesla */ - int32_t fieldStrengthX = (packet[1] << 16 | packet[2] << 8 | packet[3]) - * scaleFactorX; - int32_t fieldStrengthY = (packet[4] << 16 | packet[5] << 8 | packet[6]) - * scaleFactorY; - int32_t fieldStrengthZ = (packet[7] << 16 | packet[8] << 8 | packet[9]) - * scaleFactorZ; + /* 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; + float fieldStrengthY = fieldStrengthRawY * scaleFactorX; + float fieldStrengthZ = fieldStrengthRawZ * scaleFactorX; #if OBSW_VERBOSE_LEVEL >= 1 if(debugDivider->checkAndIncrement()) { @@ -343,6 +346,7 @@ ReturnValue_t MGMHandlerRM3100::handleDataReadout(const uint8_t *packet) { } #endif + /* TODO: Sanity check on values */ PoolReadGuard readGuard(&primaryDataset); if(readGuard.getReadResult() == HasReturnvaluesIF::RETURN_OK) { primaryDataset.fieldStrengthX = fieldStrengthX;