123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364 |
- /******************************************************************************
- * 2016 ideasX (Tyler Berezowsky and Marc Jurchak)
- *
- * FileName: LSM6DS3.c
- *
- * Description: SPIdriver for LSM6DS3
- * Modification history:
- * 2015/10/12, v1.0 created this file (Marc)
- * 2016/3/15, v1.1 added comments for Tyler.
- *******************************************************************************/
- #include "hal/lsm6ds3.h"
- #include "log/esp_log.h"
- static const char* TAG = "lsm6ds3.c";
- static int32_t ICACHE_FLASH_ATTR Sensor_IO_Write(uint16_t WriteAddr, uint8_t nBytesToWrite, uint32_t *pBuffer)
- {
- // Initalize HSPI Interface GPIO
- WRITE_PERI_REG(PERIPHS_IO_MUX, 0x105);
- PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTDI_U, 2);
- PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTCK_U, 2);
- PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTMS_U, 2);
- PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTDO_U, 2);
- // Setup SPI Attributes Struct
- SpiAttr spiConfig;
- spiConfig.mode = SpiMode_Master;
- spiConfig.subMode = SpiSubMode_0;
- spiConfig.speed = SpiSpeed_8MHz;
- spiConfig.bitOrder = SpiBitOrder_MSBFirst;
- // Initalize SPI Interface
- SPIInit(SpiNum_HSPI, &spiConfig);
- // Setup SPI Data Struct
- SpiData spiData;
- uint32_t addr = (WriteAddr);
- spiData.cmd = addr;
- spiData.cmdLen = 1;
- spiData.addrLen = 0;
- spiData.addr = 0;
- spiData.data = pBuffer;
- spiData.dataLen = nBytesToWrite;
- // Write Data to SPI port
- return SPIMasterSendData(SpiNum_HSPI, &spiData);
- }
- static int32_t ICACHE_FLASH_ATTR Sensor_IO_Read(uint16_t ReadAddr, uint8_t nBytesToRead, uint32_t *pBuffer)
- {
- // Initalize HSPI Interface GPIO
- WRITE_PERI_REG(PERIPHS_IO_MUX, 0x105);
- PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTDI_U, 2);
- PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTCK_U, 2);
- PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTMS_U, 2);
- PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTDO_U, 2);
- // Setup SPI Attributes Struct
- SpiAttr spiConfig;
- spiConfig.mode = SpiMode_Master;
- spiConfig.subMode = SpiSubMode_0;
- spiConfig.speed = SpiSpeed_8MHz;
- spiConfig.bitOrder = SpiBitOrder_MSBFirst;
- // Initalize SPI Interface
- SPIInit(SpiNum_HSPI, &spiConfig);
- // Setup SPI Data Struct
- SpiData spiData;
- uint32_t addr = (ReadAddr | 0x80);
- spiData.cmd = addr;
- spiData.cmdLen = 1;
- spiData.addrLen = 0;
- spiData.addr = 0;
- spiData.data = pBuffer;
- spiData.dataLen = nBytesToRead;
- return SPIMasterRecvData(SpiNum_HSPI, &spiData);
- }
- static int32_t ICACHE_FLASH_ATTR Sensor_IO_Set_Register(uint16_t WriteAddr, uint32_t v0)
- {
- uint32_t v1;
- Sensor_IO_Write(WriteAddr, 1, &v0);
- Sensor_IO_Read(WriteAddr, 1, &v1);
- if (v0 == v1)
- return 0;
- else
- return -1;
- }
- /******************************************************************************
- * FunctionName : lsm6ds3_i2c_enable
- * Description : Setup LSM6DS3 registers for I2C sensor hub.
- * Parameters : uint8_t start
- 1: enable
- 0: disable
- * Returns : uint8_t 1: success 0: failed
- *******************************************************************************/
- int32_t ICACHE_FLASH_ATTR
- LSM6DS3_Enable_I2C_Pullups(uint8_t enable)
- {
- uint32_t ctrl10_c, master_config;
- Sensor_IO_Read(LSM6DS3_CTRL10_C, 1, &ctrl10_c);
- Sensor_IO_Read(LSM6DS3_MASTER_CONFIG, 1, &master_config);
- if (enable)
- {
- ctrl10_c |= LSM6DS3_FUNC_EN;
- //master_config |= (PULL_UP_EN + MASTER_ON);
- master_config |= LSM6DS3_PULL_UP_EN; // pull-ups on
- master_config &= ~LSM6DS3_MASTER_ON; // disable i2c master function?
- }
- else
- {
- ctrl10_c &= ~LSM6DS3_FUNC_EN;
- master_config &= ~(LSM6DS3_PULL_UP_EN + LSM6DS3_MASTER_ON);
- }
- return (Sensor_IO_Set_Register(LSM6DS3_CTRL10_C, ctrl10_c) & Sensor_IO_Set_Register(LSM6DS3_MASTER_CONFIG, master_config));
- }
- int32_t ICACHE_FLASH_ATTR
- LSM6DS3_Enable_I2C_Bridge(uint8_t enable)
- {
- LSM6DS3_Enable_I2C_Pullups(1);
- uint32_t master_config;
- Sensor_IO_Read(LSM6DS3_MASTER_CONFIG, 1, &master_config);
- if (enable)
- {
- master_config |= LSM6DS3_PASS_THROUGH_MODE;
- }
- else
- {
- master_config &= ~LSM6DS3_PASS_THROUGH_MODE;
- }
- ESP_LOGI(TAG, "Writing %x to LSM6DS3_MASTER_CONFIG", master_config);
- return Sensor_IO_Set_Register(LSM6DS3_MASTER_CONFIG, master_config);
- }
- // /******************************************************************************
- // * FunctionName : lsm6ds3_xl_config
- // * Description : Setup LSM6DS3 registers for accel. sample and other cool nonsense.
- // Note: 20ms is required from boot for the sensor to load some magic before you
- // can play.
- // * Parameters : bool en_hp_mode: Enable or Disable high performance mode. NOTE: HP mode is
- // is required for LP filter data and certain sampling rates.
- // odr_xl: Output data rate for XL. See .h file for available rates.
- // uint8_t scale_xl: Range of senstiviity selection. See .h file for available rates.
- // uint8_t bw_xl: Anti-aliasing LP filter selection. See .h file and datasheet.
- // * Returns : bool 1: success 0: failed
- // *******************************************************************************/
- // bool ICACHE_FLASH_ATTR
- // lsm6ds3_xl_config(bool en_hp_mode, uint8_t odr_xl, uint8_t scale_xl, uint8_t bw_xl)
- // {
- // uint8_t ctrl6_c = spi_rx8_address(HSPI, LSM6DS3_CTRL6_C);
- // uint8_t ctrl1_xl = odr_xl | scale_xl | bw_xl;
- //
- // if(en_hp_mode)
- // ctrl6_c &= ~LSM6DS3_XL_HM_MODE;
- // else
- // ctrl6_c |= LSM6DS3_XL_HM_MODE;
- // spi_tx8_address(HSPI, LSM6DS3_CTRL6_C, ctrl6_c);
- // spi_tx8_address(HSPI, LSM6DS3_CTRL1_XL, ctrl1_xl);
- // if ( (spi_rx8_address(HSPI, LSM6DS3_CTRL1_XL) == ctrl1_xl) & (spi_rx8_address(HSPI, LSM6DS3_CTRL6_C) == ctrl6_c) )
- // return 1;
- // else
- // return 0;
- // }
- // /******************************************************************************
- // * FunctionName : lsm6ds3_g_config
- // * Description : Setup LSM6DS3 registers for accel. sample and other cool nonsense.
- // Note: 20ms is required from boot for the sensor to load some magic before you
- // can play.
- // * Parameters : bool en_hp_mode: Enable or Disable high performance mode. NOTE: HP mode is
- // is required for LP filter data and certain sampling rates.
- // odr_xl: Output data rate for XL. See .h file for available rates.
- // uint8_t scale_xl: Range of senstiviity selection. See .h file for available rates.
- // uint8_t bw_xl: Anti-aliasing LP filter selection. See .h file and datasheet.
- // * Returns : bool 1: success 0: failed
- // *******************************************************************************/
- // bool ICACHE_FLASH_ATTR
- // lsm6ds3_g_config(bool en_hp_mode, bool hp_g_en, bool hp_g_rst,
- // uint8_t odr_g, uint8_t scale_g, uint8_t hpcf_g)
- // {
- // uint8_t ctrl7_g;
- // uint8_t ctrl2_g = odr_g | scale_g;
- //
- // if (en_hp_mode)
- // ctrl7_g = LSM6DS3_G_HM_MODE;
- // else
- // ctrl7_g &= ~LSM6DS3_G_HM_MODE;
- // if (hp_g_en)
- // ctrl7_g |= LSM6DS3_HP_G_EN | hpcf_g;
- // else
- // ctrl7_g &= ~LSM6DS3_HP_G_EN;
- // if (hp_g_rst)
- // ctrl7_g |= LSM6DS3_HP_G_RST;
- // else
- // ctrl7_g &= ~LSM6DS3_HP_G_RST;
- //
- // spi_tx8_address(HSPI, LSM6DS3_CTRL2_G, ctrl2_g);
- // spi_tx8_address(HSPI, LSM6DS3_CTRL7_G, ctrl7_g);
- // if ( (spi_rx8_address(HSPI, LSM6DS3_CTRL2_G) == ctrl2_g) & (spi_rx8_address(HSPI, LSM6DS3_CTRL7_G) == ctrl7_g))
- // return 1;
- // else
- // return 0;
- // }
- // /******************************************************************************
- // * FunctionName : lsm6ds3_fifo_config
- // * Description : Setup LSM6DS3 registers for FIFO. FIFO is automatically placed into continous mode.
- // Decimation is set to zero, and the ODR of the FIFO is set to match that max(XL_ODR, G_ODR)
- // INT1 is automatically enabled / disabled for FIFO overflow and threshold.
- // If FIFO is enable it will automatically disable the significant motion ISR.
- // * Parameters : bool enable: Enable or Disable FIFO
- // threshold: Set the threshold level of the FIFO
- // * Returns : bool 1: success 0: failed
- // *******************************************************************************/
- // bool ICACHE_FLASH_ATTR
- // lsm6ds3_fifo_config(uint16_t threshold)
- // {
- // uint8_t ctrl1_xl, ctrl2_g, max_odr, fifo_ctrl3;
- //
- // // read ODR rates
- // ctrl1_xl = spi_rx8_address(HSPI, LSM6DS3_CTRL1_XL) & 0xF0; // get ODR bits and clear the rest
- // ctrl2_g = spi_rx8_address(HSPI, LSM6DS3_CTRL2_G) & 0xF0; // get ODR bits and clear the rest
- // os_printf("CTRL1_XL, CTRL2_G: %x, %x\r\n", ctrl1_xl, ctrl2_g);
- // if (!(ctrl1_xl || ctrl2_g))
- // {
- // // disable FIFO
- // os_printf("LSM6DS3: XL and G disabled. FIFO disabled.\r\n");
- // lsm6ds3_set_register(LSM6DS3_FIFO_CTRL5, 0x00);
- // lsm6ds3_set_register(LSM6DS3_INT1_CTRL, 0x00); // disable INT1 for overflow and threshold
- //
- // }
- // else
- // {
- //
- // // set Decimation Factor to 1 if sensor is active
- // //lsm6ds3_set_register(FIFO_CTRL3, 0b00001001);
- //
- // // set threshold
- // lsm6ds3_set_register(LSM6DS3_FIFO_CTRL1, (uint8_t)(0xFF & threshold));
- // lsm6ds3_set_register(LSM6DS3_FIFO_CTRL2, (uint8_t)(0xFF & (threshold >> 8)));
- //
- // // set decimation factor to one if sensor ODR is not zero
- // fifo_ctrl3 = 0;
- // if (ctrl1_xl & 0xF0)
- // fifo_ctrl3 = BIT0;
- // if(ctrl2_g & 0xF0)
- // fifo_ctrl3 |= BIT3;
- // lsm6ds3_set_register(LSM6DS3_FIFO_CTRL3, fifo_ctrl3);
- // os_printf("FIFO_CTRL3: %x\r\n", fifo_ctrl3);
- //
- // // determine max ODR
- // if ( (ctrl1_xl) >= (ctrl2_g) )
- // max_odr = ctrl1_xl;
- // else
- // max_odr = ctrl2_g;
- // max_odr = max_odr >> 1; // format for fifo register
- // lsm6ds3_set_register(LSM6DS3_FIFO_CTRL5, max_odr | 0x06); // set FIFO ODR and mode to continously
- // lsm6ds3_set_register(LSM6DS3_INT1_CTRL, BIT3); // set INT1 for overflow and threshold
- // }
- // return 1;
- // }
- //
- // bool ICACHE_FLASH_ATTR lsm6ds3_reset(void)
- // {
- // lsm6ds3_set_register(LSM6DS3_CTRL3_C, BIT0);
- // os_delay_us(30000);
- // return !(bool)(spi_rx8_address(HSPI, LSM6DS3_CTRL3_C) & BIT7);
- // }
- //
- // uint16_t ICACHE_FLASH_ATTR lsm6ds3_fifo_dump(uint16_t *buf, uint16_t buf_len, uint16_t bufPos)
- // {
- // uint16_t samples = (uint16_t)spi_rx8_address(HSPI, LSM6DS3_FIFO_STATUS1);
- // samples |= (((uint16_t)spi_rx8_address(HSPI, LSM6DS3_FIFO_STATUS2) & 0x0F) << 8);
- //
- // // read and store into buff in multiples of 3
- // if (samples <= (buf_len - bufPos) && samples != 0)
- // {
- // uint16_t i;
- // for(i=bufPos; i<samples+bufPos; i++)
- // {
- // buf[i] = spi_rx16_address(HSPI, LSM6DS3_FIFO_DATA_OUT_L);
- // }
- // return samples;
- // }
- // else
- // return 0;
- //
- // }
- //
- //
- // uint8_t ICACHE_FLASH_ATTR lsm6ds3_fifo_getFlags(void)
- // {
- // /* There is (2) possible sources of interrupts caused by the IMU
- // that the ESP8266 actually cares about.
- // 1) The significant motion interrupt
- // 2) The FIFO interrupts
- //
- // LSM6DS3_FIFO_EMPTY
- // LSM6DS3_FIFO_FULL
- // LSM6DS3_FIFO_OVER_RUN
- // LSM6DS3_FIFO_THRESHOLD
- // */
- // return (uint8_t)spi_rx8_address(HSPI, LSM6DS3_FIFO_STATUS2);
- // }
- //
- // bool ICACHE_FLASH_ATTR lsm6ds3_read_motion(void)
- // {
- // uint8_t wake_up_src;
- // wake_up_src = SPIread(LSM6DS3_WAKE_UP_SRC);
- // //if (wake_up_src)
- // {
- // os_printf("WAKE_UP_SRC: %x\r\n", wake_up_src);
- // os_printf("Direction ");
- // if (wake_up_src & BIT2)
- // os_printf("x");
- // if (wake_up_src & BIT1)
- // os_printf("y");
- // if (wake_up_src & BIT0)
- // os_printf("z");
- //
- // os_printf("\r\n");
- // }
- // return wake_up_src & BIT3;
- // }
- //
- // bool ICACHE_FLASH_ATTR lsm6ds3_motion_enable(void)
- // {
- // SPIwrite(LSM6DS3_CTRL2_G, LSM6DS3_ODR_G_POWERDOWN); // disable gyro
- // SPIwrite(LSM6DS3_CTRL1_XL, (LSM6DS3_ODR_XL_13Hz + LSM6DS3_FS_XL_2G)); // 208Hz 2g scale
- // SPIwrite(LSM6DS3_TAP_CFG, LSM6DS3_LIR); // enable interuppt latch and slope filter
- // SPIwrite(LSM6DS3_WAKE_UP_THS, 0x01); // set thres 32/64*2G = 1G
- // SPIwrite(LSM6DS3_WAKE_UP_DUR, 0x02); // zero duration
- // SPIwrite(LSM6DS3_MD1_CFG, LSM6DS3_INT1_WU); // set interrupt for INTX (1 is connected to PBA)
- // SPIwrite(LSM6DS3_CTRL3_C, LSM6DS3_H_LACTIVE); // set int lines active low
- // os_printf("MOTION PROCESS: Setup IMU for motion detection\r\n");
- //
- // return 1;
- // }
- //
- // bool ICACHE_FLASH_ATTR lsm6ds3_motion_disable(void)
- // {
- // lsm6ds3_reset();
- // SPIwrite(LSM6DS3_CTRL2_G, LSM6DS3_ODR_G_POWERDOWN); // disable gyro
- // SPIwrite(LSM6DS3_CTRL1_XL, LSM6DS3_ODR_XL_POWERDOWN); // disable XL
- // SPIwrite(LSM6DS3_CTRL3_C, LSM6DS3_H_LACTIVE);
- // return 1;
- // }
- //
- // #if LSM6DS3_DEBUG
- // void ICACHE_FLASH_ATTR
- // lsm6ds3_xl_print(void)
- // {
- // os_printf("XL_X: %d\r\nXL_Y: %d\r\n XL_Z: %d\r\n",
- // (SPIread(LSM6DS3_OUTX_H_XL)<<8)+SPIread(LSM6DS3_OUTX_L_XL), (SPIread(LSM6DS3_OUTY_H_XL)<<8)+SPIread(LSM6DS3_OUTY_L_XL),
- // (SPIread(LSM6DS3_OUTZ_H_XL)<<8)+SPIread(LSM6DS3_OUTX_L_XL));
- // return;
- // }
- // #endif
|