From 271c7ce7ec67b7abcd2b2b6d415fb9912027e1bf Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Sat, 6 Mar 2021 14:01:18 +0100 Subject: [PATCH 01/13] made shell scripts executable --- cmake/scripts/RPi/create_cmake_debug_cfg.sh | 0 cmake/scripts/RPi/create_cmake_release_cfg.sh | 0 cmake/scripts/RPi/create_cmake_relwithdeb_cfg.sh | 0 cmake/scripts/RPi/create_cmake_size_cfg.sh | 0 4 files changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 cmake/scripts/RPi/create_cmake_debug_cfg.sh mode change 100644 => 100755 cmake/scripts/RPi/create_cmake_release_cfg.sh mode change 100644 => 100755 cmake/scripts/RPi/create_cmake_relwithdeb_cfg.sh mode change 100644 => 100755 cmake/scripts/RPi/create_cmake_size_cfg.sh diff --git a/cmake/scripts/RPi/create_cmake_debug_cfg.sh b/cmake/scripts/RPi/create_cmake_debug_cfg.sh old mode 100644 new mode 100755 diff --git a/cmake/scripts/RPi/create_cmake_release_cfg.sh b/cmake/scripts/RPi/create_cmake_release_cfg.sh old mode 100644 new mode 100755 diff --git a/cmake/scripts/RPi/create_cmake_relwithdeb_cfg.sh b/cmake/scripts/RPi/create_cmake_relwithdeb_cfg.sh old mode 100644 new mode 100755 diff --git a/cmake/scripts/RPi/create_cmake_size_cfg.sh b/cmake/scripts/RPi/create_cmake_size_cfg.sh old mode 100644 new mode 100755 From 9bf581a528c587df1365b6247a4dbeb6b53c890c Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Sat, 6 Mar 2021 14:06:46 +0100 Subject: [PATCH 02/13] updated project files --- cmake/scripts/Q7S/create_cmake_debug_cfg.sh | 0 cmake/scripts/Q7S/create_cmake_release_cfg.sh | 0 .../Q7S/create_cmake_relwithdeb_cfg.sh | 0 misc/eclipse/.cproject | 50 +++++++++---------- 4 files changed, 23 insertions(+), 27 deletions(-) mode change 100644 => 100755 cmake/scripts/Q7S/create_cmake_debug_cfg.sh mode change 100644 => 100755 cmake/scripts/Q7S/create_cmake_release_cfg.sh mode change 100644 => 100755 cmake/scripts/Q7S/create_cmake_relwithdeb_cfg.sh diff --git a/cmake/scripts/Q7S/create_cmake_debug_cfg.sh b/cmake/scripts/Q7S/create_cmake_debug_cfg.sh old mode 100644 new mode 100755 diff --git a/cmake/scripts/Q7S/create_cmake_release_cfg.sh b/cmake/scripts/Q7S/create_cmake_release_cfg.sh old mode 100644 new mode 100755 diff --git a/cmake/scripts/Q7S/create_cmake_relwithdeb_cfg.sh b/cmake/scripts/Q7S/create_cmake_relwithdeb_cfg.sh old mode 100644 new mode 100755 diff --git a/misc/eclipse/.cproject b/misc/eclipse/.cproject index c8b1c059..e5b4ecb0 100644 --- a/misc/eclipse/.cproject +++ b/misc/eclipse/.cproject @@ -218,9 +218,9 @@ - + - + @@ -233,7 +233,7 @@ - + - + @@ -382,7 +382,7 @@ - + - + @@ -531,7 +531,7 @@ - + - + @@ -676,7 +676,7 @@ - + - + @@ -837,9 +837,17 @@ + + make + -j16 + clean + true + true + true + make - -f Makefile-Hosted -j16 + -j16 hardclean true true @@ -852,25 +860,13 @@ - - - - - - - - - - + - - - - + From bf56ac90ad2fba5be45cb181e1717777bf0876a7 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Sat, 6 Mar 2021 15:13:28 +0100 Subject: [PATCH 03/13] added additional build config RPI flag --- bsp_rpi/ObjectFactory.cpp | 3 ++- misc/eclipse/.cproject | 17 +++++++++++------ 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/bsp_rpi/ObjectFactory.cpp b/bsp_rpi/ObjectFactory.cpp index 3e6eeaa2..a31d6b21 100644 --- a/bsp_rpi/ObjectFactory.cpp +++ b/bsp_rpi/ObjectFactory.cpp @@ -97,8 +97,9 @@ void ObjectFactory::produce(){ "GYRO_2_L3G", gpio::Direction::OUT, 1); gpioIF->addGpios(gpioCookieAcsBoard); + std::string spiDev = "/dev/spidev0.0"; SpiCookie* spiCookie = new SpiCookie(addresses::MGM_0_LIS3, - gpioIds::MGM_0_LIS3_CS, "/dev/spidev0.0", 24, spi::SpiMode::MODE_3, 3'900'000); + gpioIds::MGM_0_LIS3_CS, spiDev, 24, spi::SpiMode::MODE_3, 3'900'000); auto mgmHandler = new MGMHandlerLIS3MDL(objects::MGM_0_LIS3_HANDLER, objects::SPI_COM_IF, spiCookie); mgmHandler->setStartUpImmediately(); diff --git a/misc/eclipse/.cproject b/misc/eclipse/.cproject index e5b4ecb0..5b4f239b 100644 --- a/misc/eclipse/.cproject +++ b/misc/eclipse/.cproject @@ -15,7 +15,7 @@ - + @@ -322,6 +322,7 @@ @@ -332,6 +333,7 @@ @@ -813,21 +815,24 @@ - - - - - + + + + + + + + From 5f3d2b1516d9712886fc40840a05520c202b30cf Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Sat, 6 Mar 2021 18:12:50 +0100 Subject: [PATCH 04/13] started rm3100 testing --- bsp_rpi/ObjectFactory.cpp | 10 +- bsp_rpi/boardconfig/rpi_config.h | 4 +- fsfw | 2 +- fsfwconfig/OBSWConfig.h | 2 + .../PollingSequenceFactory.cpp | 21 +++- linux/boardtest/SpiTestClass.cpp | 81 +++++++++++--- linux/boardtest/SpiTestClass.h | 6 + linux/spi/SpiComIF.cpp | 13 +++ linux/spi/spiDefinitions.h | 4 +- mission/devices/MGMHandlerRM3100.cpp | 103 +++++++++--------- 10 files changed, 172 insertions(+), 74 deletions(-) diff --git a/bsp_rpi/ObjectFactory.cpp b/bsp_rpi/ObjectFactory.cpp index a31d6b21..ff0970a4 100644 --- a/bsp_rpi/ObjectFactory.cpp +++ b/bsp_rpi/ObjectFactory.cpp @@ -27,6 +27,7 @@ #include #include #include +#include void Factory::setStaticFrameworkObjectIds() { PusServiceBase::packetSource = objects::PUS_PACKET_DISTRIBUTOR; @@ -100,9 +101,14 @@ void ObjectFactory::produce(){ 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); - auto mgmHandler = new MGMHandlerLIS3MDL(objects::MGM_0_LIS3_HANDLER, + auto mgmLis3Handler = new MGMHandlerLIS3MDL(objects::MGM_0_LIS3_HANDLER, objects::SPI_COM_IF, spiCookie); - mgmHandler->setStartUpImmediately(); + mgmLis3Handler->setStartUpImmediately(); + spiCookie = new SpiCookie(addresses::MGM_1_RM3100, gpioIds::MGM_1_RM3100_CS, spiDev, 32, + spi::SpiMode::MODE_3, 976'000); + auto mgmRm3100Handler = new MGMHandlerRM3100(objects::MGM_1_RM3100_HANDLER, + objects::SPI_COM_IF, spiCookie); + mgmRm3100Handler->setStartUpImmediately(); #endif /* RPI_TEST_ACS_BOARD == 1 */ } diff --git a/bsp_rpi/boardconfig/rpi_config.h b/bsp_rpi/boardconfig/rpi_config.h index d1acacb6..be8dac4e 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 0 -#define RPI_TEST_ACS_BOARD 1 +#define RPI_ADD_SPI_TEST 1 +#define RPI_TEST_ACS_BOARD 0 /* Adapt these values accordingly */ namespace gpio { diff --git a/fsfw b/fsfw index d57955ad..17b8d3fe 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit d57955ade7944ccd895606f244ebd88f3e710eb0 +Subproject commit 17b8d3fed05cae6e208dc3f14b6ba0d44c9ec5ef diff --git a/fsfwconfig/OBSWConfig.h b/fsfwconfig/OBSWConfig.h index f40fbec3..a7b078a2 100644 --- a/fsfwconfig/OBSWConfig.h +++ b/fsfwconfig/OBSWConfig.h @@ -26,6 +26,8 @@ debugging. */ #define PDU2_DEBUG 0 #define ACU_DEBUG 1 +#define FSFW_LINUX_SPI_WIRETAPPING 1 + #ifdef __cplusplus #include "objects/systemObjectList.h" diff --git a/fsfwconfig/pollingsequence/PollingSequenceFactory.cpp b/fsfwconfig/pollingsequence/PollingSequenceFactory.cpp index 8b1e8e11..dc82c80d 100644 --- a/fsfwconfig/pollingsequence/PollingSequenceFactory.cpp +++ b/fsfwconfig/pollingsequence/PollingSequenceFactory.cpp @@ -99,15 +99,26 @@ ReturnValue_t pst::gomspacePstInit(FixedTimeslotTaskIF *thisSequence){ ReturnValue_t pst::pollingSequenceAcsTest(FixedTimeslotTaskIF *thisSequence) { uint32_t length = thisSequence->getPeriodMs(); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0, +// thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0, +// DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.2, +// DeviceHandlerIF::SEND_WRITE); +// thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.4, +// DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.6, +// DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.8, +// DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.2, + thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.4, + thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.6, + thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.8, + thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ); if (thisSequence->checkSequence() != HasReturnvaluesIF::RETURN_OK) { sif::error << "Initialization of ACS Board PST failed" << std::endl; diff --git a/linux/boardtest/SpiTestClass.cpp b/linux/boardtest/SpiTestClass.cpp index df32ec5b..ed73b518 100644 --- a/linux/boardtest/SpiTestClass.cpp +++ b/linux/boardtest/SpiTestClass.cpp @@ -2,9 +2,11 @@ #include #include +#include #include #include +#include #include #include #include @@ -19,7 +21,7 @@ SpiTestClass::SpiTestClass(object_id_t objectId, GpioIF* gpioIF): TestTask(objec if(gpioIF == nullptr) { sif::error << "SpiTestClass::SpiTestClass: Invalid GPIO ComIF!" << std::endl; } - testMode = TestModes::MGM_LIS3MDL; + testMode = TestModes::MGM_RM3100; spiTransferStruct.rx_buf = reinterpret_cast<__u64>(recvBuffer.data()); spiTransferStruct.tx_buf = reinterpret_cast<__u64>(sendBuffer.data()); } @@ -52,6 +54,9 @@ void SpiTestClass::performRm3100Test(uint8_t mgmId) { /* Configure all SPI chip selects and pull them high */ acsInit(); + /* Select between mgm3Rm3100ChipSelect and mgm1Rm3100ChipSelect here */ + mgmId = mgm1Rm3100ChipSelect; + /* Adapt accordingly */ if(mgmId != mgm1Rm3100ChipSelect and mgmId != mgm3Rm3100ChipSelect) { sif::warning << "SpiTestClass::performRm3100Test: Invalid MGM ID!" << std::endl; @@ -64,10 +69,10 @@ void SpiTestClass::performRm3100Test(uint8_t mgmId) { else { currentGpioId = gpioIds::MGM_3_RM3100_CS; } - uint32_t rm3100speed = 3'900'000; + uint32_t rm3100speed = 195'300; uint8_t rm3100revidReg = 0x36; spi::SpiMode rm3100mode = spi::SpiMode::MODE_3; - //spiTransferStruct.speed_hz = rm3100Speed; + #ifdef RASPBERRY_PI std::string deviceName = "/dev/spidev0.0"; #else @@ -85,9 +90,21 @@ void SpiTestClass::performRm3100Test(uint8_t mgmId) { } setSpiSpeedAndMode(fileDescriptor, rm3100mode, rm3100speed); - uint8_t revId = readStmRegister(fileDescriptor, currentGpioId, rm3100revidReg, false); + uint8_t revId = readRegister(fileDescriptor, currentGpioId, rm3100revidReg); sif::info << "SpiTestClass::performRm3100Test: Revision ID 0b" << std::bitset<8>(revId) << std::endl; + + /* 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; + + /* Read the cycle count registers */ + uint8_t cycleCountsRaw[6]; + readMultipleRegisters(fileDescriptor, currentGpioId, 0x04, cycleCountsRaw, 6); + arrayprinter::print(cycleCountsRaw, 6); } void SpiTestClass::performLis3MdlTest(uint8_t lis3Id) { @@ -128,6 +145,7 @@ void SpiTestClass::performLis3MdlTest(uint8_t lis3Id) { uint8_t whoAmIRegVal = readStmRegister(fileDescriptor, currentGpioId, whoAmIReg, false); sif::info << "SpiTestClass::performLis3MdlTest: WHO AM I Regiter 0b" << std::bitset<8>(whoAmIRegVal) << std::endl; + } @@ -174,11 +192,7 @@ void SpiTestClass::acsInit() { } } -void SpiTestClass::writeStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value, - bool autoIncrement) { - if(autoIncrement) { - reg |= STM_AUTO_INCR_MASK; - } +void SpiTestClass::writeRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value) { spiTransferStruct.len = 2; sendBuffer[0] = reg; sendBuffer[1] = value; @@ -187,16 +201,25 @@ void SpiTestClass::writeStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, ui gpioIF->pullLow(chipSelect); } int retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct); - if(retval != 0) { - utility::handleIoctlError("SpiTestClass::writeStmRegister: Write failed"); + if(retval < 0) { + utility::handleIoctlError("SpiTestClass::writeRegister: Write failed"); } if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) { gpioIF->pullHigh(chipSelect); } } +void SpiTestClass::writeStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value, + bool autoIncrement) { + if(autoIncrement) { + reg |= STM_AUTO_INCR_MASK; + } + writeRegister(fd, chipSelect, reg, value); +} + void SpiTestClass::setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed) { - int retval = ioctl(spiFd, SPI_IOC_WR_MODE, reinterpret_cast(&mode)); + int mode_test = SPI_MODE_3; + int retval = ioctl(spiFd, SPI_IOC_WR_MODE, &mode_test);//reinterpret_cast(&mode)); if(retval != 0) { utility::handleIoctlError("SpiTestClass::performRm3100Test: Setting SPI mode failed!"); } @@ -207,12 +230,43 @@ void SpiTestClass::setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t spe } } +uint8_t SpiTestClass::readRm3100Register(int fd, gpioId_t chipSelect, uint8_t reg) { + return readStmRegister(fd, chipSelect, reg, false); +} + +void SpiTestClass::readMultipleRegisters(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t *reply, + size_t len) { + if(reply == NULL) { + return; + } + + spiTransferStruct.len = len + 1; + sendBuffer[0] = reg | STM_READ_MASK; + for(uint8_t idx = 0; idx < len ; idx ++) { + sendBuffer[idx + 1] = 0; + } + + int retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct); + if(retval < 0) { + utility::handleIoctlError("SpiTestClass::readRegister: Read failed"); + } + if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) { + gpioIF->pullHigh(chipSelect); + } + std::memcpy(reply, recvBuffer.data() + 1, len); +} + uint8_t SpiTestClass::readStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, bool autoIncrement) { reg |= STM_READ_MASK; if(autoIncrement) { reg |= STM_AUTO_INCR_MASK; } + return readRegister(fd, chipSelect, reg); +} + + +uint8_t SpiTestClass::readRegister(int fd, gpioId_t chipSelect, uint8_t reg) { spiTransferStruct.len = 2; sendBuffer[0] = reg; sendBuffer[1] = 0; @@ -222,10 +276,11 @@ uint8_t SpiTestClass::readStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, } int retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct); if(retval < 0) { - utility::handleIoctlError("SpiTestClass::readStmRegiste: Read failed"); + utility::handleIoctlError("SpiTestClass::readRegister: Read failed"); } if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) { gpioIF->pullHigh(chipSelect); } return recvBuffer[1]; } + diff --git a/linux/boardtest/SpiTestClass.h b/linux/boardtest/SpiTestClass.h index 61c2d2b2..6b7bbbd2 100644 --- a/linux/boardtest/SpiTestClass.h +++ b/linux/boardtest/SpiTestClass.h @@ -46,11 +46,17 @@ private: uint8_t mgm3Rm3100ChipSelect = 27; static constexpr uint8_t STM_READ_MASK = 0b1000'0000; + static constexpr uint8_t RM3100_READ_MASK = STM_READ_MASK; static constexpr uint8_t STM_AUTO_INCR_MASK = 0b0100'0000; void setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed); + void writeRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value); void writeStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value, bool autoIncrement); + void readMultipleRegisters(int fd, gpioId_t chipSelect, uint8_t reg, + uint8_t* reply, size_t len); + uint8_t readRegister(int fd, gpioId_t chipSelect, uint8_t reg); + uint8_t readRm3100Register(int fd, gpioId_t chipSelect, uint8_t reg); uint8_t readStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, bool autoIncrement); }; diff --git a/linux/spi/SpiComIF.cpp b/linux/spi/SpiComIF.cpp index eef44126..f678ce91 100644 --- a/linux/spi/SpiComIF.cpp +++ b/linux/spi/SpiComIF.cpp @@ -1,4 +1,5 @@ #include "SpiComIF.h" +#include #include #include @@ -188,6 +189,18 @@ ReturnValue_t SpiComIF::sendMessage(CookieIF *cookie, const uint8_t *sendData, s utility::handleIoctlError("SpiComIF::sendMessage: ioctl error."); result = FULL_DUPLEX_TRANSFER_FAILED; } +#if FSFW_LINUX_SPI_WIRETAPPING == 1 +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::info << "Sent SPI data: " << std::endl; + size_t dataLen = spiCookie->getTransferStructHandle()->len; + uint8_t* dataPtr = reinterpret_cast(spiCookie->getTransferStructHandle()->tx_buf); + arrayprinter::print(dataPtr, dataLen, OutputType::HEX, false); + sif::info << "Received SPI data: " << std::endl; + dataPtr = reinterpret_cast(spiCookie->getTransferStructHandle()->rx_buf); + arrayprinter::print(dataPtr, dataLen, OutputType::HEX, false); +#else +#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */ +#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */ } else { /* We write with a blocking half-duplex transfer here */ diff --git a/linux/spi/spiDefinitions.h b/linux/spi/spiDefinitions.h index 9c278a70..b4310697 100644 --- a/linux/spi/spiDefinitions.h +++ b/linux/spi/spiDefinitions.h @@ -1,9 +1,11 @@ #ifndef LINUX_SPI_SPIDEFINITONS_H_ #define LINUX_SPI_SPIDEFINITONS_H_ +#include + namespace spi { -enum SpiMode { +enum SpiMode: uint8_t { MODE_0, MODE_1, MODE_2, diff --git a/mission/devices/MGMHandlerRM3100.cpp b/mission/devices/MGMHandlerRM3100.cpp index 76d3dd70..1b0db34d 100644 --- a/mission/devices/MGMHandlerRM3100.cpp +++ b/mission/devices/MGMHandlerRM3100.cpp @@ -1,3 +1,4 @@ +#include #include "MGMHandlerRM3100.h" #include @@ -17,28 +18,37 @@ MGMHandlerRM3100::MGMHandlerRM3100(object_id_t objectId, MGMHandlerRM3100::~MGMHandlerRM3100() {} void MGMHandlerRM3100::doStartUp() { - if(internalState == InternalState::STATE_NONE) { - internalState = InternalState::STATE_CONFIGURE_CMM; - } - - if(internalState == InternalState::STATE_CONFIGURE_CMM) { - internalState = InternalState::STATE_READ_CMM; - } - else if(internalState == InternalState::STATE_READ_CMM) { - if(commandExecuted) { - internalState = InternalState::STATE_CONFIGURE_TMRC; - } - } - - if(internalState == InternalState::STATE_CONFIGURE_TMRC) { - internalState = InternalState::STATE_READ_TMRC; - } - else if(internalState == InternalState::STATE_READ_TMRC) { - if(commandExecuted) { - internalState = InternalState::STATE_NORMAL; - setMode(_MODE_TO_ON); - } - } + switch(internalState) { + case(InternalState::STATE_NONE): { + internalState = InternalState::STATE_CONFIGURE_CMM; + break; + } + case(InternalState::STATE_CONFIGURE_CMM): { + internalState = InternalState::STATE_READ_CMM; + break; + } + case(InternalState::STATE_READ_CMM): { + if(commandExecuted) { + internalState = InternalState::STATE_CONFIGURE_TMRC; + } + break; + } + case(InternalState::STATE_CONFIGURE_TMRC): { + internalState = InternalState::STATE_READ_TMRC; + break; + } + case(InternalState::STATE_READ_TMRC): { + if(commandExecuted) { + internalState = InternalState::STATE_NORMAL; + //setMode(_MODE_TO_ON); + setMode(MODE_NORMAL); + } + break; + } + default: { + break; + } + } } void MGMHandlerRM3100::doShutDown() { @@ -141,15 +151,10 @@ ReturnValue_t MGMHandlerRM3100::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { - // SPI, ID will always be the one of the last sent command. + /* For SPI, ID will always be the one of the last sent command. */ *foundId = this->getPendingCommand(); - *foundLen = this->rawPacketLen; - - // Data with SPI Interface has always this answer - if (start[0] == 0b11111111) { - return RETURN_OK; - } - return DeviceHandlerIF::INVALID_DATA; + *foundLen = len; + return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t MGMHandlerRM3100::interpretDeviceReply( @@ -159,7 +164,7 @@ ReturnValue_t MGMHandlerRM3100::interpretDeviceReply( case(RM3100::CONFIGURE_CMM): case(RM3100::CONFIGURE_CYCLE_COUNT): case(RM3100::CONFIGURE_TMRC): { - // We can only check whether write was sucessful with read operation. + /* We can only check whether write was successful with read operation. */ break; } case(RM3100::READ_CMM): { @@ -167,7 +172,7 @@ ReturnValue_t MGMHandlerRM3100::interpretDeviceReply( commandExecuted = true; } else { - // Attempt reconfiguration. + /* Attempt reconfiguration. */ internalState = InternalState::STATE_CONFIGURE_CMM; return DeviceHandlerIF::DEVICE_REPLY_INVALID; } @@ -176,13 +181,13 @@ ReturnValue_t MGMHandlerRM3100::interpretDeviceReply( case(RM3100::READ_TMRC): { if(packet[1] == tmrcRegValue) { commandExecuted = true; - // Reading TMRC was commanded. Trigger event to inform ground. + /* Reading TMRC was commanded. Trigger event to inform ground. */ if(mode != _MODE_START_UP) { triggerEvent(tmrcSet, tmrcRegValue, 0); } } else { - // Attempt reconfiguration. + /* Attempt reconfiguration. */ internalState = InternalState::STATE_CONFIGURE_TMRC; return DeviceHandlerIF::DEVICE_REPLY_INVALID; } @@ -197,7 +202,7 @@ ReturnValue_t MGMHandlerRM3100::interpretDeviceReply( cycleCountZ != cycleCountRegValueZ) { return DeviceHandlerIF::DEVICE_REPLY_INVALID; } - // Reading TMRC was commanded. Trigger event to inform ground. + /* Reading TMRC was commanded. Trigger event to inform ground. */ if(mode != _MODE_START_UP) { uint32_t eventParam1 = cycleCountX << 16 | cycleCountY; triggerEvent(cycleCountersSet, eventParam1, cycleCountZ); @@ -287,16 +292,16 @@ ReturnValue_t MGMHandlerRM3100::handleTmrcConfigCommand( } void MGMHandlerRM3100::fillCommandAndReplyMap() { - insertInCommandAndReplyMap(RM3100::CONFIGURE_CMM, 1); - insertInCommandAndReplyMap(RM3100::READ_CMM, 1); + insertInCommandAndReplyMap(RM3100::CONFIGURE_CMM, 2); + insertInCommandAndReplyMap(RM3100::READ_CMM, 2); - insertInCommandAndReplyMap(RM3100::CONFIGURE_TMRC, 1); - insertInCommandAndReplyMap(RM3100::READ_TMRC, 1); + insertInCommandAndReplyMap(RM3100::CONFIGURE_TMRC, 2); + insertInCommandAndReplyMap(RM3100::READ_TMRC, 2); - insertInCommandAndReplyMap(RM3100::CONFIGURE_CYCLE_COUNT, 1); - insertInCommandAndReplyMap(RM3100::READ_CYCLE_COUNT, 1); + insertInCommandAndReplyMap(RM3100::CONFIGURE_CYCLE_COUNT, 2); + insertInCommandAndReplyMap(RM3100::READ_CYCLE_COUNT, 2); - insertInCommandAndReplyMap(RM3100::READ_DATA, 1, &primaryDataset); + insertInCommandAndReplyMap(RM3100::READ_DATA, 2, &primaryDataset); } void MGMHandlerRM3100::modeChanged(void) { @@ -315,12 +320,11 @@ ReturnValue_t MGMHandlerRM3100::initializeLocalDataPool( } uint32_t MGMHandlerRM3100::getTransitionDelayMs(Mode_t from, Mode_t to) { - return 5000; + return 60000; } ReturnValue_t MGMHandlerRM3100::handleDataReadout(const uint8_t *packet) { - // analyze data here. - // Field strengths in micro Tesla + /* 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]) @@ -332,20 +336,19 @@ ReturnValue_t MGMHandlerRM3100::handleDataReadout(const uint8_t *packet) { 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. + /* 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 - ReturnValue_t result = primaryDataset.read(); - if(result == HasReturnvaluesIF::RETURN_OK) { + PoolReadGuard readGuard(&primaryDataset); + if(readGuard.getReadResult() == HasReturnvaluesIF::RETURN_OK) { primaryDataset.fieldStrengthX = fieldStrengthX; primaryDataset.fieldStrengthY = fieldStrengthY; primaryDataset.fieldStrengthZ = fieldStrengthZ; primaryDataset.setValidity(true, true); - result = primaryDataset.commit(); } - return result; + return RETURN_OK; } From 32670f4a8151d55a152cec53ff5a482e868149a1 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Sat, 6 Mar 2021 20:21:23 +0100 Subject: [PATCH 05/13] 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; From 88f84c439370683fe9cae0be2c8ce7e059b91040 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Sat, 6 Mar 2021 20:37:17 +0100 Subject: [PATCH 06/13] rm3100 tested now as well --- bsp_rpi/ObjectFactory.cpp | 6 +- bsp_rpi/boardconfig/rpi_config.h | 4 +- fsfw | 2 +- fsfwconfig/OBSWConfig.h | 3 +- mission/devices/MGMHandlerRM3100.cpp | 491 +++++++++++++-------------- 5 files changed, 252 insertions(+), 254 deletions(-) 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; } From 9ddbdbe19e7304124501dde299a279ecbdaf39e1 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Sat, 6 Mar 2021 20:49:22 +0100 Subject: [PATCH 07/13] both magnetic sensors working at once now --- .../PollingSequenceFactory.cpp | 20 +++++++++---------- mission/devices/MGMHandlerRM3100.cpp | 7 +++---- 2 files changed, 13 insertions(+), 14 deletions(-) diff --git a/fsfwconfig/pollingsequence/PollingSequenceFactory.cpp b/fsfwconfig/pollingsequence/PollingSequenceFactory.cpp index dc82c80d..6fd92ec6 100644 --- a/fsfwconfig/pollingsequence/PollingSequenceFactory.cpp +++ b/fsfwconfig/pollingsequence/PollingSequenceFactory.cpp @@ -99,16 +99,16 @@ ReturnValue_t pst::gomspacePstInit(FixedTimeslotTaskIF *thisSequence){ ReturnValue_t pst::pollingSequenceAcsTest(FixedTimeslotTaskIF *thisSequence) { uint32_t length = thisSequence->getPeriodMs(); -// thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0, -// DeviceHandlerIF::PERFORM_OPERATION); -// thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.2, -// DeviceHandlerIF::SEND_WRITE); -// thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.4, -// DeviceHandlerIF::GET_WRITE); -// thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.6, -// DeviceHandlerIF::SEND_READ); -// thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.8, -// DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.2, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.4, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.6, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.8, + DeviceHandlerIF::GET_READ); thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); diff --git a/mission/devices/MGMHandlerRM3100.cpp b/mission/devices/MGMHandlerRM3100.cpp index 154f3574..2f6cef0e 100644 --- a/mission/devices/MGMHandlerRM3100.cpp +++ b/mission/devices/MGMHandlerRM3100.cpp @@ -41,8 +41,7 @@ void MGMHandlerRM3100::doStartUp() { case(InternalState::STATE_READ_TMRC): { if(commandExecuted) { internalState = InternalState::STATE_NORMAL; - //setMode(_MODE_TO_ON); - setMode(MODE_NORMAL); + setMode(_MODE_TO_ON); } break; } @@ -317,7 +316,7 @@ ReturnValue_t MGMHandlerRM3100::initializeLocalDataPool( } uint32_t MGMHandlerRM3100::getTransitionDelayMs(Mode_t from, Mode_t to) { - return 60000; + return 10000; } ReturnValue_t MGMHandlerRM3100::handleDataReadout(const uint8_t *packet) { @@ -334,7 +333,7 @@ ReturnValue_t MGMHandlerRM3100::handleDataReadout(const uint8_t *packet) { #if OBSW_VERBOSE_LEVEL >= 1 if(debugDivider->checkAndIncrement()) { - sif::info << "MGMHandlerLIS3: Magnetic field strength in" + sif::info << "MGMHandlerRM3100: 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; From a581ce8bd649a77ba191a473b8e32c1b777ce8f9 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Sun, 7 Mar 2021 12:50:41 +0100 Subject: [PATCH 08/13] added l3g handler --- bsp_rpi/boardconfig/rpi_config.h | 4 +- linux/boardtest/SpiTestClass.cpp | 164 +++++++++++++++++++++++--- linux/boardtest/SpiTestClass.h | 17 ++- mission/devices/GyroL3GD20Handler.cpp | 2 +- 4 files changed, 166 insertions(+), 21 deletions(-) diff --git a/bsp_rpi/boardconfig/rpi_config.h b/bsp_rpi/boardconfig/rpi_config.h index d1acacb6..be8dac4e 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 0 -#define RPI_TEST_ACS_BOARD 1 +#define RPI_ADD_SPI_TEST 1 +#define RPI_TEST_ACS_BOARD 0 /* Adapt these values accordingly */ namespace gpio { diff --git a/linux/boardtest/SpiTestClass.cpp b/linux/boardtest/SpiTestClass.cpp index a1e2cd8e..f3077ad4 100644 --- a/linux/boardtest/SpiTestClass.cpp +++ b/linux/boardtest/SpiTestClass.cpp @@ -23,7 +23,7 @@ SpiTestClass::SpiTestClass(object_id_t objectId, GpioIF* gpioIF): TestTask(objec if(gpioIF == nullptr) { sif::error << "SpiTestClass::SpiTestClass: Invalid GPIO ComIF!" << std::endl; } - testMode = TestModes::MGM_RM3100; + testMode = TestModes::GYRO_L3GD20H; spiTransferStruct.rx_buf = reinterpret_cast<__u64>(recvBuffer.data()); spiTransferStruct.tx_buf = reinterpret_cast<__u64>(sendBuffer.data()); } @@ -42,6 +42,7 @@ ReturnValue_t SpiTestClass::performOneShotAction() { break; } case(TestModes::GYRO_L3GD20H): { + performL3gTest(gyro1L3gd20ChipSelect); break; } } @@ -56,9 +57,6 @@ void SpiTestClass::performRm3100Test(uint8_t mgmId) { /* Configure all SPI chip selects and pull them high */ acsInit(); - /* Select between mgm3Rm3100ChipSelect and mgm1Rm3100ChipSelect here */ - mgmId = mgm1Rm3100ChipSelect; - /* Adapt accordingly */ if(mgmId != mgm1Rm3100ChipSelect and mgmId != mgm3Rm3100ChipSelect) { sif::warning << "SpiTestClass::performRm3100Test: Invalid MGM ID!" << std::endl; @@ -191,12 +189,108 @@ void SpiTestClass::performLis3MdlTest(uint8_t lis3Id) { setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed); uint8_t whoAmIRegVal = readStmRegister(fileDescriptor, currentGpioId, whoAmIReg, false); - sif::info << "SpiTestClass::performLis3MdlTest: WHO AM I Regiter 0b" << + sif::info << "SpiTestClass::performLis3MdlTest: WHO AM I register 0b" << std::bitset<8>(whoAmIRegVal) << std::endl; } +void SpiTestClass::performL3gTest(uint8_t l3gId) { + /* Configure all SPI chip selects and pull them high */ + acsInit(); + + l3gId = gyro2L3gd20ChipSelect; + + /* Adapt accordingly */ + if(l3gId != gyro1L3gd20ChipSelect and l3gId != gyro2L3gd20ChipSelect) { + sif::warning << "SpiTestClass::performLis3MdlTest: Invalid MGM ID!" << std::endl; + } + gpioId_t currentGpioId = 0; + uint8_t chipSelectPin = l3gId; + uint8_t whoAmIReg = 0b0000'1111; + uint8_t whoAmIRegExpectedVal = 0b1101'0111; + + if(chipSelectPin == gyro1L3gd20ChipSelect) { + currentGpioId = gpioIds::GYRO_1_L3G_CS; + } + else { + currentGpioId = gpioIds::GYRO_2_L3G_CS; + } + uint32_t spiSpeed = 3'900'000; + spi::SpiMode spiMode = spi::SpiMode::MODE_3; +#ifdef RASPBERRY_PI + std::string deviceName = "/dev/spidev0.0"; +#else + std::string deviceName = "placeholder"; +#endif + int fileDescriptor = 0; + + utility::UnixFileHelper fileHelper(deviceName, &fileDescriptor, O_RDWR, + "SpiComIF::initializeInterface: "); + if(fileHelper.getOpenResult()) { + sif::error << "SpiTestClass::performLis3Mdl3100Test: File descriptor could not be opened!" + << std::endl; + return; + } + setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed); + uint8_t whoAmIRegVal = readStmRegister(fileDescriptor, currentGpioId, whoAmIReg, false); + sif::info << "SpiTestClass::performLis3MdlTest: WHO AM I register 0b" << + std::bitset<8>(whoAmIRegVal) << std::endl; + if(whoAmIRegVal != whoAmIRegExpectedVal) { + sif::warning << "SpiTestClass::performL3gTest: Read WHO AM I register invalid!" << + std::endl; + } + + uint8_t ctrlReg1Addr = 0b0010'0000; + { + uint8_t commandRegs[5]; + commandRegs[0] = 0b0000'1111; + commandRegs[1] = 0x0; + commandRegs[2] = 0x0; + /* Configure big endian data format */ + commandRegs[3] = 0b0100'0000; + commandRegs[4] = 0x0; + writeMultipleStmRegisters(fileDescriptor, currentGpioId, ctrlReg1Addr, commandRegs, + sizeof(commandRegs)); + uint8_t readRegs[5]; + readMultipleRegisters(fileDescriptor, currentGpioId, ctrlReg1Addr, readRegs, + sizeof(readRegs)); + for(uint8_t idx = 0; idx < sizeof(readRegs); idx++) { + if(readRegs[idx] != commandRegs[0]) { + sif::warning << "SpiTestClass::performL3gTest: Read control register" << + static_cast(idx + 1) << "not equal to configured value" << std::endl; + } + } + } + + uint8_t readOutBuffer[14]; + readMultipleStmRegisters(fileDescriptor, currentGpioId, ctrlReg1Addr, readOutBuffer, + sizeof(readOutBuffer)); + + uint8_t statusReg = readOutBuffer[7]; + sif::info << "SpiTestClass::performL3gTest: Status Register 0b" << + std::bitset<8>(statusReg) << std::endl; + + uint16_t l3gRange = 245; + float scaleFactor = static_cast(l3gRange) / INT16_MAX; + /* The sensor spits out little endian */ + int16_t angVelocRawX = (readOutBuffer[8] << 8) | readOutBuffer[9]; + int16_t angVelocRawY = (readOutBuffer[10] << 8) | readOutBuffer[11]; + int16_t angVelocRawZ = (readOutBuffer[12] << 8) | readOutBuffer[13]; + + float angVelocX = scaleFactor * angVelocRawX; + float angVelocY = scaleFactor * angVelocRawY; + float angVelocZ = scaleFactor * angVelocRawZ; + + sif::info << "Angular velocities for the L3GD20H in degrees per second:" << std::endl; + sif::info << "X: " << angVelocX << std::endl; + sif::info << "Y: " << angVelocY << std::endl; + sif::info << "Z: " << angVelocZ << std::endl; + +} + + + void SpiTestClass::acsInit() { GpioCookie* gpioCookie = new GpioCookie(); std::string rpiGpioName = "gpiochip0"; @@ -240,6 +334,19 @@ void SpiTestClass::acsInit() { } } +void SpiTestClass::setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed) { + int mode_test = SPI_MODE_3; + int retval = ioctl(spiFd, SPI_IOC_WR_MODE, &mode_test);//reinterpret_cast(&mode)); + if(retval != 0) { + utility::handleIoctlError("SpiTestClass::performRm3100Test: Setting SPI mode failed!"); + } + + retval = ioctl(spiFd, SPI_IOC_WR_MAX_SPEED_HZ, &speed); + if(retval != 0) { + utility::handleIoctlError("SpiTestClass::performRm3100Test: Setting SPI speed failed!"); + } +} + void SpiTestClass::writeRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value) { spiTransferStruct.len = 2; sendBuffer[0] = reg; @@ -265,16 +372,38 @@ void SpiTestClass::writeStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, ui writeRegister(fd, chipSelect, reg, value); } -void SpiTestClass::setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed) { - int mode_test = SPI_MODE_3; - int retval = ioctl(spiFd, SPI_IOC_WR_MODE, &mode_test);//reinterpret_cast(&mode)); - if(retval != 0) { - utility::handleIoctlError("SpiTestClass::performRm3100Test: Setting SPI mode failed!"); +void SpiTestClass::writeMultipleStmRegisters(int fd, gpioId_t chipSelect, uint8_t reg, + uint8_t *values, size_t len) { + if(values == nullptr) { + return; } - retval = ioctl(spiFd, SPI_IOC_WR_MAX_SPEED_HZ, &speed); - if(retval != 0) { - utility::handleIoctlError("SpiTestClass::performRm3100Test: Setting SPI speed failed!"); + reg |= STM_AUTO_INCR_MASK; + /* Clear read mask */ + reg &= ~STM_READ_MASK; + writeMultipleRegisters(fd, chipSelect, reg, values, len); + +} + +void SpiTestClass::writeMultipleRegisters(int fd, gpioId_t chipSelect, uint8_t reg, + uint8_t *values, size_t len) { + if(values == nullptr) { + return; + } + + sendBuffer[0] = reg; + std::memcpy(sendBuffer.data() + 1, values, len); + spiTransferStruct.len = len + 1; + + 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"); + } + if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) { + gpioIF->pullHigh(chipSelect); } } @@ -282,9 +411,16 @@ uint8_t SpiTestClass::readRm3100Register(int fd, gpioId_t chipSelect, uint8_t re return readStmRegister(fd, chipSelect, reg, false); } + +void SpiTestClass::readMultipleStmRegisters(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t *reply, + size_t len) { + reg |= STM_AUTO_INCR_MASK; + readMultipleRegisters(fd, chipSelect, reg, reply, len); +} + void SpiTestClass::readMultipleRegisters(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t *reply, size_t len) { - if(reply == NULL) { + if(reply == nullptr) { return; } diff --git a/linux/boardtest/SpiTestClass.h b/linux/boardtest/SpiTestClass.h index 190bc504..5998022b 100644 --- a/linux/boardtest/SpiTestClass.h +++ b/linux/boardtest/SpiTestClass.h @@ -32,6 +32,7 @@ private: void performRm3100Test(uint8_t mgmId); void performLis3MdlTest(uint8_t lis3Id); + void performL3gTest(uint8_t l3gId); /* ACS board specific code which pulls all GPIOs high */ void acsInit(); @@ -50,14 +51,22 @@ private: static constexpr uint8_t STM_AUTO_INCR_MASK = 0b0100'0000; void setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed); - void writeRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value); + void writeStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value, bool autoIncrement); - void readMultipleRegisters(int fd, gpioId_t chipSelect, uint8_t reg, - uint8_t* reply, size_t len); - uint8_t readRegister(int fd, gpioId_t chipSelect, uint8_t reg); + void writeMultipleStmRegisters(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t* values, + size_t len); + void writeMultipleRegisters(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t *values, + size_t len); + void writeRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value); + uint8_t readRm3100Register(int fd, gpioId_t chipSelect, uint8_t reg); uint8_t readStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, bool autoIncrement); + uint8_t readRegister(int fd, gpioId_t chipSelect, uint8_t reg); + void readMultipleStmRegisters(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t *reply, + size_t len); + void readMultipleRegisters(int fd, gpioId_t chipSelect, uint8_t reg, + uint8_t* reply, size_t len); }; diff --git a/mission/devices/GyroL3GD20Handler.cpp b/mission/devices/GyroL3GD20Handler.cpp index 997ec129..364c8728 100644 --- a/mission/devices/GyroL3GD20Handler.cpp +++ b/mission/devices/GyroL3GD20Handler.cpp @@ -190,7 +190,7 @@ ReturnValue_t GyroHandler::interpretDeviceReply(DeviceCommandId_t id, uint32_t GyroHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { - return 5000; + return 20000; } ReturnValue_t GyroHandler::initializeLocalDataPool( From b1dc0122b7aeef1741970a5ed2de4c6235538dc4 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Sun, 7 Mar 2021 12:53:10 +0100 Subject: [PATCH 09/13] using big endian now for gyro --- mission/devices/GyroL3GD20Handler.cpp | 26 ++++++++--------- mission/devices/GyroL3GD20Handler.h | 6 ++-- .../devicedefinitions/GyroL3GD20Definitions.h | 29 ++++++++++--------- 3 files changed, 31 insertions(+), 30 deletions(-) diff --git a/mission/devices/GyroL3GD20Handler.cpp b/mission/devices/GyroL3GD20Handler.cpp index 364c8728..279edf73 100644 --- a/mission/devices/GyroL3GD20Handler.cpp +++ b/mission/devices/GyroL3GD20Handler.cpp @@ -1,15 +1,15 @@ #include #include -GyroHandler::GyroHandler(object_id_t objectId, object_id_t deviceCommunication, +GyroHandlerL3GD20H::GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie): DeviceHandlerBase(objectId, deviceCommunication, comCookie), dataset(this) { } -GyroHandler::~GyroHandler() {} +GyroHandlerL3GD20H::~GyroHandlerL3GD20H() {} -void GyroHandler::doStartUp() { +void GyroHandlerL3GD20H::doStartUp() { if(internalState == InternalState::STATE_NONE) { internalState = InternalState::STATE_CONFIGURE; } @@ -22,11 +22,11 @@ void GyroHandler::doStartUp() { } } -void GyroHandler::doShutDown() { +void GyroHandlerL3GD20H::doShutDown() { setMode(_MODE_POWER_DOWN); } -ReturnValue_t GyroHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { +ReturnValue_t GyroHandlerL3GD20H::buildTransitionDeviceCommand(DeviceCommandId_t *id) { switch(internalState) { case(InternalState::STATE_NONE): case(InternalState::STATE_NORMAL): { @@ -52,12 +52,12 @@ ReturnValue_t GyroHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { return HasReturnvaluesIF::RETURN_OK; } -ReturnValue_t GyroHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { +ReturnValue_t GyroHandlerL3GD20H::buildNormalDeviceCommand(DeviceCommandId_t *id) { *id = L3GD20H::READ_REGS; return buildCommandFromCommand(*id, nullptr, 0); } -ReturnValue_t GyroHandler::buildCommandFromCommand( +ReturnValue_t GyroHandlerL3GD20H::buildCommandFromCommand( DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) { switch(deviceCommand) { @@ -120,7 +120,7 @@ ReturnValue_t GyroHandler::buildCommandFromCommand( return HasReturnvaluesIF::RETURN_OK; } -ReturnValue_t GyroHandler::scanForReply(const uint8_t *start, size_t len, +ReturnValue_t GyroHandlerL3GD20H::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { // SPI, ID will always be the one of the last sent command. *foundId = this->getPendingCommand(); @@ -133,7 +133,7 @@ ReturnValue_t GyroHandler::scanForReply(const uint8_t *start, size_t len, return DeviceHandlerIF::INVALID_DATA; } -ReturnValue_t GyroHandler::interpretDeviceReply(DeviceCommandId_t id, +ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; switch(id) { @@ -189,11 +189,11 @@ ReturnValue_t GyroHandler::interpretDeviceReply(DeviceCommandId_t id, } -uint32_t GyroHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { +uint32_t GyroHandlerL3GD20H::getTransitionDelayMs(Mode_t from, Mode_t to) { return 20000; } -ReturnValue_t GyroHandler::initializeLocalDataPool( +ReturnValue_t GyroHandlerL3GD20H::initializeLocalDataPool( localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X, new PoolEntry({0.0})); @@ -206,12 +206,12 @@ ReturnValue_t GyroHandler::initializeLocalDataPool( return HasReturnvaluesIF::RETURN_OK; } -void GyroHandler::fillCommandAndReplyMap() { +void GyroHandlerL3GD20H::fillCommandAndReplyMap() { insertInCommandAndReplyMap(L3GD20H::READ_REGS, 1, &dataset); insertInCommandAndReplyMap(L3GD20H::CONFIGURE_CTRL_REGS, 1); insertInCommandAndReplyMap(L3GD20H::READ_CTRL_REGS, 1); } -void GyroHandler::modeChanged() { +void GyroHandlerL3GD20H::modeChanged() { internalState = InternalState::STATE_NONE; } diff --git a/mission/devices/GyroL3GD20Handler.h b/mission/devices/GyroL3GD20Handler.h index 20396315..1d5f5073 100644 --- a/mission/devices/GyroL3GD20Handler.h +++ b/mission/devices/GyroL3GD20Handler.h @@ -11,11 +11,11 @@ * Advanced documentation: * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/L3GD20H_Gyro */ -class GyroHandler: public DeviceHandlerBase { +class GyroHandlerL3GD20H: public DeviceHandlerBase { public: - GyroHandler(object_id_t objectId, object_id_t deviceCommunication, + GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication, CookieIF* comCookie); - virtual ~GyroHandler(); + virtual ~GyroHandlerL3GD20H(); protected: diff --git a/mission/devices/devicedefinitions/GyroL3GD20Definitions.h b/mission/devices/devicedefinitions/GyroL3GD20Definitions.h index d2fa9903..ee1cef30 100644 --- a/mission/devices/devicedefinitions/GyroL3GD20Definitions.h +++ b/mission/devices/devicedefinitions/GyroL3GD20Definitions.h @@ -22,7 +22,7 @@ static constexpr uint8_t CTRL_REG_3 = 0b0010'0010; static constexpr uint8_t CTRL_REG_4 = 0b0010'0011; static constexpr uint8_t CTRL_REG_5 = 0b0010'0100; -// Register 1 +/* Register 1 */ static constexpr uint8_t SET_DR_1 = 1 << 7; static constexpr uint8_t SET_DR_0 = 1 << 6; static constexpr uint8_t SET_BW_1 = 1 << 5; @@ -35,7 +35,7 @@ static constexpr uint8_t SET_Y_ENABLE = 1; static constexpr uint8_t CTRL_REG_1_VAL = SET_POWER_NORMAL_MODE | SET_Z_ENABLE | SET_Y_ENABLE | SET_X_ENABLE; -// Register 2 +/* Register 2 */ static constexpr uint8_t EXTERNAL_EDGE_ENB = 1 << 7; static constexpr uint8_t LEVEL_SENSITIVE_TRIGGER = 1 << 6; static constexpr uint8_t SET_HPM_1 = 1 << 5; @@ -47,10 +47,10 @@ static constexpr uint8_t SET_HPCF_0 = 1; static constexpr uint8_t CTRL_REG_2_VAL = 0b0000'0000; -// Register 3 +/* Register 3 */ static constexpr uint8_t CTRL_REG_3_VAL = 0b0000'0000; -// Register 4 +/* Register 4 */ static constexpr uint8_t SET_BNU = 1 << 7; static constexpr uint8_t SET_BLE = 1 << 6; static constexpr uint8_t SET_FS_1 = 1 << 5; @@ -60,15 +60,16 @@ static constexpr uint8_t SET_SELF_TEST_ENB_1 = 1 << 2; static constexpr uint8_t SET_SELF_TEST_ENB_0 = 1 << 1; static constexpr uint8_t SET_SPI_IF_SELECT = 1; -static constexpr uint8_t CTRL_REG_4_VAL = 0b0000'0000; +/* Enable big endian data format */ +static constexpr uint8_t CTRL_REG_4_VAL = SET_BLE; -// Register 5 +/* Register 5 */ static constexpr uint8_t SET_REBOOT_MEM = 1 << 7; static constexpr uint8_t SET_FIFO_ENB = 1 << 6; static constexpr uint8_t CTRL_REG_5_VAL = 0b0000'0000; -// In degrees per second (DPS) for now. +/* Possible range values in degrees per second (DPS). */ static constexpr uint16_t RANGE_DPS_00 = 245; static constexpr uint16_t RANGE_DPS_01 = 500; static constexpr uint16_t RANGE_DPS_11 = 2000; @@ -76,16 +77,16 @@ static constexpr uint16_t RANGE_DPS_11 = 2000; static constexpr uint8_t READ_START = CTRL_REG_1; static constexpr size_t READ_LEN = 14; -// Indexing +/* Indexing */ static constexpr uint8_t REFERENCE_IDX = 6; static constexpr uint8_t TEMPERATURE_IDX = 7; static constexpr uint8_t STATUS_IDX = 8; -static constexpr uint8_t OUT_X_L = 9; -static constexpr uint8_t OUT_X_H = 10; -static constexpr uint8_t OUT_Y_L = 11; -static constexpr uint8_t OUT_Y_H = 12; -static constexpr uint8_t OUT_Z_L = 13; -static constexpr uint8_t OUT_Z_H = 14; +static constexpr uint8_t OUT_X_H = 9; +static constexpr uint8_t OUT_X_L = 10; +static constexpr uint8_t OUT_Y_H = 11; +static constexpr uint8_t OUT_Y_L = 12; +static constexpr uint8_t OUT_Z_H = 13; +static constexpr uint8_t OUT_Z_L = 14; /*------------------------------------------------------------------------*/ /* Device Handler specific */ From 5c04ddea1d005f3e221a19265b8c8b0b77fc4dd9 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Sun, 7 Mar 2021 12:55:22 +0100 Subject: [PATCH 10/13] doc added --- mission/devices/GyroL3GD20Handler.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/mission/devices/GyroL3GD20Handler.h b/mission/devices/GyroL3GD20Handler.h index 1d5f5073..952ff194 100644 --- a/mission/devices/GyroL3GD20Handler.h +++ b/mission/devices/GyroL3GD20Handler.h @@ -10,6 +10,8 @@ * @details * Advanced documentation: * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/L3GD20H_Gyro + * + * Data is read big endian with the smallest possible range of 245 degrees per second. */ class GyroHandlerL3GD20H: public DeviceHandlerBase { public: From dfec824222bcb055a0aaeba2ab5cc60aaf29eccb Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Sun, 7 Mar 2021 14:06:29 +0100 Subject: [PATCH 11/13] L3DG20H device handler tested --- bsp_q7s/InitMission.cpp | 3 +- bsp_rpi/InitMission.cpp | 3 +- bsp_rpi/ObjectFactory.cpp | 35 +++++---- bsp_rpi/boardconfig/rpi_config.h | 4 +- fsfwconfig/CMakeLists.txt | 2 +- fsfwconfig/OBSWConfig.h | 22 +++--- fsfwconfig/devices/spi.h | 27 +++++++ ...Factory.cpp => pollingSequenceFactory.cpp} | 15 +++- ...enceFactory.h => pollingSequenceFactory.h} | 1 + linux/boardtest/SpiTestClass.cpp | 8 +- linux/boardtest/SpiTestClass.h | 2 +- linux/spi/SpiComIF.cpp | 6 +- linux/spi/SpiComIF.h | 2 +- linux/spi/SpiCookie.cpp | 14 +++- linux/spi/SpiCookie.h | 14 +++- linux/spi/spiDefinitions.h | 2 +- mission/devices/GyroL3GD20Handler.cpp | 77 +++++++++++++++---- mission/devices/GyroL3GD20Handler.h | 21 +++-- mission/devices/MGMHandlerLIS3MDL.cpp | 7 +- mission/devices/MGMHandlerRM3100.cpp | 47 ++++++----- mission/devices/MGMHandlerRM3100.h | 10 +-- .../devicedefinitions/GyroL3GD20Definitions.h | 48 +++++++----- .../MGMHandlerLIS3Definitions.h | 3 + .../MGMHandlerRM3100Definitions.h | 3 + 24 files changed, 260 insertions(+), 116 deletions(-) create mode 100644 fsfwconfig/devices/spi.h rename fsfwconfig/pollingsequence/{PollingSequenceFactory.cpp => pollingSequenceFactory.cpp} (91%) rename fsfwconfig/pollingsequence/{PollingSequenceFactory.h => pollingSequenceFactory.h} (99%) diff --git a/bsp_q7s/InitMission.cpp b/bsp_q7s/InitMission.cpp index 72240c5b..4fda5f85 100644 --- a/bsp_q7s/InitMission.cpp +++ b/bsp_q7s/InitMission.cpp @@ -11,8 +11,7 @@ #include #include #include -#include - +#include #include /* This is configured for linux without CR */ diff --git a/bsp_rpi/InitMission.cpp b/bsp_rpi/InitMission.cpp index ce2a5940..5788f0f1 100644 --- a/bsp_rpi/InitMission.cpp +++ b/bsp_rpi/InitMission.cpp @@ -3,8 +3,6 @@ #include #include -#include - #include #include @@ -14,6 +12,7 @@ #include #include #include +#include #include diff --git a/bsp_rpi/ObjectFactory.cpp b/bsp_rpi/ObjectFactory.cpp index 8979e701..13cadfb3 100644 --- a/bsp_rpi/ObjectFactory.cpp +++ b/bsp_rpi/ObjectFactory.cpp @@ -1,23 +1,25 @@ #include "ObjectFactory.h" #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include #include #include #include - #include +#include #include #include #include +#include #include #include @@ -26,8 +28,8 @@ #include #include #include -#include -#include +#include + void Factory::setStaticFrameworkObjectIds() { PusServiceBase::packetSource = objects::PUS_PACKET_DISTRIBUTOR; @@ -99,16 +101,23 @@ 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, + MGMLIS3MDL::MAX_BUFFER_SIZE, spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED); 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, 16, - spi::SpiMode::MODE_3, 976'000); + + spiCookie = new SpiCookie(addresses::MGM_1_RM3100, gpioIds::MGM_1_RM3100_CS, spiDev, + RM3100::MAX_BUFFER_SIZE, spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED); auto mgmRm3100Handler = new MGMHandlerRM3100(objects::MGM_1_RM3100_HANDLER, objects::SPI_COM_IF, spiCookie); mgmRm3100Handler->setStartUpImmediately(); + spiCookie = new SpiCookie(addresses::GYRO_1_L3G, gpioIds::GYRO_1_L3G_CS, spiDev, + L3GD20H::MAX_BUFFER_SIZE, spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED); + auto gyroL3gHandler = new GyroHandlerL3GD20H(objects::GYRO_1_L3G_HANDLER, objects::SPI_COM_IF, + spiCookie); + gyroL3gHandler->setStartUpImmediately(); + #endif /* RPI_TEST_ACS_BOARD == 1 */ } 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/fsfwconfig/CMakeLists.txt b/fsfwconfig/CMakeLists.txt index 897789ef..87ddea03 100644 --- a/fsfwconfig/CMakeLists.txt +++ b/fsfwconfig/CMakeLists.txt @@ -1,6 +1,6 @@ target_sources(${TARGET_NAME} PRIVATE ipc/MissionMessageTypes.cpp - pollingsequence/PollingSequenceFactory.cpp + pollingsequence/pollingSequenceFactory.cpp ) target_include_directories(${TARGET_NAME} PUBLIC diff --git a/fsfwconfig/OBSWConfig.h b/fsfwconfig/OBSWConfig.h index ab1526bd..b8fecaac 100644 --- a/fsfwconfig/OBSWConfig.h +++ b/fsfwconfig/OBSWConfig.h @@ -14,20 +14,22 @@ /* These defines should be disabled for mission code but are useful for debugging. */ -#define OBSW_VERBOSE_LEVEL 1 -#define OBSW_PRINT_MISSED_DEADLINES 1 -#define OBSW_ADD_TEST_CODE 1 -#define TEST_LIBGPIOD 0 +#define OBSW_VERBOSE_LEVEL 1 +#define OBSW_PRINT_MISSED_DEADLINES 1 +#define OBSW_ADD_TEST_CODE 1 +#define TEST_LIBGPIOD 0 -#define TE0720 0 +#define TE0720 0 -#define P60DOCK_DEBUG 0 -#define PDU1_DEBUG 0 -#define PDU2_DEBUG 0 -#define ACU_DEBUG 1 +#define P60DOCK_DEBUG 0 +#define PDU1_DEBUG 0 +#define PDU2_DEBUG 0 +#define ACU_DEBUG 1 +/* Can be used to switch device to NORMAL mode immediately */ +#define OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP 1 /* Can be used for low-level debugging of the SPI bus */ -#define FSFW_LINUX_SPI_WIRETAPPING 0 +#define FSFW_LINUX_SPI_WIRETAPPING 0 #ifdef __cplusplus diff --git a/fsfwconfig/devices/spi.h b/fsfwconfig/devices/spi.h new file mode 100644 index 00000000..7085aa4d --- /dev/null +++ b/fsfwconfig/devices/spi.h @@ -0,0 +1,27 @@ +#ifndef FSFWCONFIG_DEVICES_SPI_H_ +#define FSFWCONFIG_DEVICES_SPI_H_ + +#include +#include + +/** + * SPI configuration will be contained here to let the device handlers remain independent + * of SPI specific properties. + */ +namespace spi { + +/* Default values, changing them is not supported for now */ +static constexpr uint32_t DEFAULT_LIS3_SPEED = 3'900'000; +static constexpr spi::SpiModes DEFAULT_LIS3_MODE = spi::SpiModes::MODE_3; + +static constexpr uint32_t DEFAULT_RM3100_SPEED = 976'000; +static constexpr spi::SpiModes DEFAULT_RM3100_MODE = spi::SpiModes::MODE_3; + +static constexpr uint32_t DEFAULT_L3G_SPEED = 3'900'000; +static constexpr spi::SpiModes DEFAULT_L3G_MODE = spi::SpiModes::MODE_3; + +} + + + +#endif /* FSFWCONFIG_DEVICES_SPI_H_ */ diff --git a/fsfwconfig/pollingsequence/PollingSequenceFactory.cpp b/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp similarity index 91% rename from fsfwconfig/pollingsequence/PollingSequenceFactory.cpp rename to fsfwconfig/pollingsequence/pollingSequenceFactory.cpp index 6fd92ec6..04b1367b 100644 --- a/fsfwconfig/pollingsequence/PollingSequenceFactory.cpp +++ b/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp @@ -1,9 +1,11 @@ +#include "pollingSequenceFactory.h" + #include #include #include #include #include -#include + ReturnValue_t pst::pollingSequenceInitDefault(FixedTimeslotTaskIF *thisSequence) { @@ -120,6 +122,17 @@ ReturnValue_t pst::pollingSequenceAcsTest(FixedTimeslotTaskIF *thisSequence) { DeviceHandlerIF::SEND_READ); thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.2, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.4, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.6, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.8, + DeviceHandlerIF::GET_READ); if (thisSequence->checkSequence() != HasReturnvaluesIF::RETURN_OK) { sif::error << "Initialization of ACS Board PST failed" << std::endl; return HasReturnvaluesIF::RETURN_FAILED; diff --git a/fsfwconfig/pollingsequence/PollingSequenceFactory.h b/fsfwconfig/pollingsequence/pollingSequenceFactory.h similarity index 99% rename from fsfwconfig/pollingsequence/PollingSequenceFactory.h rename to fsfwconfig/pollingsequence/pollingSequenceFactory.h index 4003058b..09dd7242 100644 --- a/fsfwconfig/pollingsequence/PollingSequenceFactory.h +++ b/fsfwconfig/pollingsequence/pollingSequenceFactory.h @@ -1,5 +1,6 @@ #ifndef POLLINGSEQUENCEFACTORY_H_ #define POLLINGSEQUENCEFACTORY_H_ + #include class FixedTimeslotTaskIF; diff --git a/linux/boardtest/SpiTestClass.cpp b/linux/boardtest/SpiTestClass.cpp index f3077ad4..3090c115 100644 --- a/linux/boardtest/SpiTestClass.cpp +++ b/linux/boardtest/SpiTestClass.cpp @@ -71,7 +71,7 @@ void SpiTestClass::performRm3100Test(uint8_t mgmId) { } uint32_t rm3100speed = 976'000; uint8_t rm3100revidReg = 0x36; - spi::SpiMode rm3100mode = spi::SpiMode::MODE_3; + spi::SpiModes rm3100mode = spi::SpiModes::MODE_3; #ifdef RASPBERRY_PI std::string deviceName = "/dev/spidev0.0"; @@ -171,7 +171,7 @@ void SpiTestClass::performLis3MdlTest(uint8_t lis3Id) { currentGpioId = gpioIds::MGM_2_LIS3_CS; } uint32_t spiSpeed = 3'900'000; - spi::SpiMode spiMode = spi::SpiMode::MODE_3; + spi::SpiModes spiMode = spi::SpiModes::MODE_3; #ifdef RASPBERRY_PI std::string deviceName = "/dev/spidev0.0"; #else @@ -217,7 +217,7 @@ void SpiTestClass::performL3gTest(uint8_t l3gId) { currentGpioId = gpioIds::GYRO_2_L3G_CS; } uint32_t spiSpeed = 3'900'000; - spi::SpiMode spiMode = spi::SpiMode::MODE_3; + spi::SpiModes spiMode = spi::SpiModes::MODE_3; #ifdef RASPBERRY_PI std::string deviceName = "/dev/spidev0.0"; #else @@ -334,7 +334,7 @@ void SpiTestClass::acsInit() { } } -void SpiTestClass::setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed) { +void SpiTestClass::setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed) { int mode_test = SPI_MODE_3; int retval = ioctl(spiFd, SPI_IOC_WR_MODE, &mode_test);//reinterpret_cast(&mode)); if(retval != 0) { diff --git a/linux/boardtest/SpiTestClass.h b/linux/boardtest/SpiTestClass.h index 5998022b..c567bc45 100644 --- a/linux/boardtest/SpiTestClass.h +++ b/linux/boardtest/SpiTestClass.h @@ -50,7 +50,7 @@ private: static constexpr uint8_t RM3100_READ_MASK = STM_READ_MASK; static constexpr uint8_t STM_AUTO_INCR_MASK = 0b0100'0000; - void setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed); + void setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed); void writeStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value, bool autoIncrement); diff --git a/linux/spi/SpiComIF.cpp b/linux/spi/SpiComIF.cpp index f678ce91..cc3b3cce 100644 --- a/linux/spi/SpiComIF.cpp +++ b/linux/spi/SpiComIF.cpp @@ -79,7 +79,7 @@ ReturnValue_t SpiComIF::initializeInterface(CookieIF *cookie) { } size_t spiSpeed = 0; - spi::SpiMode spiMode = spi::SpiMode::MODE_0; + spi::SpiModes spiMode = spi::SpiModes::MODE_0; SpiCookie::UncommonParameters params; spiCookie->getSpiParameters(spiMode, spiSpeed, ¶ms); @@ -163,7 +163,7 @@ ReturnValue_t SpiComIF::sendMessage(CookieIF *cookie, const uint8_t *sendData, s if(fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) { return OPENING_FILE_FAILED; } - spi::SpiMode spiMode = spi::SpiMode::MODE_0; + spi::SpiModes spiMode = spi::SpiModes::MODE_0; uint32_t spiSpeed = 0; spiCookie->getSpiParameters(spiMode, spiSpeed, nullptr); setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed); @@ -308,7 +308,7 @@ ReturnValue_t SpiComIF::getReadBuffer(address_t spiAddress, uint8_t** buffer) { return HasReturnvaluesIF::RETURN_OK; } -void SpiComIF::setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed) { +void SpiComIF::setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed) { int retval = ioctl(spiFd, SPI_IOC_WR_MODE, reinterpret_cast(&mode)); if(retval != 0) { utility::handleIoctlError("SpiTestClass::performRm3100Test: Setting SPI mode failed!"); diff --git a/linux/spi/SpiComIF.h b/linux/spi/SpiComIF.h index cbcaa4e0..0e27a595 100644 --- a/linux/spi/SpiComIF.h +++ b/linux/spi/SpiComIF.h @@ -57,7 +57,7 @@ private: ReturnValue_t getReadBuffer(address_t spiAddress, uint8_t** buffer); - void setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed); + void setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed); }; #endif /* LINUX_SPI_SPICOMIF_H_ */ diff --git a/linux/spi/SpiCookie.cpp b/linux/spi/SpiCookie.cpp index f0b81f67..91117682 100644 --- a/linux/spi/SpiCookie.cpp +++ b/linux/spi/SpiCookie.cpp @@ -1,17 +1,17 @@ #include "SpiCookie.h" SpiCookie::SpiCookie(address_t spiAddress, gpioId_t chipSelect, std::string spiDev, - const size_t maxSize, spi::SpiMode spiMode, uint32_t spiSpeed): spiAddress(spiAddress), + const size_t maxSize, spi::SpiModes spiMode, uint32_t spiSpeed): spiAddress(spiAddress), chipSelectPin(chipSelect), spiDevice(spiDev), maxSize(maxSize), spiMode(spiMode), spiSpeed(spiSpeed) { } SpiCookie::SpiCookie(address_t spiAddress, std::string spiDev, const size_t maxSize, - spi::SpiMode spiMode, uint32_t spiSpeed): + spi::SpiModes spiMode, uint32_t spiSpeed): SpiCookie(spiAddress, gpio::NO_GPIO, spiDev, maxSize, spiMode, spiSpeed) { } -void SpiCookie::getSpiParameters(spi::SpiMode& spiMode, uint32_t& spiSpeed, +void SpiCookie::getSpiParameters(spi::SpiModes& spiMode, uint32_t& spiSpeed, UncommonParameters* parameters) const { spiMode = this->spiMode; spiSpeed = this->spiSpeed; @@ -97,3 +97,11 @@ void SpiCookie::assignTransferSize(size_t transferSize) { size_t SpiCookie::getCurrentTransferSize() const { return spiTransferStruct.len; } + +void SpiCookie::setSpiSpeed(uint32_t newSpeed) { + this->spiSpeed = newSpeed; +} + +void SpiCookie::setSpiMode(spi::SpiModes newMode) { + this->spiMode = newMode; +} diff --git a/linux/spi/SpiCookie.h b/linux/spi/SpiCookie.h index 42004993..59d0e206 100644 --- a/linux/spi/SpiCookie.h +++ b/linux/spi/SpiCookie.h @@ -19,19 +19,25 @@ public: * @param maxSize */ SpiCookie(address_t spiAddress, gpioId_t chipSelect, std::string spiDev, - const size_t maxReplySize, spi::SpiMode spiMode, uint32_t spiSpeed); + const size_t maxReplySize, spi::SpiModes spiMode, uint32_t spiSpeed); /** * Like constructor above, but without a dedicated GPIO CS. Can be used for hardware * slave select or if CS logic is performed with decoders. */ SpiCookie(address_t spiAddress, std::string spiDev, const size_t maxReplySize, - spi::SpiMode spiMode, uint32_t spiSpeed); + spi::SpiModes spiMode, uint32_t spiSpeed); address_t getSpiAddress() const; std::string getSpiDevice() const; gpioId_t getChipSelectPin() const; size_t getMaxBufferSize() const; + + /** Enables changing SPI speed at run-time */ + void setSpiSpeed(uint32_t newSpeed); + /** Enables changing the SPI mode at run-time */ + void setSpiMode(spi::SpiModes newMode); + /** * True if SPI transfers should be performed in full duplex mode * @return @@ -81,7 +87,7 @@ public: void setCsHigh(bool enable); void setBitsPerWord(uint8_t bitsPerWord); - void getSpiParameters(spi::SpiMode& spiMode, uint32_t& spiSpeed, + void getSpiParameters(spi::SpiModes& spiMode, uint32_t& spiSpeed, UncommonParameters* parameters = nullptr) const; /** @@ -100,7 +106,7 @@ private: std::string spiDevice; const size_t maxSize; - spi::SpiMode spiMode; + spi::SpiModes spiMode; uint32_t spiSpeed; bool halfDuplex = false; diff --git a/linux/spi/spiDefinitions.h b/linux/spi/spiDefinitions.h index b4310697..e8c48147 100644 --- a/linux/spi/spiDefinitions.h +++ b/linux/spi/spiDefinitions.h @@ -5,7 +5,7 @@ namespace spi { -enum SpiMode: uint8_t { +enum SpiModes: uint8_t { MODE_0, MODE_1, MODE_2, diff --git a/mission/devices/GyroL3GD20Handler.cpp b/mission/devices/GyroL3GD20Handler.cpp index 279edf73..abf9731c 100644 --- a/mission/devices/GyroL3GD20Handler.cpp +++ b/mission/devices/GyroL3GD20Handler.cpp @@ -1,25 +1,42 @@ -#include +#include "GyroL3GD20Handler.h" +#include + #include GyroHandlerL3GD20H::GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie): DeviceHandlerBase(objectId, deviceCommunication, comCookie), dataset(this) { +#if OBSW_VERBOSE_LEVEL >= 1 + debugDivider = new PeriodicOperationDivider(5); +#endif } GyroHandlerL3GD20H::~GyroHandlerL3GD20H() {} void GyroHandlerL3GD20H::doStartUp() { - if(internalState == InternalState::STATE_NONE) { - internalState = InternalState::STATE_CONFIGURE; + if(internalState == InternalState::NONE) { + internalState = InternalState::CONFIGURE; } - if(internalState == InternalState::STATE_CONFIGURE) { + if(internalState == InternalState::CONFIGURE) { if(commandExecuted) { - internalState = InternalState::STATE_NORMAL; + internalState = InternalState::CHECK_REGS; commandExecuted = false; } } + + if(internalState == InternalState::CHECK_REGS) { + if(commandExecuted) { + internalState = InternalState::NORMAL; +#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1 + setMode(MODE_NORMAL); +#else + setMode(_MODE_TO_ON); +#endif + commandExecuted = false; + } + } } void GyroHandlerL3GD20H::doShutDown() { @@ -28,11 +45,11 @@ void GyroHandlerL3GD20H::doShutDown() { ReturnValue_t GyroHandlerL3GD20H::buildTransitionDeviceCommand(DeviceCommandId_t *id) { switch(internalState) { - case(InternalState::STATE_NONE): - case(InternalState::STATE_NORMAL): { + case(InternalState::NONE): + case(InternalState::NORMAL): { return HasReturnvaluesIF::RETURN_OK; } - case(InternalState::STATE_CONFIGURE): { + case(InternalState::CONFIGURE): { *id = L3GD20H::CONFIGURE_CTRL_REGS; uint8_t command [5]; command[0] = L3GD20H::CTRL_REG_1_VAL; @@ -41,10 +58,13 @@ ReturnValue_t GyroHandlerL3GD20H::buildTransitionDeviceCommand(DeviceCommandId_t command[3] = L3GD20H::CTRL_REG_4_VAL; command[4] = L3GD20H::CTRL_REG_5_VAL; return buildCommandFromCommand(*id, command, 5); - break; + } + case(InternalState::CHECK_REGS): { + *id = L3GD20H::READ_REGS; + return buildCommandFromCommand(*id, nullptr, 0); } default: - // might be a configuration error. + /* Might be a configuration error. */ sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown " << "internal state!" << std::endl; return HasReturnvaluesIF::RETURN_OK; @@ -122,11 +142,11 @@ ReturnValue_t GyroHandlerL3GD20H::buildCommandFromCommand( ReturnValue_t GyroHandlerL3GD20H::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { - // SPI, ID will always be the one of the last sent command. + /* For SPI, the ID will always be the one of the last sent command. */ *foundId = this->getPendingCommand(); *foundLen = this->rawPacketLen; - // Data with SPI Interface has always this answer + /* Data with SPI Interface has always this answer */ if (start[0] == 0b11111111) { return HasReturnvaluesIF::RETURN_OK; } @@ -138,6 +158,7 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; switch(id) { case(L3GD20H::CONFIGURE_CTRL_REGS): { + commandExecuted = true; break; } case(L3GD20H::READ_CTRL_REGS): { @@ -147,18 +168,23 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, commandExecuted = true; } else { - // Attempt reconfiguration. - internalState = InternalState::STATE_CONFIGURE; + /* Attempt reconfiguration. */ + internalState = InternalState::CONFIGURE; return DeviceHandlerIF::DEVICE_REPLY_INVALID; } break; } - case(L3GD20H::READ_START): { + case(L3GD20H::READ_REGS): { if(packet[1] != ctrlReg1Value and packet[2] != ctrlReg2Value and packet[3] != ctrlReg3Value and packet[4] != ctrlReg4Value and packet[5] != ctrlReg5Value) { return DeviceHandlerIF::DEVICE_REPLY_INVALID; } + else { + if(internalState == InternalState::CHECK_REGS) { + commandExecuted = true; + } + } statusReg = packet[L3GD20H::STATUS_IDX]; @@ -171,6 +197,23 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, int8_t temperaturOffset = (-1) * packet[L3GD20H::TEMPERATURE_IDX]; float temperature = 25.0 + temperaturOffset; +#if OBSW_VERBOSE_LEVEL >= 1 + if(debugDivider->checkAndIncrement()) { + /* Set terminal to utf-8 if there is an issue with micro printout. */ +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::info << "GyroHandlerL3GD20H: Angular velocities in degrees per second:" << + std::endl; + sif::info << "X: " << angVelocX << " \xC2\xB0" << std::endl; + sif::info << "Y: " << angVelocY << " \xC2\xB0" << std::endl; + sif::info << "Z: " << angVelocZ << " \xC2\xB0" << std::endl; +#else + sif::printInfo("GyroHandlerL3GD20H: Angular velocities in degrees per second:\n"); + sif::printInfo("X: %f " "\xC2\xB0" "T\n", angVelocX); + sif::printInfo("Y: %f " "\xC2\xB0" "T\n", angVelocY); + sif::printInfo("Z: %f " "\xC2\xB0" "T\n", angVelocZ); +#endif + } +#endif PoolReadGuard readSet(&dataset); if(readSet.getReadResult() == HasReturnvaluesIF::RETURN_OK) { @@ -190,7 +233,7 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, uint32_t GyroHandlerL3GD20H::getTransitionDelayMs(Mode_t from, Mode_t to) { - return 20000; + return 25000; } ReturnValue_t GyroHandlerL3GD20H::initializeLocalDataPool( @@ -213,5 +256,5 @@ void GyroHandlerL3GD20H::fillCommandAndReplyMap() { } void GyroHandlerL3GD20H::modeChanged() { - internalState = InternalState::STATE_NONE; + internalState = InternalState::NONE; } diff --git a/mission/devices/GyroL3GD20Handler.h b/mission/devices/GyroL3GD20Handler.h index 952ff194..442e28f8 100644 --- a/mission/devices/GyroL3GD20Handler.h +++ b/mission/devices/GyroL3GD20Handler.h @@ -1,8 +1,12 @@ #ifndef MISSION_DEVICES_GYROL3GD20HANDLER_H_ #define MISSION_DEVICES_GYROL3GD20HANDLER_H_ -#include #include "devicedefinitions/GyroL3GD20Definitions.h" +#include + +#include +#include + /** * @brief Device Handler for the L3GD20H gyroscope sensor @@ -43,14 +47,15 @@ protected: LocalDataPoolManager &poolManager) override; private: - L3GD20H::GyroPrimaryDataset dataset; + GyroPrimaryDataset dataset; enum class InternalState { - STATE_NONE, - STATE_CONFIGURE, - STATE_NORMAL + NONE, + CONFIGURE, + CHECK_REGS, + NORMAL }; - InternalState internalState = InternalState::STATE_NONE; + InternalState internalState = InternalState::NONE; bool commandExecuted = false; uint8_t statusReg = 0; @@ -64,6 +69,10 @@ private: uint8_t commandBuffer[L3GD20H::READ_LEN + 1]; float scaleFactor = static_cast(L3GD20H::RANGE_DPS_00) / INT16_MAX; + +#if OBSW_VERBOSE_LEVEL >= 1 + PeriodicOperationDivider* debugDivider = nullptr; +#endif }; diff --git a/mission/devices/MGMHandlerLIS3MDL.cpp b/mission/devices/MGMHandlerLIS3MDL.cpp index d2573f73..8fca515a 100644 --- a/mission/devices/MGMHandlerLIS3MDL.cpp +++ b/mission/devices/MGMHandlerLIS3MDL.cpp @@ -7,7 +7,7 @@ MGMHandlerLIS3MDL::MGMHandlerLIS3MDL(object_id_t objectId, DeviceHandlerBase(objectId, deviceCommunication, comCookie), dataset(this) { #if OBSW_VERBOSE_LEVEL >= 1 - debugDivider = new PeriodicOperationDivider(10); + debugDivider = new PeriodicOperationDivider(5); #endif /* Set to default values right away. */ registers[0] = MGMLIS3MDL::CTRL_REG1_DEFAULT; @@ -44,8 +44,11 @@ void MGMHandlerLIS3MDL::doStartUp() { /* Set up cached registers which will be used to configure the MGM. */ if(commandExecuted) { commandExecuted = false; - /* Replace _MODE_TO_ON with MODE_NORMAL to jump to normal mode quickly */ +#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1 + setMode(MODE_NORMAL); +#else setMode(_MODE_TO_ON); +#endif } break; } diff --git a/mission/devices/MGMHandlerRM3100.cpp b/mission/devices/MGMHandlerRM3100.cpp index 2f6cef0e..ae97f6ff 100644 --- a/mission/devices/MGMHandlerRM3100.cpp +++ b/mission/devices/MGMHandlerRM3100.cpp @@ -12,7 +12,7 @@ MGMHandlerRM3100::MGMHandlerRM3100(object_id_t objectId, DeviceHandlerBase(objectId, deviceCommunication, comCookie), primaryDataset(this) { #if OBSW_VERBOSE_LEVEL >= 1 - debugDivider = new PeriodicOperationDivider(10); + debugDivider = new PeriodicOperationDivider(5); #endif } @@ -20,28 +20,34 @@ MGMHandlerRM3100::~MGMHandlerRM3100() {} void MGMHandlerRM3100::doStartUp() { switch(internalState) { - case(InternalState::STATE_NONE): { - internalState = InternalState::STATE_CONFIGURE_CMM; + case(InternalState::NONE): { + internalState = InternalState::CONFIGURE_CMM; break; } - case(InternalState::STATE_CONFIGURE_CMM): { - internalState = InternalState::STATE_READ_CMM; + case(InternalState::CONFIGURE_CMM): { + internalState = InternalState::READ_CMM; break; } - case(InternalState::STATE_READ_CMM): { + case(InternalState::READ_CMM): { if(commandExecuted) { internalState = InternalState::STATE_CONFIGURE_TMRC; } break; } case(InternalState::STATE_CONFIGURE_TMRC): { - internalState = InternalState::STATE_READ_TMRC; + if(commandExecuted) { + internalState = InternalState::STATE_READ_TMRC; + } break; } case(InternalState::STATE_READ_TMRC): { if(commandExecuted) { - internalState = InternalState::STATE_NORMAL; + internalState = InternalState::NORMAL; +#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1 + setMode(MODE_NORMAL); +#else setMode(_MODE_TO_ON); +#endif } break; } @@ -58,15 +64,15 @@ void MGMHandlerRM3100::doShutDown() { ReturnValue_t MGMHandlerRM3100::buildTransitionDeviceCommand( DeviceCommandId_t *id) { switch(internalState) { - case(InternalState::STATE_NONE): - case(InternalState::STATE_NORMAL): { + case(InternalState::NONE): + case(InternalState::NORMAL): { return HasReturnvaluesIF::RETURN_OK; } - case(InternalState::STATE_CONFIGURE_CMM): { + case(InternalState::CONFIGURE_CMM): { *id = RM3100::CONFIGURE_CMM; break; } - case(InternalState::STATE_READ_CMM): { + case(InternalState::READ_CMM): { *id = RM3100::READ_CMM; break; } @@ -79,9 +85,9 @@ ReturnValue_t MGMHandlerRM3100::buildTransitionDeviceCommand( break; } default: - // might be a configuration error. - sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown " - << "internal state!" << std::endl; + /* Might be a configuration error. */ + sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown internal state!" << + std::endl; return HasReturnvaluesIF::RETURN_OK; } @@ -164,6 +170,9 @@ ReturnValue_t MGMHandlerRM3100::interpretDeviceReply( case(RM3100::CONFIGURE_CYCLE_COUNT): case(RM3100::CONFIGURE_TMRC): { /* We can only check whether write was successful with read operation. */ + if(mode == _MODE_START_UP) { + commandExecuted = true; + } break; } case(RM3100::READ_CMM): { @@ -171,12 +180,12 @@ ReturnValue_t MGMHandlerRM3100::interpretDeviceReply( /* We clear the seventh bit in any case * because this one is zero sometimes for some reason */ bitutil::bitClear(&cmmValue, 6); - if(cmmValue == cmmRegValue) { + if(cmmValue == cmmRegValue and internalState == InternalState::READ_CMM) { commandExecuted = true; } else { /* Attempt reconfiguration. */ - internalState = InternalState::STATE_CONFIGURE_CMM; + internalState = InternalState::CONFIGURE_CMM; return DeviceHandlerIF::DEVICE_REPLY_INVALID; } break; @@ -304,7 +313,7 @@ void MGMHandlerRM3100::fillCommandAndReplyMap() { } void MGMHandlerRM3100::modeChanged(void) { - internalState = InternalState::STATE_NONE; + internalState = InternalState::NONE; } ReturnValue_t MGMHandlerRM3100::initializeLocalDataPool( @@ -316,7 +325,7 @@ ReturnValue_t MGMHandlerRM3100::initializeLocalDataPool( } uint32_t MGMHandlerRM3100::getTransitionDelayMs(Mode_t from, Mode_t to) { - return 10000; + return 15000; } ReturnValue_t MGMHandlerRM3100::handleDataReadout(const uint8_t *packet) { diff --git a/mission/devices/MGMHandlerRM3100.h b/mission/devices/MGMHandlerRM3100.h index 968953bf..ce107637 100644 --- a/mission/devices/MGMHandlerRM3100.h +++ b/mission/devices/MGMHandlerRM3100.h @@ -61,18 +61,18 @@ protected: private: enum class InternalState { - STATE_NONE, - STATE_CONFIGURE_CMM, - STATE_READ_CMM, + NONE, + CONFIGURE_CMM, + READ_CMM, // The cycle count states are propably not going to be used because // the default cycle count will be used. STATE_CONFIGURE_CYCLE_COUNT, STATE_READ_CYCLE_COUNT, STATE_CONFIGURE_TMRC, STATE_READ_TMRC, - STATE_NORMAL + NORMAL }; - InternalState internalState = InternalState::STATE_NONE; + InternalState internalState = InternalState::NONE; bool commandExecuted = false; RM3100::Rm3100PrimaryDataset primaryDataset; diff --git a/mission/devices/devicedefinitions/GyroL3GD20Definitions.h b/mission/devices/devicedefinitions/GyroL3GD20Definitions.h index ee1cef30..30669b1d 100644 --- a/mission/devices/devicedefinitions/GyroL3GD20Definitions.h +++ b/mission/devices/devicedefinitions/GyroL3GD20Definitions.h @@ -1,11 +1,15 @@ #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ #define MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ +#include #include #include namespace L3GD20H { +/* Actual size is 15 but we round up a bit */ +static constexpr size_t MAX_BUFFER_SIZE = 16; + static constexpr uint8_t READ_MASK = 0b1000'0000; static constexpr uint8_t AUTO_INCREMENT_MASK = 0b0100'0000; @@ -104,27 +108,33 @@ enum GyroPoolIds: lp_id_t { TEMPERATURE }; -class GyroPrimaryDataset: public StaticLocalDataSet<3 * sizeof(float)> { -public: - GyroPrimaryDataset(HasLocalDataPoolIF* hkOwner): - StaticLocalDataSet(hkOwner, GYRO_DATASET_ID) {} - - GyroPrimaryDataset(object_id_t mgmId): - StaticLocalDataSet(sid_t(mgmId, GYRO_DATASET_ID)) {} - - // Angular velocities in degrees per second (DPS) - lp_var_t angVelocX = lp_var_t(sid.objectId, - ANG_VELOC_X, this); - lp_var_t angVelocY = lp_var_t(sid.objectId, - ANG_VELOC_Y, this); - lp_var_t angVelocZ = lp_var_t(sid.objectId, - ANG_VELOC_Z, this); - lp_var_t temperature = lp_var_t(sid.objectId, - TEMPERATURE, this); -}; - } +class GyroPrimaryDataset: public StaticLocalDataSet<3 * sizeof(float)> { +public: + + /** Constructor for data users like controllers */ + GyroPrimaryDataset(object_id_t mgmId): + StaticLocalDataSet(sid_t(mgmId, L3GD20H::GYRO_DATASET_ID)) { + setAllVariablesReadOnly(); + } + + /* Angular velocities in degrees per second (DPS) */ + lp_var_t angVelocX = lp_var_t(sid.objectId, + L3GD20H::ANG_VELOC_X, this); + lp_var_t angVelocY = lp_var_t(sid.objectId, + L3GD20H::ANG_VELOC_Y, this); + lp_var_t angVelocZ = lp_var_t(sid.objectId, + L3GD20H::ANG_VELOC_Z, this); + lp_var_t temperature = lp_var_t(sid.objectId, + L3GD20H::TEMPERATURE, this); +private: + + friend class GyroHandlerL3GD20H; + /** Constructor for the data creator */ + GyroPrimaryDataset(HasLocalDataPoolIF* hkOwner): + StaticLocalDataSet(hkOwner, L3GD20H::GYRO_DATASET_ID) {} +}; #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ */ diff --git a/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h b/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h index 0d51cda1..b8b2463d 100644 --- a/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h +++ b/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h @@ -15,6 +15,9 @@ enum opMode { LOW, MEDIUM, HIGH, ULTRA }; +/* Actually 15, we just round up a bit */ +static constexpr size_t MAX_BUFFER_SIZE = 16; + static constexpr uint8_t GAUSS_TO_MICROTESLA_FACTOR = 100; static const DeviceCommandId_t SETUP_MGM = 0x00; diff --git a/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h b/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h index 921c9e5f..2856c170 100644 --- a/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h +++ b/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h @@ -9,6 +9,9 @@ namespace RM3100 { +/* Actually 10, we round up a little bit */ +static constexpr size_t MAX_BUFFER_SIZE = 12; + static constexpr uint8_t READ_MASK = 0b1000'0000; /*----------------------------------------------------------------------------*/ From cdcb049736e564b16de56881ad20df68053f9d38 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Sun, 7 Mar 2021 14:07:17 +0100 Subject: [PATCH 12/13] disabled acs board test --- bsp_rpi/boardconfig/rpi_config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bsp_rpi/boardconfig/rpi_config.h b/bsp_rpi/boardconfig/rpi_config.h index d1acacb6..a9f4fd9f 100644 --- a/bsp_rpi/boardconfig/rpi_config.h +++ b/bsp_rpi/boardconfig/rpi_config.h @@ -8,7 +8,7 @@ /* Only one of those 2 should be enabled! */ #define RPI_ADD_SPI_TEST 0 -#define RPI_TEST_ACS_BOARD 1 +#define RPI_TEST_ACS_BOARD 0 /* Adapt these values accordingly */ namespace gpio { From cbe55a605df458635af1addfed9179354d75c564 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Sun, 7 Mar 2021 14:11:13 +0100 Subject: [PATCH 13/13] lowered transition times and changed pst to 1sec --- bsp_rpi/InitMission.cpp | 2 +- mission/devices/GyroL3GD20Handler.cpp | 2 +- mission/devices/MGMHandlerLIS3MDL.cpp | 2 +- mission/devices/MGMHandlerRM3100.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/bsp_rpi/InitMission.cpp b/bsp_rpi/InitMission.cpp index 5788f0f1..18e4f01a 100644 --- a/bsp_rpi/InitMission.cpp +++ b/bsp_rpi/InitMission.cpp @@ -127,7 +127,7 @@ void initmission::initTasks() { #if RPI_TEST_ACS_BOARD == 1 FixedTimeslotTaskIF* acsTask = factory->createFixedTimeslotTask( - "ACS_PST", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 2.0, missedDeadlineFunc); + "ACS_PST", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 1.0, missedDeadlineFunc); result = pst::pollingSequenceAcsTest(acsTask); if(result != HasReturnvaluesIF::RETURN_OK) { sif::warning << "initmission::initTasks: ACS PST initialization failed!" << std::endl; diff --git a/mission/devices/GyroL3GD20Handler.cpp b/mission/devices/GyroL3GD20Handler.cpp index abf9731c..850be83c 100644 --- a/mission/devices/GyroL3GD20Handler.cpp +++ b/mission/devices/GyroL3GD20Handler.cpp @@ -233,7 +233,7 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, uint32_t GyroHandlerL3GD20H::getTransitionDelayMs(Mode_t from, Mode_t to) { - return 25000; + return 10000; } ReturnValue_t GyroHandlerL3GD20H::initializeLocalDataPool( diff --git a/mission/devices/MGMHandlerLIS3MDL.cpp b/mission/devices/MGMHandlerLIS3MDL.cpp index 8fca515a..fc9b8b4f 100644 --- a/mission/devices/MGMHandlerLIS3MDL.cpp +++ b/mission/devices/MGMHandlerLIS3MDL.cpp @@ -462,7 +462,7 @@ void MGMHandlerLIS3MDL::doTransition(Mode_t modeFrom, Submode_t subModeFrom) { } uint32_t MGMHandlerLIS3MDL::getTransitionDelayMs(Mode_t from, Mode_t to) { - return 30000; + return 10000; } void MGMHandlerLIS3MDL::modeChanged(void) { diff --git a/mission/devices/MGMHandlerRM3100.cpp b/mission/devices/MGMHandlerRM3100.cpp index ae97f6ff..4fd654d3 100644 --- a/mission/devices/MGMHandlerRM3100.cpp +++ b/mission/devices/MGMHandlerRM3100.cpp @@ -325,7 +325,7 @@ ReturnValue_t MGMHandlerRM3100::initializeLocalDataPool( } uint32_t MGMHandlerRM3100::getTransitionDelayMs(Mode_t from, Mode_t to) { - return 15000; + return 10000; } ReturnValue_t MGMHandlerRM3100::handleDataReadout(const uint8_t *packet) {