added l3g handler

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_ADD_SPI_TEST                1
-#define RPI_TEST_ACS_BOARD              1
+#define RPI_TEST_ACS_BOARD              0
 
 /* Adapt these values accordingly */
 namespace gpio {

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<int>(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<float>(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<uint8_t*>(&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) {
+void SpiTestClass::writeMultipleStmRegisters(int fd, gpioId_t chipSelect, uint8_t reg,
-    int mode_test = SPI_MODE_3;
+        uint8_t *values, size_t len) {
-    int retval = ioctl(spiFd, SPI_IOC_WR_MODE, &mode_test);//reinterpret_cast<uint8_t*>(&mode));
+    if(values == nullptr) {
-    if(retval != 0) {
+        return;
-        utility::handleIoctlError("SpiTestClass::performRm3100Test: Setting SPI mode failed!");
     }
 
-    retval = ioctl(spiFd, SPI_IOC_WR_MAX_SPEED_HZ, &speed);
+    reg |= STM_AUTO_INCR_MASK;
-    if(retval != 0) {
+    /* Clear read mask */
-        utility::handleIoctlError("SpiTestClass::performRm3100Test: Setting SPI speed failed!");
+    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;
     }
 

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,
+    void writeMultipleStmRegisters(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t* values,
-            uint8_t* reply, size_t len);
+            size_t len);
-    uint8_t readRegister(int fd, gpioId_t chipSelect, uint8_t reg);
+    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);
 
 };
 

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(