diff --git a/sbs.conf b/sbs.conf index 20f00f2773861e1e03f445adb1ddb0f92397041a..a1e257e21ca3e424eae9c05fdfc5fe9d890917e6 100644 --- a/sbs.conf +++ b/sbs.conf @@ -499,10 +499,10 @@ Include: %shared Defines: Main: test-rls -[test-lsm9ds1-mag] +[test-lsm9ds1-fifo] Type: test BoardId: stm32f407vg_stm32f4discovery -BinName: test-lsm9ds1-mag +BinName: test-lsm9ds1-fifo Include: %shared %spi Defines: -DDEBUG -Main: drivers/test-lsm9ds1-mag +Main: drivers/test-lsm9ds1-fifo diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h index f24c3ed46ab896b6b503eebf8e227cb2a0ad1dfe..652f982a920b1677a49810ababaed2502efda7ea 100644 --- a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h +++ b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h @@ -189,11 +189,11 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen { spi.write(regMapXLG::FIFO_CTRL, (FIFO_CTRL_VAL|fifo_watermark)); //FIFO continous mode + fifo watermark threshold setup spi.write(regMapXLG::INT1_CTRL, INT1_CTRL_VAL); //interrupt on FIFO treshold - spi.write(regMapXLG::CTRL_REG9, (CTRL_REG9_VAL|0x02)); //DRDY_mask_bit ON, I2C OFF, FIFO ON + spi.write(regMapXLG::CTRL_REG9, (CTRL_REG9_VAL|0x02)); //DRDY_mask_bit OFF, I2C OFF, FIFO ON } else { - spi.write(regMapXLG::CTRL_REG9, CTRL_REG9_VAL); //DRDY_mask_bit ON, I2C OFF, FIFO OFF + spi.write(regMapXLG::CTRL_REG9, CTRL_REG9_VAL); //DRDY_mask_bit OFF, I2C OFF, FIFO OFF } @@ -223,24 +223,8 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen if(spi.read(regMapXLG::CTRL_REG6_XL) != CTRL_REG6_XL_VAL) {return false;} if(spi.read(regMapXLG::CTRL_REG1_G) != CTRL_REG1_G_VAL) {return false;} - - //@ startup, some samples have to be discarded (datasheet) - if(odr != ODR::PWR_DW) - { - uint16_t toWait_ms = samplesToDiscard * 1000 / odrHz; - //TRACE("toWait_ms: %d", toWait_ms); - miosix::Thread::sleep(toWait_ms); //if FIFO is disabled, just wait - if(fifo_enabled) - { - //if FIFO is enabled, read first <samplesToDiscard> samples and discard them - SPITransaction spi(spislave); - for(int i=0; i < samplesToDiscard; i++) - { - spi.read(regMapXLG::OUT_X_L_XL, 6); - spi.read(regMapXLG::OUT_X_L_G, 6); - } - } - } + discardSamples(); + return true; } @@ -253,28 +237,23 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen { if(!fifo_enabled){ //if FIFO disabled - uint8_t axelData[6], gyroData[6], tempData[2]; + uint8_t data[12], tempData[2]; // Read output axel+gyro data X,Y,Z { SPITransaction spi(spislave); - spi.read(regMapXLG::OUT_X_L_XL, axelData, 6); - spi.read(regMapXLG::OUT_X_L_G, gyroData, 6); + spi.read(regMapXLG::OUT_X_L_G, data, 12); spi.read(regMapXLG::OUT_TEMP_L, tempData, 2); } - int16_t x_xl = axelData[0] | axelData[1] << 8; - int16_t y_xl = axelData[2] | axelData[3] << 8; - int16_t z_xl = axelData[4] | axelData[5] << 8; + int16_t x_gy = data[0] | data[1] << 8; + int16_t y_gy = data[2] | data[3] << 8; + int16_t z_gy = data[4] | data[5] << 8; - int16_t x_gy = gyroData[0] | gyroData[1] << 8; - int16_t y_gy = gyroData[2] | gyroData[3] << 8; - int16_t z_gy = gyroData[4] | gyroData[5] << 8; + int16_t x_xl = data[6] | data[7] << 8; + int16_t y_xl = data[8] | data[9] << 8; + int16_t z_xl = data[10] | data[11] << 8; int16_t temp = tempData[0] | tempData[1] << 8; - - // TRACE("LSM9DS1 axel: %02X,%02X,%02X\n", x_xl, y_xl, z_xl); - // TRACE("LSM9DS1 gyro: %02X,%02X,%02X\n", x_gy, y_gy, z_gy); - // TRACE("LSM9DS1 temp: %02X\n, temp"); mLastAccel = Vec3(x_xl * axelSensitivity / 1000, @@ -289,7 +268,7 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen mLastTemp = tempZero + (temp / tempSensistivity); //25°C + TEMP*S devo castare a float "temp"? } else{ //if FIFO enabled: do not store temperature, it can be read using "temperatureUpdate()" function at low sampling frequency - uint8_t buf[384]; + uint8_t buf[384]; //2 bytes per data * 3 axes * 2 (axel+gyro) * 32(FIFO DEPTH) = 384 samples { SPITransaction spi(spislave); //read FIFO status and dump all the samples inside the FIFO @@ -314,13 +293,13 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen uint16_t z_xl = buf[i*12 + 10] | buf[i*12 + 11] << 8; gyro_fifo[i] = - Vec3(x_gy / gyroSensitivity, - y_gy / gyroSensitivity, - z_gy / gyroSensitivity); + Vec3(x_gy * gyroSensitivity / 1000, + y_gy * gyroSensitivity / 1000, + z_gy * gyroSensitivity / 1000); axel_fifo[i] = - Vec3(x_xl / axelSensitivity, - y_xl / axelSensitivity, - z_xl / axelSensitivity); + Vec3(x_xl * axelSensitivity / 1000, + y_xl * axelSensitivity / 1000, + z_xl * axelSensitivity / 1000); } } return true; @@ -339,10 +318,20 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen return true; } + void clearFIFO() //FIFO clearing routine (see datasheet) + { + SPITransaction spi(spislave); + spi.write(FIFO_CTRL, 0); //Bypass Mode + miosix::Thread::sleep(20); //Wait + spi.write(FIFO_CTRL, FIFO_CTRL_VAL | fifo_watermark); //re-enable FIFO + discardSamples(); + + } + const array<Vec3, 32>& getGyroFIFO() const { return gyro_fifo; } const array<Vec3, 32>& getAxelFIFO() const { return axel_fifo; } uint8_t getFIFOSamples() const { return fifo_samples;} //fifo_samples is the same for both axel & gyro FIFOs - + private: bool fifo_enabled; @@ -363,6 +352,29 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen float tempSensistivity = 16.0f; static const uint8_t samplesToDiscard = 8; //max possible val + void discardSamples() + { + + //@ startup, some samples have to be discarded (datasheet) + if(odr != ODR::PWR_DW) + { + uint16_t toWait_ms = samplesToDiscard * 1000 / odrHz; + //TRACE("toWait_ms: %d", toWait_ms); + miosix::Thread::sleep(toWait_ms); //if FIFO is disabled, just wait + if(fifo_enabled) + { + //if FIFO is enabled, read first <samplesToDiscard> samples and discard them + SPITransaction spi(spislave); + for(int i=0; i < samplesToDiscard; i++) + { + spi.read(regMapXLG::OUT_X_L_XL, 6); + spi.read(regMapXLG::OUT_X_L_G, 6); + } + } + } + + } + enum regMapXLG { ACT_THS = 0x04, diff --git a/src/tests/drivers/test-lsm9ds1-fifo.cpp b/src/tests/drivers/test-lsm9ds1-fifo.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a8e358de60e1d3254c7578fe5bf2216097b676cb --- /dev/null +++ b/src/tests/drivers/test-lsm9ds1-fifo.cpp @@ -0,0 +1,199 @@ +/** + * test LSM9DS1 axel + gyro + * Copyright (c) 2020 Skyward Experimental Rocketry + * Authors: Andrea Milluzzo + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "drivers/HardwareTimer.h" +#include "drivers/spi/SPIDriver.h" +#include "sensors/LSM9DS1/LSM9DS1_AxelGyro.h" +#include <array> + +using namespace miosix; +using namespace std; + +typedef Gpio<GPIOA_BASE, 5> GpioSck; //questi sono i pin SPI per f407_discovery +typedef Gpio<GPIOA_BASE, 6> GpioMiso; +typedef Gpio<GPIOA_BASE, 7> GpioMosi; + +typedef Gpio<GPIOB_BASE, 1> GpioINT1; + +static const bool FIFO_ENABLED = true; +static const uint8_t FIFO_WATERMARK = 12; + +//SPI +SPIBus bus(SPI1); +GpioPin cs_XLG(GPIOE_BASE, 7); + +//LED just for init +GpioPin LED1(GPIOD_BASE, 15); + +//SPI read flag +bool flagSPIReadRequest = false; + +//High Resolution hardware timer using TIM5 +HardwareTimer<uint32_t> hrclock{ + TIM5, TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB1)}; + +//Last interrupt tick & delta +volatile uint32_t last_tick; +volatile uint32_t delta; + +//Interrupt handlers +void __attribute__((naked)) EXTI1_IRQHandler() +{ + saveContext(); + asm volatile("bl _Z20EXTI1_IRQHandlerImplv"); + restoreContext(); +} + +void __attribute__((used)) EXTI1_IRQHandlerImpl() +{ + //Computing delta beetween interrupts + uint32_t tick = hrclock.tick(); + delta = tick - last_tick; + last_tick = tick; + + //IRQ routine func here + flagSPIReadRequest = true; + + //Clear pending interrupt register + EXTI->PR |= EXTI_PR_PR1; + + //Built-in LED on + LED1.high(); + +} + +void gpioConfig(); +void timer5Config(); +void EXTI1Config(); + +int main(){ + + gpioConfig(); + timer5Config(); + EXTI1Config(); + + LSM9DS1_XLG lsm9ds1( + bus, + cs_XLG, + LSM9DS1_XLG::AxelFSR::FS_8, + LSM9DS1_XLG::GyroFSR::FS_245, + LSM9DS1_XLG::ODR::ODR_15, + FIFO_ENABLED, + FIFO_WATERMARK); + + while(!lsm9ds1.init()); + + long long last_tick = getTick(); + + while(1) + { + /*if(flagSPIReadRequest) + { + flagSPIReadRequest = false; + Thread::sleep(500); + LED1.low(); + Thread::sleep(500); + lsm9ds1.clearFIFO(); + printf("interrupt occured...\n"); + }*/ + + if(getTick()- last_tick> 10000) //every 10 sec Clear FIFO + { + printf("Resetting ... \n"); + last_tick = getTick(); + lsm9ds1.clearFIFO(); + } + } + + return 0; +} + +void gpioConfig() +{ + { + FastInterruptDisableLock dLock; + + //Enable SPI1 Peripheral + RCC->APB2ENR |= RCC_APB2ENR_SPI1EN; + + //Select GPIO mode (ALTERNATE 5 for SPI1 on PA5, PA6, PA7) + GpioSck::mode(Mode::ALTERNATE); + GpioMiso::mode(Mode::ALTERNATE); + GpioMosi::mode(Mode::ALTERNATE); + + + GpioSck::alternateFunction(5); + GpioMiso::alternateFunction(5); + GpioMosi::alternateFunction(5); + + //Select Speed for clk (needs precision) + GpioSck::speed(Speed::_25MHz); + + //Select GPIO mode for Chip Select + cs_XLG.mode(Mode::OUTPUT); + + //Select GPIO mode for INTERRUPT (PULL-DOWN because there's no pull-down in HW) + GpioINT1::mode(Mode::INPUT_PULL_DOWN); + + //Select LED built in GPIO mode + LED1.mode(Mode::OUTPUT); + } + + cs_XLG.high(); + +} + +void timer5Config() +{ + { + FastInterruptDisableLock dl; + //Enable high resolution TIM5 + RCC->APB1ENR |= RCC_APB1ENR_TIM5EN; + } +} + +void EXTI1Config() +{ + //Enable SYSCFG for setting interrupts + { + FastInterruptDisableLock dl; + RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN; + } + + //Configure mask bit of 1st interrupt line + EXTI->IMR |= EXTI_IMR_MR1; + + //Configure trigger selection bit (rising edge) + EXTI->RTSR |= EXTI_RTSR_TR1; + + //Clear pending interrupt register before enable + EXTI->PR |= EXTI_PR_PR1; + + //Select PB1 as interrupt source in line 1 + SYSCFG->EXTICR[0] &= 0xFFFFFF1F; + + //Enable the interrupt in the interrupt controller + NVIC_EnableIRQ(EXTI1_IRQn); + NVIC_SetPriority(EXTI1_IRQn, 14); +} \ No newline at end of file diff --git a/src/tests/drivers/test-lsm9ds1-mag.cpp b/src/tests/drivers/test-lsm9ds1-mag.cpp deleted file mode 100644 index 40cf4e3bd805fc235730600d1cda018d9ab71b67..0000000000000000000000000000000000000000 --- a/src/tests/drivers/test-lsm9ds1-mag.cpp +++ /dev/null @@ -1,148 +0,0 @@ -/** - * test LSM9DS1 magneto - * Copyright (c) 2020 Skyward Experimental Rocketry - * Authors: Andrea Milluzzo - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#include "drivers/spi/SPIDriver.h" -#include "sensors/LSM9DS1/LSM9DSI_Magneto.h" - -#define MAX_ADDR 0x33 - -using namespace miosix; - -typedef Gpio<GPIOA_BASE, 5> GpioSck; //pin SPI1 per f407_discovery -typedef Gpio<GPIOA_BASE, 6> GpioMiso; -typedef Gpio<GPIOA_BASE, 7> GpioMosi; - - -//GpioPin PushButton(GPIOA_BASE, 0); -GpioPin LED(GPIOD_BASE, 15); - -Vec3 mdata; - -//SPI -SPIBus bus(SPI1); -SPIBusConfig cfg; -GpioPin cs_M(GPIOE_BASE, 8); - -int main() -{ - cfg.clock_div=SPIClockDivider::DIV64; - { - FastInterruptDisableLock dLock; - - RCC->APB2ENR |= RCC_APB2ENR_SPI1EN; //SPI1 ENABLE - - GpioSck::mode(Mode::ALTERNATE); - GpioMiso::mode(Mode::ALTERNATE); - GpioMosi::mode(Mode::ALTERNATE); - - cs_M.mode(Mode::OUTPUT); - - GpioSck::alternateFunction(5); - GpioMiso::alternateFunction(5); - GpioMosi::alternateFunction(5); - - GpioSck::speed(Speed::_25MHz); - - //PushButton.mode(Mode::INPUT); - LED.mode(Mode::OUTPUT); - } - - cs_M.high(); - - Thread::sleep(200); - - //dump&test registers - /*while(1) - { - if(PushButton.value()) - { - LED.high(); - - printf("\n\nREGISTER SET BEFORE INIT:\n\n"); - uint8_t regset1[0x34]; - for(int addr=0; addr<=MAX_ADDR; addr++) - { - SPISlave spislave(bus, cs_M, cfg); - SPITransaction spi(spislave); - regset1[addr] = spi.read(addr); - printf("0x%02X-->0x%02X\n", addr,regset1[addr]); - } - - LSM9DS1_M lsm9ds1( - bus, - cs_M, - LSM9DS1_M::MagFSR::FS_12, - LSM9DS1_M::ODR::ODR_40 - ); - - lsm9ds1.init(); - - printf("\n\nREGISTER SET AFTER INIT:\n\n"); - uint8_t regset2[0x34]; - for(int addr=0; addr<=MAX_ADDR; addr++) - { - SPISlave spislave(bus, cs_M, cfg); - SPITransaction spi(spislave); - regset2[addr] = spi.read(addr); - if(regset1[addr] == regset2[addr]) - { - printf("0x%02X-->0x%02X\n", addr,regset2[addr]); - } - else - { - printf("0x%02X-->0x%02X <--\n", addr,regset2[addr]); - } - } - } - else - LED.low(); - }*/ - - LSM9DS1_M lsm9ds1( - bus, - cs_M, - LSM9DS1_M::MagFSR::FS_12, - LSM9DS1_M::ODR::ODR_40 - ); - - while(!lsm9ds1.init()); - LED.high(); //init OK - - Thread::sleep(500); - printf("x;y;z\n"); - long long first_tick = getTick(); - for(;;) - { - long long last_tick = getTick(); - lsm9ds1.onSimpleUpdate(); - mdata = *(lsm9ds1.compassDataPtr()); - printf("%d;%.3f;%.3f;%.3f\n", - (int)(last_tick - first_tick), - mdata.getX(), mdata.getY(), mdata.getZ() - ); - } - - - -} diff --git a/src/tests/drivers/test-lsm9ds1.cpp b/src/tests/drivers/test-lsm9ds1.cpp index 3a01ac312fab82029f25827413bd12da20c1ce97..68b30977551376c46b0e0f78fa9a03d71075c042 100644 --- a/src/tests/drivers/test-lsm9ds1.cpp +++ b/src/tests/drivers/test-lsm9ds1.cpp @@ -34,8 +34,6 @@ typedef Gpio<GPIOA_BASE, 6> GpioMiso; typedef Gpio<GPIOA_BASE, 7> GpioMosi; static const bool FIFO_ENABLED = false; -Vec3 adata, gdata, mdata; -float tdata; //SPI SPIBus bus(SPI1); @@ -51,6 +49,10 @@ GpioPin LED2(GPIOD_BASE, 13); int main(){ + Vec3 adata, gdata, mdata; + float tdata; + + cfg.clock_div=SPIClockDivider::DIV64; {