35
35
#define ACCEL_SENSITIVITY_16G 2048 // Currently in Primary Use
36
36
37
37
ICM42688::ICM42688 (SPI_HandleTypeDef * spi_handle, GPIO_TypeDef * cs_gpio_port, uint16_t cs_pin) {
38
- SPI_HANDLE = spi_handle;
39
- CS_GPIO_PORT = cs_gpio_port;
40
- CS_PIN = cs_pin;
38
+ spiHandle_ = spi_handle;
39
+ csGpioPort_ = cs_gpio_port;
40
+ csPin_ = cs_pin;
41
41
}
42
42
43
43
void ICM42688::readRegister (uint8_t sub_address, uint8_t num_bytes_to_retrieve, uint8_t * destination) {
@@ -51,27 +51,27 @@ void ICM42688::readRegister(uint8_t sub_address, uint8_t num_bytes_to_retrieve,
51
51
// Initialize values to 0
52
52
memset (dummy_tx, 0 , num_bytes_to_retrieve * sizeof (dummy_tx[0 ]));
53
53
54
- HAL_GPIO_WritePin (CS_GPIO_PORT, CS_PIN , GPIO_PIN_RESET);
54
+ HAL_GPIO_WritePin (csGpioPort_, csPin_ , GPIO_PIN_RESET);
55
55
56
- HAL_SPI_TransmitReceive (SPI_HANDLE , &tx, &dummy_rx, 1 , HAL_MAX_DELAY);
57
- HAL_SPI_TransmitReceive (SPI_HANDLE , dummy_tx, destination, num_bytes_to_retrieve, HAL_MAX_DELAY);
56
+ HAL_SPI_TransmitReceive (spiHandle_ , &tx, &dummy_rx, 1 , HAL_MAX_DELAY);
57
+ HAL_SPI_TransmitReceive (spiHandle_ , dummy_tx, destination, num_bytes_to_retrieve, HAL_MAX_DELAY);
58
58
59
- HAL_GPIO_WritePin (CS_GPIO_PORT, CS_PIN , GPIO_PIN_SET);
59
+ HAL_GPIO_WritePin (csGpioPort_, csPin_ , GPIO_PIN_SET);
60
60
}
61
61
62
62
void ICM42688::writeRegister (uint8_t sub_address, uint8_t data_to_imu) {
63
63
// Prepare transmit buffer
64
64
uint8_t tx_buf[2 ] = {sub_address, data_to_imu};
65
65
66
- HAL_GPIO_WritePin (CS_GPIO_PORT, CS_PIN , GPIO_PIN_RESET);
66
+ HAL_GPIO_WritePin (csGpioPort_, csPin_ , GPIO_PIN_RESET);
67
67
68
- HAL_SPI_Transmit (SPI_HANDLE , tx_buf, sizeof (tx_buf), HAL_MAX_DELAY);
68
+ HAL_SPI_Transmit (spiHandle_ , tx_buf, sizeof (tx_buf), HAL_MAX_DELAY);
69
69
70
- HAL_GPIO_WritePin (CS_GPIO_PORT, CS_PIN , GPIO_PIN_SET);
70
+ HAL_GPIO_WritePin (csGpioPort_, csPin_ , GPIO_PIN_SET);
71
71
}
72
72
73
73
void ICM42688::beginMeasuring () {
74
- HAL_GPIO_WritePin (CS_GPIO_PORT, CS_PIN , GPIO_PIN_SET);
74
+ HAL_GPIO_WritePin (csGpioPort_, csPin_ , GPIO_PIN_SET);
75
75
reset ();
76
76
setLowNoiseMode ();
77
77
calibrate ();
@@ -104,51 +104,51 @@ void ICM42688::setGyroFS(uint8_t fssel) {
104
104
105
105
writeRegister (0x4F , reg);
106
106
107
- gyro_scale = (2000 .0f / (float )(1 << fssel)) / 32768 .0f ;
108
- gyroFS = fssel;
107
+ gyroScale_ = (2000 .0f / (float )(1 << fssel)) / 32768 .0f ;
108
+ gyroFS_ = fssel;
109
109
}
110
110
111
111
void ICM42688::calibrate () {
112
112
// Set at a lower range (more resolution since IMU not moving)
113
- const uint8_t current_fssel = gyroFS ;
113
+ const uint8_t currentFSSel_ = gyroFS_ ;
114
114
setGyroFS (GYRO_SENSITIVITY_250DPS); // Set to 250 dps
115
115
116
116
// Take samples and find bias
117
- gyroBD [0 ] = 0 ;
118
- gyroBD [1 ] = 0 ;
119
- gyroBD [2 ] = 0 ;
117
+ gyroBD_ [0 ] = 0 ;
118
+ gyroBD_ [1 ] = 0 ;
119
+ gyroBD_ [2 ] = 0 ;
120
120
121
121
for (size_t i = 0 ; i < NUM_GYRO_SAMPLES; i++) {
122
- readRegister (UB0_REG_TEMP_DATA1, 14 , gyro_buffer );
122
+ readRegister (UB0_REG_TEMP_DATA1, 14 , gyro_buffer_ );
123
123
124
124
// Combine raw bytes into 16-bit values
125
125
for (size_t j = 0 ; j < 7 ; j++) {
126
- raw_meas_gyro [j] = ((int16_t )gyro_buffer [j * 2 ] << 8 ) | gyro_buffer [j * 2 + 1 ];
126
+ raw_meas_gyro_ [j] = ((int16_t )gyro_buffer_ [j * 2 ] << 8 ) | gyro_buffer_ [j * 2 + 1 ];
127
127
}
128
128
129
129
// Process gyro data
130
130
for (size_t k = 0 ; k < 3 ; k++) {
131
- gyr [k] = (float )raw_meas_gyro [k + 3 ] / GYRO_SENSITIVITY_250DPS;
131
+ gyr_ [k] = (float )raw_meas_gyro_ [k + 3 ] / GYRO_SENSITIVITY_250DPS;
132
132
}
133
133
134
134
/*
135
135
Calculate bias by collecting samples and considering pre-existing bias
136
136
For each iteration, add the existing bias with the new bias and divide by the sample size
137
137
to get an average bias over a specified number of gyro samples
138
138
*/
139
- gyroBD [0 ] += (gyr [0 ] + gyrB [0 ]) / NUM_GYRO_SAMPLES;
140
- gyroBD [1 ] += (gyr [1 ] + gyrB [1 ]) / NUM_GYRO_SAMPLES;
141
- gyroBD [2 ] += (gyr [2 ] + gyrB [2 ]) / NUM_GYRO_SAMPLES;
139
+ gyroBD_ [0 ] += (gyr_ [0 ] + gyrB_ [0 ]) / NUM_GYRO_SAMPLES;
140
+ gyroBD_ [1 ] += (gyr_ [1 ] + gyrB_ [1 ]) / NUM_GYRO_SAMPLES;
141
+ gyroBD_ [2 ] += (gyr_ [2 ] + gyrB_ [2 ]) / NUM_GYRO_SAMPLES;
142
142
143
143
HAL_Delay (1 );
144
144
}
145
145
146
- gyrB [0 ] = gyroBD [0 ];
147
- gyrB [1 ] = gyroBD [1 ];
148
- gyrB [2 ] = gyroBD [2 ];
146
+ gyrB_ [0 ] = gyroBD_ [0 ];
147
+ gyrB_ [1 ] = gyroBD_ [1 ];
148
+ gyrB_ [2 ] = gyroBD_ [2 ];
149
149
150
150
// Recover the full scale setting
151
- setGyroFS (current_fssel );
151
+ setGyroFS (currentFSSel_ );
152
152
}
153
153
154
154
IMUData_t ICM42688::getResult (uint8_t * data_buffer) {
@@ -157,18 +157,18 @@ IMUData_t ICM42688::getResult(uint8_t * data_buffer) {
157
157
158
158
// Formatting raw data
159
159
for (size_t i = 0 ; i < 3 ; i++) {
160
- raw_meas [i] = ((int16_t )data_buffer[i * 2 ] << 8 ) | data_buffer[i * 2 + 1 ];
160
+ raw_meas_ [i] = ((int16_t )data_buffer[i * 2 ] << 8 ) | data_buffer[i * 2 + 1 ];
161
161
}
162
162
163
163
IMUData_t IMUData {};
164
164
165
- IMUData.accx = (float )raw_meas [1 ] / ACCEL_SENSITIVITY_16G;
166
- IMUData.accy = (float )raw_meas [2 ] / ACCEL_SENSITIVITY_16G;
167
- IMUData.accz = (float )raw_meas [3 ] / ACCEL_SENSITIVITY_16G;
165
+ IMUData.accx = (float )raw_meas_ [1 ] / ACCEL_SENSITIVITY_16G;
166
+ IMUData.accy = (float )raw_meas_ [2 ] / ACCEL_SENSITIVITY_16G;
167
+ IMUData.accz = (float )raw_meas_ [3 ] / ACCEL_SENSITIVITY_16G;
168
168
169
- IMUData.gyrx = (float )raw_meas [4 ] / GYRO_SENSITIVITY_2000DPS;
170
- IMUData.gyry = (float )raw_meas [5 ] / GYRO_SENSITIVITY_2000DPS;
171
- IMUData.gyrz = (float )raw_meas [6 ] / GYRO_SENSITIVITY_2000DPS;
169
+ IMUData.gyrx = (float )raw_meas_ [4 ] / GYRO_SENSITIVITY_2000DPS;
170
+ IMUData.gyry = (float )raw_meas_ [5 ] / GYRO_SENSITIVITY_2000DPS;
171
+ IMUData.gyrz = (float )raw_meas_ [6 ] / GYRO_SENSITIVITY_2000DPS;
172
172
173
173
return IMUData;
174
174
}
0 commit comments