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(