diff --git a/sbs.conf b/sbs.conf index b879db4ca58e2ddc186c2f739679543f985d1659..20f00f2773861e1e03f445adb1ddb0f92397041a 100644 --- a/sbs.conf +++ b/sbs.conf @@ -498,3 +498,11 @@ BinName: test-rls Include: %shared Defines: Main: test-rls + +[test-lsm9ds1-mag] +Type: test +BoardId: stm32f407vg_stm32f4discovery +BinName: test-lsm9ds1-mag +Include: %shared %spi +Defines: -DDEBUG +Main: drivers/test-lsm9ds1-mag diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h index 09f7767cc352a06fef5fc355d409f2830e5fa343..f24c3ed46ab896b6b503eebf8e227cb2a0ad1dfe 100644 --- a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h +++ b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h @@ -55,8 +55,8 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen enum ODR { PWR_DW = 0X00, - ODR_10 = 0X01, - ODR_50 = 0X02, + ODR_15 = 0X01, + ODR_60 = 0X02, ODR_119 = 0X03, ODR_238 = 0X04, ODR_476 = 0X05, @@ -82,7 +82,7 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen GpioPin cs, AxelFSR axelRange = AxelFSR::FS_2, GyroFSR gyroRange = GyroFSR::FS_245, - ODR odr = ODR::ODR_10, + ODR odr = ODR::ODR_15, bool fifo_enabled = false, unsigned int fifo_watermark = 24 ):fifo_enabled(fifo_enabled), fifo_watermark(fifo_watermark), @@ -97,7 +97,7 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen SPIBusConfig config, AxelFSR axelRange = AxelFSR::FS_2, GyroFSR gyroRange = GyroFSR::FS_245, - ODR odr = ODR::ODR_10, + ODR odr = ODR::ODR_15, bool fifo_enabled = false, unsigned int fifo_watermark = 24 ):fifo_enabled(fifo_enabled), fifo_watermark(fifo_watermark), @@ -127,7 +127,7 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen break; } - switch (gyroFSR) + switch(gyroFSR) { case GyroFSR::FS_245: gyroSensitivity = 8.75f; @@ -143,12 +143,40 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen break; } + switch(odr) + { + case ODR::PWR_DW: + odrHz = 0.0f; + break; + case ODR::ODR_15: + odrHz = 14.9f; + break; + case ODR::ODR_60: + odrHz = 59.5f; + break; + case ODR::ODR_119: + odrHz = 119.0f; + break; + case ODR::ODR_238: + odrHz = 238.0f; + break; + case ODR::ODR_476: + odrHz = 476.0f; + break; + case ODR::ODR_952: + odrHz = 952.0f; + break; + default: + odrHz = 14.9f; + break; + } + SPITransaction spi(spislave); //Who Am I check: uint8_t whoami = spi.read(regMapXLG::WHO_AM_I); if(whoami != WHO_AM_I_XLG_VAL){ - TRACE("LSM9DS1 WAMI: %02X\n", whoami); + TRACE("LSM9DS1 AXEL+GYRO WAMI: %02X\n", whoami); last_error = ERR_NOT_ME; return false; } @@ -197,16 +225,20 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen //@ startup, some samples have to be discarded (datasheet) - uint16_t toWait_ms = samplesToDiscard * 1000 / (int)odr; - 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++) + 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) { - spi.read(regMapXLG::OUT_X_L_XL, 6); - spi.read(regMapXLG::OUT_X_L_G, 6); + //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); + } } } return true; @@ -263,7 +295,11 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen //read FIFO status and dump all the samples inside the FIFO uint8_t fifo_src = spi.read(FIFO_SRC); fifo_samples = fifo_src & 0x3F; - + //sanity check + if(fifo_samples > 32) + { + fifo_samples = 32; + } spi.read(OUT_X_L_G, buf, fifo_samples*12); //format: gxl,gxh,gyl,gyh,gzl,gzh,axl,axh,ayl,ayh,azl,azh for each sample } //convert & store @@ -322,6 +358,7 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen float axelSensitivity; float gyroSensitivity; + float odrHz; float tempZero = 25.0f; float tempSensistivity = 16.0f; static const uint8_t samplesToDiscard = 8; //max possible val diff --git a/src/shared/sensors/LSM9DS1/LSM9DSI_Magneto.h b/src/shared/sensors/LSM9DS1/LSM9DSI_Magneto.h index f1eb18151251300f7a417835a23b91bc3bef280d..658899900bd8d65693e054661ee0495fd9849f31 100644 --- a/src/shared/sensors/LSM9DS1/LSM9DSI_Magneto.h +++ b/src/shared/sensors/LSM9DS1/LSM9DSI_Magneto.h @@ -73,7 +73,7 @@ class LSM9DS1_M : public CompassSensor ODR odr = ODR::ODR_0_625 ): spislave(bus, cs), magFSR(magRange), odr(odr){ //SPI config - spislave.config.br = SPIBaudRate::DIV_64; //baud = fclk/64 + spislave.config.clock_div=SPIClockDivider::DIV64; } LSM9DS1_M( @@ -92,49 +92,53 @@ class LSM9DS1_M : public CompassSensor switch(magFSR) { case MagFSR::FS_4: - magFSRval = 4.0f; + magSensitivity = 0.14f; break; case MagFSR::FS_8: - magFSRval = 8.0f; + magSensitivity = 0.29f; break; case MagFSR::FS_12: - magFSRval = 12.0f; + magSensitivity = 0.43f; break; case MagFSR::FS_16: - magFSRval = 16.0f; + magSensitivity = 0.58f; break; default: - magFSRval = 4.0f; + magSensitivity = 0.14f; break; } SPITransaction spi(spislave); //Who Am I check: - uint8_t whoami = spi.read(regMapM::WHO_AM_I_M); + uint8_t whoami = spi.read(WHO_AM_I_M);//regMapM::WHO_AM_I_M); + if(whoami != WHO_AM_I_M_VAL){ - TRACE("LSM9DS1 WAMI: %d\n", whoami); + TRACE("LSM9DS1 MAG WAMI: 0x%02X\n", whoami); last_error = ERR_NOT_ME; return false; } - + //setup uint8_t CTRL_REG1_M_VAL = 0x60 | (int)odr<<2; spi.write(regMapM::CTRL_REG1_M, CTRL_REG1_M_VAL); //X,Y axes in ultra-high performance mode, NO auto-temp compensation and ODR defined by constructor - + uint8_t CTRL_REG2_M_VAL = (int)magFSR<<5; spi.write(regMapM::CTRL_REG2_M, CTRL_REG2_M_VAL); //FSR defined by constructor spi.write(regMapM::CTRL_REG4_M, CTRL_REG4_M_VAL); //Z axis in ultra-high performance mode spi.write(regMapM::CTRL_REG3_M, CTRL_REG3_M_VAL); //I2C disabled, SPI mode: read/write + + spi.write(regMapM::INT_CFG_M, INT_CFG_M_VAL); //disable all interrupts //check that all registers have been written correctly - if(spi.read(regMapM::CTRL_REG1_M) != CTRL_REG1_M_VAL) return false; - if(spi.read(regMapM::CTRL_REG2_M) != CTRL_REG2_M_VAL) return false; - if(spi.read(regMapM::CTRL_REG3_M) != CTRL_REG3_M_VAL) return false; - if(spi.read(regMapM::CTRL_REG4_M) != CTRL_REG4_M_VAL) return false; + if(spi.read(regMapM::CTRL_REG1_M) != CTRL_REG1_M_VAL) {return false;} + if(spi.read(regMapM::CTRL_REG2_M) != CTRL_REG2_M_VAL) {return false;} + if(spi.read(regMapM::CTRL_REG3_M) != CTRL_REG3_M_VAL) {return false;} + if(spi.read(regMapM::CTRL_REG4_M) != CTRL_REG4_M_VAL) {return false;} + if(spi.read(regMapM::INT_CFG_M) != INT_CFG_M_VAL) {return false;} return true; } @@ -161,14 +165,15 @@ class LSM9DS1_M : public CompassSensor // TRACE("LSM9DS1 mageto: %02X,%02X,%02X\n", x, y, z); mLastCompass = - Vec3(x * magFSRval / 0xFFFF, - y * magFSRval / 0xFFFF, - z * magFSRval / 0xFFFF); + Vec3(x * magSensitivity / 1000, + y * magSensitivity / 1000, + z * magSensitivity / 1000); + return true; } void setOffset(vector<uint16_t>& offVect) //X,Y,Z { - if(offVect.size < 3) return; + if(offVect.size() < 3) return; uint8_t toStore[6]; for(int i=6; i > 0 ; i=i-2){ toStore[i-1] = offVect.back() & 0x00FF; //LSB @@ -188,7 +193,7 @@ class LSM9DS1_M : public CompassSensor MagFSR magFSR; ODR odr; - float magFSRval; + float magSensitivity; enum regMapM { @@ -218,8 +223,9 @@ class LSM9DS1_M : public CompassSensor }; static const uint8_t WHO_AM_I_M_VAL = 0x3D; - static const uint8_t CTRL_REG3_M_VAL = 0x84; + static const uint8_t CTRL_REG3_M_VAL = 0x80; static const uint8_t CTRL_REG4_M_VAL = 0x0C; + static const uint8_t INT_CFG_M_VAL = 0x00; }; \ No newline at end of file diff --git a/src/tests/drivers/test-lsm9ds1-mag.cpp b/src/tests/drivers/test-lsm9ds1-mag.cpp new file mode 100644 index 0000000000000000000000000000000000000000..40cf4e3bd805fc235730600d1cda018d9ab71b67 --- /dev/null +++ b/src/tests/drivers/test-lsm9ds1-mag.cpp @@ -0,0 +1,148 @@ +/** + * 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 e97c6059c9d7b26cef72ea99469c677be6115898..7967308a440b1437312a7eb383c43cb14ab5b6e3 100644 --- a/src/tests/drivers/test-lsm9ds1.cpp +++ b/src/tests/drivers/test-lsm9ds1.cpp @@ -39,7 +39,7 @@ float tdata; //SPI SPIBus bus(SPI1); SPIBusConfig cfg; -GpioPin cs(GPIOE_BASE, 7); +GpioPin cs_XLG(GPIOE_BASE, 7); @@ -55,7 +55,8 @@ int main(){ GpioSck::mode(Mode::ALTERNATE); GpioMiso::mode(Mode::ALTERNATE); GpioMosi::mode(Mode::ALTERNATE); - cs.mode(Mode::OUTPUT); + + cs_XLG.mode(Mode::OUTPUT); GpioSck::alternateFunction(5); GpioMiso::alternateFunction(5); @@ -65,16 +66,33 @@ int main(){ } - cs.high(); + cs_XLG.high(); + +//dump regiters +/* + for(int reg=0; reg<=0x38; reg++) + { + SPISlave spislave(bus, cs_XLG, cfg); + SPITransaction spi(spislave); + uint8_t data = spi.read(reg); + printf("0x%02X-->0x%02X\n", reg,data); + } +}*/ + + + LSM9DS1_XLG lsm9ds1( bus, - cs, + cs_XLG, + cfg, LSM9DS1_XLG::AxelFSR::FS_8, LSM9DS1_XLG::GyroFSR::FS_245, LSM9DS1_XLG::ODR::ODR_952 ); + while(!lsm9ds1.init()){} + Thread::sleep(500); printf("ax,ay,az,gx,gy,gz,t\n"); long long first_tick = getTick(); @@ -86,7 +104,7 @@ int main(){ gdata = *(lsm9ds1.gyroDataPtr()); tdata = *(lsm9ds1.tempDataPtr()); printf("%d;%.3f;%.3f;%.3f;%.3f;%.3f;%.3f;%.1f\n", - (last_tick - first_tick), + (int)(last_tick - first_tick), adata.getX(), adata.getY(), adata.getZ(), gdata.getX(), gdata.getY(), gdata.getZ(), tdata);