diff --git a/stm32/mainboard_firmware/debug.cfg b/stm32/mainboard_firmware/debug.cfg new file mode 100644 index 0000000..59fa6fd --- /dev/null +++ b/stm32/mainboard_firmware/debug.cfg @@ -0,0 +1,7 @@ +source [find interface/stlink-v2.cfg] +source [find target/stm32f4x.cfg] +init + +tpiu config internal - uart off 84000000 +itm ports on + diff --git a/stm32/mainboard_firmware/orig_firmware.bin b/stm32/mainboard_firmware/orig_firmware.bin new file mode 100644 index 0000000..b472226 Binary files /dev/null and b/stm32/mainboard_firmware/orig_firmware.bin differ diff --git a/stm32/ros_usbnode/include/board.h b/stm32/ros_usbnode/include/board.h index 29e4bd4..2c8df1e 100755 --- a/stm32/ros_usbnode/include/board.h +++ b/stm32/ros_usbnode/include/board.h @@ -104,7 +104,7 @@ extern "C" #define EXTERNAL_IMU_ANGULAR 1 // Force disable IMU to be detected - CURRENTLY THIS SETTING DOES NOT WORK! -//#define DISABLE_ALTIMU10v5 +//#define DISABLE_LSM6 //#define DISABLE_MPU6050 //#define DISABLE_WT901 @@ -254,7 +254,7 @@ extern "C" #define PANEL_USART_IRQ USART1_IRQn #endif -// J18 has the SPI3 pins, as we dont use SPI3, we recycle them for I2C Bitbanging (for our Pololu ALtIMU-10v5) +// J18 has the SPI3 pins, as we dont use SPI3, we recycle them for I2C Bitbanging (for our IMU) #ifdef SOFT_I2C_ENABLED #define SOFT_I2C_SCL_PIN GPIO_PIN_3 #define SOFT_I2C_SCL_PORT GPIOB diff --git a/stm32/ros_usbnode/include/imu/altimu-10v5.h b/stm32/ros_usbnode/include/imu/altimu-10v5.h deleted file mode 100644 index 57acb5d..0000000 --- a/stm32/ros_usbnode/include/imu/altimu-10v5.h +++ /dev/null @@ -1,151 +0,0 @@ - -#ifndef __ALTIMU_10V5_H -#define __ALTIMU_10V5_H - -/* Calibration, Conversion factors */ - -#define DS33_G_FACTOR 0.000061f // LSM6DS33 datasheet (page 15) 0.061 mg/LSB -#define DS33_DPS_FACTOR 0.00875f // LSM6DS33 datasheet (page 15) 0.00875 °/sec/LSB -#define LIS3MDL_GAUSS_FACTOR 1.0/6842 // LIS3MDL datasheet (page8) Gauss/LSB - -/* Gyro / Accelerometer */ -#define DS33_ADDRESS 0b1101011 -#define DS33_WHO_ID 0x69 // WHO_AM_I will report this value - -/* Magnetometer */ -#define LIS3MDL_ADDRESS 0b0011110 -#define LIS3MDL_WHO_ID 0x3D // WHO_AM_I will report this value - -/* Barometer */ -#define LPS25H_ADDRESS 0b1011101 -#define LPS25H_WHO_ID 0xBD // WHO_AM_I will report this value - -/* Conversion functions */ -#define IMU_MAG_TO_T(r) ((r)*LIS3MDL_GAUSS_FACTOR*T_PER_GAUSS) - -#define DS33_FUNC_CFG_ACCESS 0x01 -#define DS33_FIFO_CTRL1 0x06 -#define DS33_FIFO_CTRL2 0x07 -#define DS33_FIFO_CTRL3 0x08 -#define DS33_FIFO_CTRL4 0x09 -#define DS33_FIFO_CTRL5 0x0A -#define DS33_ORIENT_CFG_G 0x0B -#define DS33_INT1_CTRL 0x0D -#define DS33_INT2_CTRL 0x0E -#define DS33_WHO_AM_I 0x0F -#define DS33_CTRL1_XL 0x10 -#define DS33_CTRL2_G 0x11 -#define DS33_CTRL3_C 0x12 -#define DS33_CTRL4_C 0x13 -#define DS33_CTRL5_C 0x14 -#define DS33_CTRL6_C 0x15 -#define DS33_CTRL7_G 0x16 -#define DS33_CTRL8_XL 0x17 -#define DS33_CTRL9_XL 0x18 -#define DS33_CTRL10_C 0x19 -#define DS33_WAKE_UP_SRC 0x1B -#define DS33_TAP_SRC 0x1C -#define DS33_D6D_SRC 0x1D -#define DS33_STATUS_REG 0x1E -#define DS33_OUT_TEMP_L 0x20 -#define DS33_OUT_TEMP_H 0x21 -#define DS33_OUTX_L_G 0x22 -#define DS33_OUTX_H_G 0x23 -#define DS33_OUTY_L_G 0x24 -#define DS33_OUTY_H_G 0x25 -#define DS33_OUTZ_L_G 0x26 -#define DS33_OUTZ_H_G 0x27 -#define DS33_OUTX_L_XL 0x28 -#define DS33_OUTX_H_XL 0x29 -#define DS33_OUTY_L_XL 0x2A -#define DS33_OUTY_H_XL 0x2B -#define DS33_OUTZ_L_XL 0x2C -#define DS33_OUTZ_H_XL 0x2D -#define DS33_FIFO_STATUS1 0x3A -#define DS33_FIFO_STATUS2 0x3B -#define DS33_FIFO_STATUS3 0x3C -#define DS33_FIFO_STATUS4 0x3D -#define DS33_FIFO_DATA_OUT_L 0x3E -#define DS33_FIFO_DATA_OUT_H 0x3F -#define DS33_TIMESTAMP0_REG 0x40 -#define DS33_TIMESTAMP1_REG 0x41 -#define DS33_TIMESTAMP2_REG 0x42 -#define DS33_STEP_TIMESTAMP_L 0x49 -#define DS33_STEP_TIMESTAMP_H 0x4A -#define DS33_STEP_COUNTER_L 0x4B -#define DS33_STEP_COUNTER_H 0x4C -#define DS33_FUNC_SRC 0x53 -#define DS33_TAP_CFG 0x58 -#define DS33_TAP_THS_6D 0x59 -#define DS33_INT_DUR2 0x5A -#define DS33_WAKE_UP_THS 0x5B -#define DS33_WAKE_UP_DUR 0x5C -#define DS33_FREE_FALL 0x5D -#define DS33_MD1_CFG 0x5E -#define DS33_MD2_CFG 0x5F - -#define LIS3MDL_WHO_AM_I 0x0F -#define LIS3MDL_CTRL_REG1 0x20 -#define LIS3MDL_CTRL_REG2 0x21 -#define LIS3MDL_CTRL_REG3 0x22 -#define LIS3MDL_CTRL_REG4 0x23 -#define LIS3MDL_CTRL_REG5 0x24 -#define LIS3MDL_STATUS_REG 0x27 -#define LIS3MDL_OUT_X_L 0x28 -#define LIS3MDL_OUT_X_H 0x29 -#define LIS3MDL_OUT_Y_L 0x2A -#define LIS3MDL_OUT_Y_H 0x2B -#define LIS3MDL_OUT_Z_L 0x2C -#define LIS3MDL_OUT_Z_H 0x2D -#define LIS3MDL_TEMP_OUT_L 0x2E -#define LIS3MDL_TEMP_OUT_H 0x2F -#define LIS3MDL_INT_CFG 0x30 -#define LIS3MDL_INT_SRC 0x31 -#define LIS3MDL_INT_THS_L 0x32 -#define LIS3MDL_INT_THS_H 0x33 - - -#define LPS25H_REF_P_XL 0x08 -#define LPS25H_REF_P_L 0x09 -#define LPS25H_REF_P_H 0x0A -#define LPS25H_WHO_AM_I 0x0F -#define LPS25H_RES_CONF 0x10 -#define LPS25H_CTRL_REG1 0x20 -#define LPS25H_CTRL_REG2 0x21 -#define LPS25H_CTRL_REG3 0x22 -#define LPS25H_CTRL_REG4 0x23 -#define LPS25H_STATUS_REG 0x27 -#define LPS25H_PRESS_OUT_XL 0x28 -#define LPS25H_PRESS_OUT_L 0x29 -#define LPS25H_PRESS_OUT_H 0x2A -#define LPS25H_TEMP_OUT_L 0x2B -#define LPS25H_TEMP_OUT_H 0x2C -#define LPS25H_FIFO_CTRL 0x2E -#define LPS25H_FIFO_STATUS 0x2F -#define LPS25H_RPDS_L 0x39 -#define LPS25H_RPDS_H 0x3A -#define LPS25H_INTERRUPT_CFG 0x24 -#define LPS25H_INT_SOURCE 0x25 -#define LPS25H_THS_P_L 0x30 -#define LPS25H_THS_P_H 0x31 - -uint8_t ALTIMU10v5_TestDevice(void); -/** - * @brief Initialize IMU - * LSM6DS33 +/- 2g acceleration and 245 gps for gyro - */ -void ALTIMU10v5_Init(void); - -/** - * @brief Reads the 3 accelerometer channels and stores them in *x,*y,*z - * units are m/s^2 - */ -void ALTIMU10v5_ReadAccelerometerRaw(float *x, float *y, float *z); - -/** - * @brief Reads the 3 gyro channels and stores them in *x,*y,*z - * units are rad/sec - */ -void ALTIMU10v5_ReadGyroRaw(float *x, float *y, float *z); - -#endif /* __ALTIMU_10V5_H */ diff --git a/stm32/ros_usbnode/include/imu/lsm6.h b/stm32/ros_usbnode/include/imu/lsm6.h new file mode 100644 index 0000000..4e82ab0 --- /dev/null +++ b/stm32/ros_usbnode/include/imu/lsm6.h @@ -0,0 +1,98 @@ + +#ifndef __LSM6_H +#define __LSM6_H + +/* Calibration, Conversion factors */ + +#define LSM6_G_FACTOR 0.000061f // LSM6DS33 datasheet (page 15) 0.061 mg/LSB +#define LSM6_DPS_FACTOR 0.00875f // LSM6DS33 datasheet (page 15) 0.00875 °/sec/LSB + +/* Gyro / Accelerometer */ +#define LSM6_SA0_LOW_ADDRESS 0b1101010 +#define LSM6_SA0_HIGH_ADDRESS 0b1101011 +#define DSO_WHO_ID 0x6C // WHO_AM_I will report this value +#define DS33_WHO_ID 0x69 // WHO_AM_I will report this value + + +#define LSM6_FUNC_CFG_ACCESS 0x01 +#define LSM6_FIFO_CTRL1 0x06 +#define LSM6_FIFO_CTRL2 0x07 +#define LSM6_FIFO_CTRL3 0x08 +#define LSM6_FIFO_CTRL4 0x09 +#define LSM6_FIFO_CTRL5 0x0A +#define LSM6_ORIENT_CFG_G 0x0B +#define LSM6_INT1_CTRL 0x0D +#define LSM6_INT2_CTRL 0x0E +#define LSM6_WHO_AM_I 0x0F +#define LSM6_CTRL1_XL 0x10 +#define LSM6_CTRL2_G 0x11 +#define LSM6_CTRL3_C 0x12 +#define LSM6_CTRL4_C 0x13 +#define LSM6_CTRL5_C 0x14 +#define LSM6_CTRL6_C 0x15 +#define LSM6_CTRL7_G 0x16 +#define LSM6_CTRL8_XL 0x17 +#define LSM6_CTRL9_XL 0x18 +#define LSM6_CTRL10_C 0x19 +#define LSM6_WAKE_UP_SRC 0x1B +#define LSM6_TAP_SRC 0x1C +#define LSM6_D6D_SRC 0x1D +#define LSM6_STATUS_REG 0x1E +#define LSM6_OUT_TEMP_L 0x20 +#define LSM6_OUT_TEMP_H 0x21 +#define LSM6_OUTX_L_G 0x22 +#define LSM6_OUTX_H_G 0x23 +#define LSM6_OUTY_L_G 0x24 +#define LSM6_OUTY_H_G 0x25 +#define LSM6_OUTZ_L_G 0x26 +#define LSM6_OUTZ_H_G 0x27 +#define LSM6_OUTX_L_XL 0x28 +#define LSM6_OUTX_H_XL 0x29 +#define LSM6_OUTY_L_XL 0x2A +#define LSM6_OUTY_H_XL 0x2B +#define LSM6_OUTZ_L_XL 0x2C +#define LSM6_OUTZ_H_XL 0x2D +#define LSM6_FIFO_STATUS1 0x3A +#define LSM6_FIFO_STATUS2 0x3B +#define LSM6_FIFO_STATUS3 0x3C +#define LSM6_FIFO_STATUS4 0x3D +#define LSM6_FIFO_DATA_OUT_L 0x3E +#define LSM6_FIFO_DATA_OUT_H 0x3F +#define LSM6_TIMESTAMP0_REG 0x40 +#define LSM6_TIMESTAMP1_REG 0x41 +#define LSM6_TIMESTAMP2_REG 0x42 +#define LSM6_STEP_TIMESTAMP_L 0x49 +#define LSM6_STEP_TIMESTAMP_H 0x4A +#define LSM6_STEP_COUNTER_L 0x4B +#define LSM6_STEP_COUNTER_H 0x4C +#define LSM6_FUNC_SRC 0x53 +#define LSM6_TAP_CFG 0x58 +#define LSM6_TAP_THS_6D 0x59 +#define LSM6_INT_DUR2 0x5A +#define LSM6_WAKE_UP_THS 0x5B +#define LSM6_WAKE_UP_DUR 0x5C +#define LSM6_FREE_FALL 0x5D +#define LSM6_MD1_CFG 0x5E +#define LSM6_MD2_CFG 0x5F + + +uint8_t LSM6_TestDevice(void); +/** + * @brief Initialize IMU + * LSM6 +/- 2g acceleration + */ +void LSM6_Init(void); + +/** + * @brief Reads the 3 accelerometer channels and stores them in *x,*y,*z + * units are m/s^2 + */ +void LSM6_ReadAccelerometerRaw(float *x, float *y, float *z); + +/** + * @brief Reads the 3 gyro channels and stores them in *x,*y,*z + * units are rad/sec + */ +void LSM6_ReadGyroRaw(float *x, float *y, float *z); + +#endif /* __LSM6_H */ \ No newline at end of file diff --git a/stm32/ros_usbnode/src/imu/altimu-10v5.c b/stm32/ros_usbnode/src/imu/altimu-10v5.c deleted file mode 100644 index e503965..0000000 --- a/stm32/ros_usbnode/src/imu/altimu-10v5.c +++ /dev/null @@ -1,223 +0,0 @@ - -/** - ****************************************************************************** - * @file altimu-10v5.c - * @author Georg Swoboda - * @brief Mowgli driver for the Pololu AltIMU-10v5 IMU - ****************************************************************************** - * @attention - * - * Some code taken from the various Arduino drivers referenced at - * https://www.pololu.com/product/2739 - * - * This IMU consists of: - * LSM6DS33 (gyro and accelerometer) - * LIS3MDL (magnetometer) - * LPS25H (barometer) - ****************************************************************************** - */ - -#include "imu/imu.h" -#include "imu/altimu-10v5.h" -#include "soft_i2c.h" -#include "main.h" -#include - -#ifndef DISABLE_ALTIMU10v5 -/** - * @brief Test Device - * Perform any tests possible before actually enabling and using the device, - * for example check the i2c address and whoami registers if existing - * - * @param ctx read / write interface definitions - * @param val get the values of hpcf in reg CTRL_REG2 - * @retval 0 -> test failed 1-> test ok, good to init and use - * - */ -uint8_t ALTIMU10v5_TestDevice(void) -{ - uint8_t val; - - /* test the LSM6DS33 (gyro and accelerometer) */ - val = SW_I2C_UTIL_Read(DS33_ADDRESS, DS33_WHO_AM_I); - if (val == DS33_WHO_ID) - { - debug_printf(" > [AltIMU-10v5] - LSM6DS33 (Gyro / Accelerometer) FOUND at I2C addr=0x%0x\r\n", DS33_ADDRESS); - } - else - { - debug_printf(" > [AltIMU-10v5] - Error probing for LSM6DS33 (Gyro / Accelerometer) at I2C addr=0x%0x\r\n", DS33_ADDRESS); - return(0); - } - /* test the LIS3MDL (magnetometer) */ - val = SW_I2C_UTIL_Read(LIS3MDL_ADDRESS, LIS3MDL_WHO_AM_I); - if (val == LIS3MDL_WHO_ID) - { - debug_printf(" > [AltIMU-10v5] - LIS3MDL (Magnetometer) FOUND at I2C addr=0x%0x\r\n", LIS3MDL_ADDRESS); - } - else - { - debug_printf(" > [AltIMU-10v5] - Error probing for LIS3MDL (Magnetometer) at I2C addr=0x%0x\r\n", LIS3MDL_ADDRESS); - return(0); - } - /* test the LPS25H (barometer) */ - val = SW_I2C_UTIL_Read(LPS25H_ADDRESS, LPS25H_WHO_AM_I); - if (val == LPS25H_WHO_ID) - { - debug_printf(" > [AltIMU-10v5] - LPS25H (Barometer) FOUND at I2C addr=0x%0x\r\n", LPS25H_ADDRESS); - } - else - { - debug_printf(" > [AltIMU-10v5] - Error probing for LPS25H (Barometer) at I2C addr=0x%0x\r\n", LPS25H_ADDRESS); - return(0); - } - // all tests passed - return(1); -} - -/** - * @brief Initialize IMU - * LSM6DS33 +/- 2g acceleration and 245 gps for gyro - */ -void ALTIMU10v5_Init(void) -{ - /*******************************/ - /* LSM6DS33 Gyro/Accelerometer */ - /*******************************/ - - // ACCLEROMETER - // 0x80 = 0b10000000 - // ODR = 1000 (1.66 kHz (high performance)); FS_XL = 00 (+/-2 g full scale) - SW_I2C_UTIL_WRITE(DS33_ADDRESS, DS33_CTRL1_XL, 0x80); - // GYRO - // 0x80 = 0b010000000 - // ODR = 1000 (1.66 kHz (high performance)); FS_XL = 00 (245 degree per s) - SW_I2C_UTIL_WRITE(DS33_ADDRESS, DS33_CTRL2_G, 0x80); - // ACCELEROMETER + GYRO - // 0x04 = 0b00000100 - // IF_INC = 1 (automatically increment register address) - SW_I2C_UTIL_WRITE(DS33_ADDRESS, DS33_CTRL3_C, 0x04); - - // MAGNETOMETER - // 0x70 = 0b01110000 - // OM = 11 (ultra-high-performance mode for X and Y); DO = 100 (10 Hz ODR) - SW_I2C_UTIL_WRITE(LIS3MDL_ADDRESS, LIS3MDL_CTRL_REG1, 0x70); - // 0x00 = 0b00000000 - // FS = 00 (+/- 4 gauss full scale) - SW_I2C_UTIL_WRITE(LIS3MDL_ADDRESS, LIS3MDL_CTRL_REG2, 0x00); - // 0x00 = 0b00000000 - // MD = 00 (continuous-conversion mode) - SW_I2C_UTIL_WRITE(LIS3MDL_ADDRESS, LIS3MDL_CTRL_REG3, 0x00); - // 0x0C = 0b00001100 - // OMZ = 11 (ultra-high-performance mode for Z) - SW_I2C_UTIL_WRITE(LIS3MDL_ADDRESS, LIS3MDL_CTRL_REG4, 0x0C); - - // BAROMETER - // 0xB0 = 0b10110000 - // PD = 1 (active mode); ODR = 011 (12.5 Hz pressure & temperature output data rate) - SW_I2C_UTIL_WRITE(LPS25H_ADDRESS, LPS25H_CTRL_REG1, 0xB0); -} - -/** - * @brief Reads the 3 accelerometer channels and stores them in *x,*y,*z - * units are m/s^2 - */ -void ALTIMU10v5_ReadAccelerometerRaw(float *x, float *y, float *z) -{ - uint8_t accel_xyz[6]; // 2 bytes each - - SW_I2C_UTIL_Read_Multi(DS33_ADDRESS, DS33_OUTX_L_XL, 6, (uint8_t*)&accel_xyz); - -/* - uint8_t i; - debug_printf("IMU_ReadAccelerometer Raw Bytes: "); - for (i=0;i<6;i++) - { - debug_printf("%02x ", accel_xyz[i]); - } - debug_printf("\r\n"); -*/ - *x = (int16_t)(accel_xyz[1] << 8 | accel_xyz[0]) * DS33_G_FACTOR * MS2_PER_G; - *y = (int16_t)(accel_xyz[3] << 8 | accel_xyz[2]) * DS33_G_FACTOR * MS2_PER_G; - *z = (int16_t)(accel_xyz[5] << 8 | accel_xyz[4]) * DS33_G_FACTOR * MS2_PER_G; -} - -/** - * @brief Reads the 3 gyro channels and stores them in *x,*y,*z - * units are rad/sec - */ -void ALTIMU10v5_ReadGyroRaw(float *x, float *y, float *z) -{ - uint8_t gyro_xyz[6]; // 2 bytes each - - SW_I2C_UTIL_Read_Multi(DS33_ADDRESS, DS33_OUTX_L_G, 6, (uint8_t*)&gyro_xyz); - - *x = (int16_t)(gyro_xyz[1] << 8 | gyro_xyz[0]) * DS33_DPS_FACTOR * RAD_PER_G; - *y = (int16_t)(gyro_xyz[3] << 8 | gyro_xyz[2]) * DS33_DPS_FACTOR * RAD_PER_G; - *z = (int16_t)(gyro_xyz[5] << 8 | gyro_xyz[4]) * DS33_DPS_FACTOR * RAD_PER_G; -} - -/** - * @brief Reads the raw barometer temp value - * (internal function only) - * @retval 16 bit raw temp value - */ -int16_t ALTIMU10v5_BarometerTempRaw(void) -{ - uint8_t temp[2]; - int16_t retval; // temp - - // assert MSB to enable register address auto-increment - SW_I2C_UTIL_Read_Multi(LPS25H_ADDRESS, LPS25H_TEMP_OUT_L | (1 << 7), 2, (uint8_t*)&temp); - - retval = (int16_t)(temp[1] << 8 | temp[0]); - return(retval); -} - -/** - * @brief Reads the raw barometer pressure value - * (internal function only) - * @retval 24 bit raw temp value - */ -static int32_t ALTIMU10v5_BarometerPressureRaw(void) -{ - uint8_t pressure[4]={0,0,0,0}; - int32_t retval; // pressure (24bit) - - // assert MSB to enable register address auto-increment - SW_I2C_UTIL_Read_Multi(LPS25H_ADDRESS, LPS25H_PRESS_OUT_XL | (1 << 7), 3, (uint8_t*)&pressure); - - retval = (int32_t)(pressure[2] << 16 | pressure[1] << 8 | pressure[0]); - return(retval); -} - -/** - * @brief Calculate millibar of pressure - * (internal function only) - * @retval pressure in millibar - */ -static float ALTIMU10v5_ReadBarometerPressureMilliBars(void) -{ - return (float)ALTIMU10v5_BarometerPressureRaw() / 4096; -} - -/** - * @brief Calculate temperature in C - * @retval temp in celsius - */ -float ALTIMU10v5_ReadBarometerTemperatureC(void) -{ - return 42.5 + (float)ALTIMU10v5_BarometerTempRaw() / 480; -} - - -/** - * @brief Calculate altitude in meters - * assumes sealevel pressure of 1013.25mbar - * @retval altitute in meters - */ -float ALTIMU10v5_ReadBarometerAltitudeMeters(void) -{ - return (1 - pow((float)ALTIMU10v5_ReadBarometerPressureMilliBars() / 1013.25, 0.190263)) * 44330.8; -} -#endif \ No newline at end of file diff --git a/stm32/ros_usbnode/src/imu/imu.c b/stm32/ros_usbnode/src/imu/imu.c index 433ab58..6e20982 100644 --- a/stm32/ros_usbnode/src/imu/imu.c +++ b/stm32/ros_usbnode/src/imu/imu.c @@ -15,7 +15,7 @@ #include #include "imu/imu.h" -#include "imu/altimu-10v5.h" +#include "imu/lsm6.h" #include "imu/mpu6050.h" #include "imu/wt901.h" #include "i2c.h" @@ -284,11 +284,11 @@ void IMU_Init() { imuReadAccelerometerRaw=NULL; imuReadGyroRaw=NULL; -#ifndef DISABLE_ALTIMU10v5 - if (ALTIMU10v5_TestDevice()) { - ALTIMU10v5_Init(); - imuReadAccelerometerRaw=ALTIMU10v5_ReadAccelerometerRaw; - imuReadGyroRaw=ALTIMU10v5_ReadGyroRaw; +#ifndef DISABLE_LSM6 + if (LSM6_TestDevice()) { + LSM6_Init(); + imuReadAccelerometerRaw=LSM6_ReadAccelerometerRaw; + imuReadGyroRaw=LSM6_ReadGyroRaw; } #endif diff --git a/stm32/ros_usbnode/src/imu/lsm6.c b/stm32/ros_usbnode/src/imu/lsm6.c new file mode 100644 index 0000000..67e430a --- /dev/null +++ b/stm32/ros_usbnode/src/imu/lsm6.c @@ -0,0 +1,142 @@ + +/** + ****************************************************************************** + * @file lsm6.c + * @author Georg Swoboda + * @brief Mowgli driver for the LSM6DS33/LSM633 IMU + ****************************************************************************** + * @attention + * + * Some code taken from the various Arduino drivers referenced at + * https://www.pololu.com/product/2739 + * + * This code supports both of the following gyro/accelerometer combos: + * LSM6DS33 + * LSM6DSO + ****************************************************************************** + */ + +#include "imu/imu.h" +#include "imu/lsm6.h" +#include "soft_i2c.h" +#include "main.h" +#include + +#ifndef DISABLE_LSM6 + +uint8_t lsm6_address = LSM6_SA0_LOW_ADDRESS; + + +/** + * @brief Test Device + * Perform any tests possible before actually enabling and using the device, + * for example check the i2c address and whoami registers if existing + * + * @param ctx read / write interface definitions + * @param val get the values of hpcf in reg CTRL_REG2 + * @retval 0 -> test failed 1-> test ok, good to init and use + * + */ +uint8_t LSM6_TestDevice(void) +{ + uint8_t val; + + /* test the SA0 high address*/ + val = SW_I2C_UTIL_Read(LSM6_SA0_LOW_ADDRESS, LSM6_WHO_AM_I); + if (val == DS33_WHO_ID) + { + debug_printf(" > [LSM6] - LSM6DS33 (Gyro / Accelerometer) FOUND at I2C addr=0x%0x\r\n", LSM6_SA0_LOW_ADDRESS); + lsm6_address = LSM6_SA0_LOW_ADDRESS; + return 1; + } else if (val == DSO_WHO_ID) + { + debug_printf(" > [LSM6] - LSM6DSO (Gyro / Accelerometer) FOUND at I2C addr=0x%0x\r\n", LSM6_SA0_LOW_ADDRESS); + lsm6_address = LSM6_SA0_LOW_ADDRESS; + return 1; + } + + /* test the SA0 high address*/ + val = SW_I2C_UTIL_Read(LSM6_SA0_HIGH_ADDRESS, LSM6_WHO_AM_I); + if (val == DS33_WHO_ID) + { + debug_printf(" > [LSM6] - LSM6DS33 (Gyro / Accelerometer) FOUND at I2C addr=0x%0x\r\n", LSM6_SA0_HIGH_ADDRESS); + lsm6_address = LSM6_SA0_HIGH_ADDRESS; + return 1; + } else if (val == DSO_WHO_ID) + { + debug_printf(" > [LSM6] - LSM6DSO (Gyro / Accelerometer) FOUND at I2C addr=0x%0x\r\n", LSM6_SA0_HIGH_ADDRESS); + lsm6_address = LSM6_SA0_HIGH_ADDRESS; + return 1; + } + + debug_printf(" > [LSM6] - Error probing for LSM6DS33/LSM6DSO (Gyro / Accelerometer) at I2C addr=0x%0x and addr=0x%0x \r\n", LSM6_SA0_LOW_ADDRESS, LSM6_SA0_HIGH_ADDRESS); + return(0); +} + +/** + * @brief Initialize IMU + * LSM6DS33/LSM6DSO +/- 2g acceleration and 245 gps for gyro + */ +void LSM6_Init(void) +{ + /*******************************/ + /* LSM6DS33 Gyro/Accelerometer */ + /*******************************/ + + // ACCLEROMETER + // 0x80 = 0b10000000 + // ODR = 1000 (1.66 kHz (high performance)); FS_XL = 00 (+/-2 g full scale) + SW_I2C_UTIL_WRITE(lsm6_address, LSM6_CTRL1_XL, 0x80); + // GYRO + // 0x80 = 0b010000000 + // ODR = 1000 (1.66 kHz (high performance)); FS_XL = 00 (245 degree per s) + SW_I2C_UTIL_WRITE(lsm6_address, LSM6_CTRL2_G, 0x80); + // ACCELEROMETER + GYRO + // 0x04 = 0b00000100 + // IF_INC = 1 (automatically increment register address) + SW_I2C_UTIL_WRITE(lsm6_address, LSM6_CTRL3_C, 0x04); +} + +/** + * @brief Reads the 3 accelerometer channels and stores them in *x,*y,*z + * units are m/s^2 + */ +void LSM6_ReadAccelerometerRaw(float *x, float *y, float *z) +{ + uint8_t accel_xyz[6]; // 2 bytes each + + uint8_t acked = SW_I2C_UTIL_Read_Multi(lsm6_address, LSM6_OUTX_L_XL, 6, (uint8_t*)&accel_xyz); +/* + uint8_t i; + debug_printf("IMU_ReadAccelerometer Raw Bytes: "); + for (i=0;i<6;i++) + { + debug_printf("%02x ", accel_xyz[i]); + } + debug_printf("\r\n"); +*/ + if(acked) { + *x = (int16_t)(accel_xyz[1] << 8 | accel_xyz[0]) * LSM6_G_FACTOR * MS2_PER_G; + *y = (int16_t)(accel_xyz[3] << 8 | accel_xyz[2]) * LSM6_G_FACTOR * MS2_PER_G; + *z = (int16_t)(accel_xyz[5] << 8 | accel_xyz[4]) * LSM6_G_FACTOR * MS2_PER_G; + } +} + +/** + * @brief Reads the 3 gyro channels and stores them in *x,*y,*z + * units are rad/sec + */ +void LSM6_ReadGyroRaw(float *x, float *y, float *z) +{ + uint8_t gyro_xyz[6]; // 2 bytes each + + uint8_t acked = SW_I2C_UTIL_Read_Multi(lsm6_address, LSM6_OUTX_L_G, 6, (uint8_t*)&gyro_xyz); + + if(acked) { + *x = (int16_t)(gyro_xyz[1] << 8 | gyro_xyz[0]) * LSM6_DPS_FACTOR * RAD_PER_G; + *y = (int16_t)(gyro_xyz[3] << 8 | gyro_xyz[2]) * LSM6_DPS_FACTOR * RAD_PER_G; + *z = (int16_t)(gyro_xyz[5] << 8 | gyro_xyz[4]) * LSM6_DPS_FACTOR * RAD_PER_G; + } +} + +#endif \ No newline at end of file