1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
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;
};
|
