diff options
Diffstat (limited to 'include/iim42653.hpp')
| -rw-r--r-- | include/iim42653.hpp | 177 |
1 files changed, 177 insertions, 0 deletions
diff --git a/include/iim42653.hpp b/include/iim42653.hpp new file mode 100644 index 0000000..ac64edc --- /dev/null +++ b/include/iim42653.hpp @@ -0,0 +1,177 @@ +#pragma once + +#include "serial.hpp" +#include <stdint.h> + +#include <hardware/i2c.h> +#include <hardware/clocks.h> + +#if (USE_FREERTOS == 1) +#include "FreeRTOS.h" +#include "FreeRTOSConfig.h" +#include "portmacro.h" +#include "projdefs.h" +#include "serial.hpp" +#include "task.h" +#include "semphr.h" +#endif + +#define IIM42653_I2C_ADDR 0x68 +#define IIM42653_CLOCK_SOURCE_SYSTEM 0x6 //Use system clock as source +#define IIM42653_CLOCK_DIVISOR 3125 //125MHz --> 40kHz + +// DEVICE_CONFIG [State Configuration, SPI mode and Reset] +#define R_IIM42653_DEVICE_CONFIG 0x11 +typedef union { + struct { + bool SOFT_RESET_CONFIG :1; //Enable/disable soft reset + uint8_t RESERVED :3; //Reserved + bool SPI_MODE :1; //SPI Mode (internal), 0/3 or 1/2 + uint8_t RESERVED_ :3; //Reserved + } fields; + uint8_t data; +} IIM42653_DEVICE_CONFIG; +#define b_IIM42653_DEVICE_CONFIG_ENABLE_SOFT_RESET 0b1 +#define b_IIM42653_DEVICE_CONFIG_DISABLE_SOFT_RESET 0b0 + +// DRIVE_CONFIG [Configuration, SPI/I2C Speed] +#define R_IIM42653_DRIVE_CONFIG 0x13 +typedef union { + struct { + uint8_t SPI_SLEW_RATE :3; //SPI slew rate on Pin 14 + uint8_t I2C_SLEW_RATE :3; //I2C slew rate on Pin 14 + uint8_t RESERVED :2; //Reserved + } fields; + uint8_t data; +} IIM42653_DRIVE_CONFIG; +#define B_IIM42653_DRIVE_CONFIG_SLEW_20_TO_60_nS 0x00 +#define B_IIM42653_DRIVE_CONFIG_SLEW_2_TO_6_nS 0x20 + +// TEMP_DATA1_UI - TEMP_DATA0_UI [Output, Temperature Sensor] +#define R_IIM42653_TEMP_DATA1_UI 0x1D +#define R_IIM42653_TEMP_DATA0_UI 0x1E + +// ACCEL_DATA_X1_UI - GYRO_DATA_Z0_UI [Output, 3-Axis Gyro and Accel Data] +#define R_IIM42653_ACCEL_DATA_X1_UI 0x1F +#define R_IIM42653_ACCEL_DATA_X0_UI 0x20 +#define R_IIM42653_ACCEL_DATA_Y1_UI 0x21 +#define R_IIM42653_ACCEL_DATA_Y0_UI 0x22 +#define R_IIM42653_ACCEL_DATA_Z1_UI 0x23 +#define R_IIM42653_ACCEL_DATA_Z0_UI 0x24 +#define R_IIM42653_GYRO_DATA_X1_UI 0x25 +#define R_IIM42653_GYRO_DATA_X0_UI 0x26 +#define R_IIM42653_GYRO_DATA_Y1_UI 0x27 +#define R_IIM42653_GYRO_DATA_Y0_UI 0x28 +#define R_IIM42653_GYRO_DATA_Z1_UI 0x29 +#define R_IIM42653_GYRO_DATA_Z0_UI 0x2A +#define S_IIM42653_ACCEL_SENSITIVITY_FACTOR 1024.0f +#define S_IIM42653_GYRO_SENSITIVITY_FACTOR 8.2f + +// PWR_MGMT0 [State Configuration, Sensor enabling] +#define R_IIM42653_PWR_MGMT0 0x4E +typedef union { + struct { + uint8_t ACCEL_MODE :2; //Accelerometer power mode + uint8_t GYRO_MODE :2; //Gyroscope power mode + bool IDLE :1; //Run RC oscillator even if Accel/Gyro are off + bool TEMP_DIS :1; //Disable the temperature sensor + bool S4S_ENABLE :1; //Enable synchronous timing control (???) with custom command + bool RESERVED :1; //Reserved + } fields; + uint8_t data; +} IIM42653_PWR_MGMT0; +#define B_IIM42653_PWR_MGMT0_ACCEL_MODE_OFF 0x00 +#define B_IIM42653_PWR_MGMT0_ACCEL_MODE_LOW_NOISE 0x03 +#define B_IIM42653_PWR_MGMT0_GYRO_MODE_OFF 0x00 +#define B_IIM42653_PWR_MGMT0_GYRO_MODE_LOW_NOISE 0x03 +#define B_IIM42653_PWR_MGMT0_TEMP_DISABLE 0b1 +#define B_IIM42653_PWR_MGMT0_TEMP_ENABLE 0b0 + +// GYRO_CONFIG0 [Configuration, Sensor FSR/ODR] +#define R_IIM42653_GYRO_CONFIG0 0x4F +typedef union { + struct { + uint8_t GYRO_ODR :4; //ODR, see datasheet for range + bool RESERVED :1; //Reserved + uint8_t GYRO_UI_FS_SEL :3; //FSR, see datasheet for range + } fields; + uint8_t data; +} IIM42653_GYRO_CONFIG0; +#define B_IIM42653_GYRO_CONFIG0_ODR_100HZ 0x08 +#define B_IIM42653_GYRO_CONFIG0_ODR_200HZ 0x07 +#define B_IIM42653_GYRO_CONFIG0_ODR_500HZ 0x0F +#define B_IIM42653_GYRO_CONFIG0_FSR_4000DPS 0x00 +#define B_IIM42653_GYRO_CONFIG0_FSR_250DPS 0x04 + +// ACCEL_CONFIG0 [Configuration, Sensor FSR/ODR] +#define R_IIM42653_ACCEL_CONFIG0 0x50 +typedef union { + struct { + uint8_t ACCEL_ODR :4; //ODR, see datasheet for range + bool RESERVED :1; //Reserved + uint8_t ACCEL_UI_FS_SEL :3; //FSR, see datasheet for range + } fields; + uint8_t data; +} IIM42653_ACCEL_CONFIG0; +#define B_IIM42653_ACCEL_CONFIG0_ODR_100HZ 0x08 +#define B_IIM42653_ACCEL_CONFIG0_ODR_200HZ 0x07 +#define B_IIM42653_ACCEL_CONFIG0_ODR_500HZ 0x0F +#define B_IIM42653_ACCEL_CONFIG0_FSR_32G 0x00 +#define B_IIM42653_ACCEL_CONFIG_FSR_4G 0x03 + +// WHO_AM_I [Validation] +#define R_IIM42653_WHO_AM_I 0x75 +#define B_IIM42653_WHO_AM_I_VALUE 0x56 + +#define IIM42653_SAMPLE_RATE_HZ 500 + +class IIM42653 { + public: + IIM42653(i2c_inst_t* i2c) : i2c {i2c} {}; + + void initialize(); + + void calibrate_gyro(); + + void sample(); + + void apply_gyro_offset(); + + int16_t get_ax() { return ax; } + int16_t get_ay() { return ay; } + int16_t get_az() { return az; } + int16_t get_gx() { return gx; } + int16_t get_gy() { return gy; } + int16_t get_gz() { return gz; } + + static float scale_accel(int16_t unscaled) { return ((float) unscaled) / S_IIM42653_ACCEL_SENSITIVITY_FACTOR; } + static float scale_gyro(int16_t unscaled) { return ((float) unscaled) / S_IIM42653_GYRO_SENSITIVITY_FACTOR; } + +#if ( USE_FREERTOS == 1 ) + static void update_iim42653_task(void* pvParameters); + + TaskHandle_t update_task_handle = NULL; +#endif + + private: + static int16_t sat_sub(int16_t a, int16_t b); + + const uint8_t addr = IIM42653_I2C_ADDR; + + i2c_inst_t* i2c; + + uint8_t buffer[16]; + + IIM42653_DEVICE_CONFIG device_config; + IIM42653_DRIVE_CONFIG drive_config; + IIM42653_PWR_MGMT0 pwr_mgmt0; + IIM42653_GYRO_CONFIG0 gyro_config0; + IIM42653_ACCEL_CONFIG0 accel_config0; + + //Internal data fields + int16_t bias_gx = 0, bias_gy = 0, bias_gz = 0; + int16_t raw_gx = 0, raw_gy = 0, raw_gz = 0; + int16_t ax, ay, az, gx, gy, gz; + + const int64_t n_gyro_bias_readings = 50; +}; |