|
@@ -140,225 +140,57 @@ LSM6DS3_Enable_I2C_Bridge(uint8_t enable)
|
|
|
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
|
|
|
+void ICACHE_FLASH_ATTR LSM6DS3_Reset(void)
|
|
|
+{
|
|
|
+ Sensor_IO_Set_Register(LSM6DS3_CTRL3_C, BIT0);
|
|
|
+ os_delay_us(30000);
|
|
|
+}
|
|
|
+
|
|
|
+void ICACHE_FLASH_ATTR LSM6DS3_EnableMotion(void)
|
|
|
+{
|
|
|
+ Sensor_IO_Set_Register(LSM6DS3_CTRL2_G, LSM6DS3_ODR_G_POWERDOWN); // disable gyro
|
|
|
+ Sensor_IO_Set_Register(LSM6DS3_CTRL1_XL, (LSM6DS3_ODR_XL_13Hz + LSM6DS3_FS_XL_2G)); // 208Hz 2g scale
|
|
|
+ Sensor_IO_Set_Register(LSM6DS3_TAP_CFG, LSM6DS3_LIR); // enable interuppt latch and slope filter
|
|
|
+ Sensor_IO_Set_Register(LSM6DS3_WAKE_UP_THS, 0x01); // set thres 32/64*2G = 1G
|
|
|
+ Sensor_IO_Set_Register(LSM6DS3_WAKE_UP_DUR, 0x02); // zero duration
|
|
|
+ Sensor_IO_Set_Register(LSM6DS3_MD1_CFG, LSM6DS3_INT1_WU); // set interrupt for INTX (1 is connected to PBA)
|
|
|
+ Sensor_IO_Set_Register(LSM6DS3_CTRL3_C, LSM6DS3_H_LACTIVE); // set int lines active low
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void ICACHE_FLASH_ATTR LSM6DS3_DisableMotion(void)
|
|
|
+{
|
|
|
+ LSM6DS3_Reset();
|
|
|
+ Sensor_IO_Set_Register(LSM6DS3_CTRL2_G, LSM6DS3_ODR_G_POWERDOWN); // disable gyro
|
|
|
+ Sensor_IO_Set_Register(LSM6DS3_CTRL1_XL, LSM6DS3_ODR_XL_POWERDOWN); // disable XL
|
|
|
+ Sensor_IO_Set_Register(LSM6DS3_CTRL3_C, LSM6DS3_H_LACTIVE);
|
|
|
+}
|
|
|
+
|
|
|
+// determine if there has been a FIFO or significant motion interrupt
|
|
|
+LSM6DS3_Interrupt_et ICACHE_FLASH_ATTR LSM6DS3_Get_Interrupts(void)
|
|
|
+{
|
|
|
+ uint32_t fifo_status2, wake_up_src;
|
|
|
+ Sensor_IO_Read(LSM6DS3_FIFO_STATUS2, 1, &fifo_status2);
|
|
|
+ Sensor_IO_Read(LSM6DS3_WAKE_UP_SRC, 1, &wake_up_src);
|
|
|
+
|
|
|
+
|
|
|
+ if (wake_up_src)
|
|
|
+ {
|
|
|
+ ESP_LOGV(TAG, "significant motion interrupt");
|
|
|
+ return LSM6DS3_SIGN_MOTION_INT;
|
|
|
+ }
|
|
|
+ else if (fifo_status2 & LSM6DS3_FIFO_OVER_RUN)
|
|
|
+ {
|
|
|
+ ESP_LOGV(TAG, "FIFO overflow interrupt");
|
|
|
+ return LSM6DS3_FIFO_OVERFLOW_INT;
|
|
|
+ }
|
|
|
+ else if (fifo_status2 & LSM6DS3_FIFO_FULL)
|
|
|
+ {
|
|
|
+ ESP_LOGV(TAG, "FIFO full interrupt");
|
|
|
+ return LSM6DS3_FIFO_FULL_INT;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ return LSM6DS3_NO_INT;
|
|
|
+ }
|
|
|
+}
|