3535#define ACCEL_SENSITIVITY_16G 2048 // Currently in Primary Use
3636
3737ICM42688::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;
4141}
4242
4343void 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,
5151 // Initialize values to 0
5252 memset (dummy_tx, 0 , num_bytes_to_retrieve * sizeof (dummy_tx[0 ]));
5353
54- HAL_GPIO_WritePin (CS_GPIO_PORT, CS_PIN , GPIO_PIN_RESET);
54+ HAL_GPIO_WritePin (csGpioPort_, csPin_ , GPIO_PIN_RESET);
5555
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);
5858
59- HAL_GPIO_WritePin (CS_GPIO_PORT, CS_PIN , GPIO_PIN_SET);
59+ HAL_GPIO_WritePin (csGpioPort_, csPin_ , GPIO_PIN_SET);
6060}
6161
6262void ICM42688::writeRegister (uint8_t sub_address, uint8_t data_to_imu) {
6363 // Prepare transmit buffer
6464 uint8_t tx_buf[2 ] = {sub_address, data_to_imu};
6565
66- HAL_GPIO_WritePin (CS_GPIO_PORT, CS_PIN , GPIO_PIN_RESET);
66+ HAL_GPIO_WritePin (csGpioPort_, csPin_ , GPIO_PIN_RESET);
6767
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);
6969
70- HAL_GPIO_WritePin (CS_GPIO_PORT, CS_PIN , GPIO_PIN_SET);
70+ HAL_GPIO_WritePin (csGpioPort_, csPin_ , GPIO_PIN_SET);
7171}
7272
7373void ICM42688::beginMeasuring () {
74- HAL_GPIO_WritePin (CS_GPIO_PORT, CS_PIN , GPIO_PIN_SET);
74+ HAL_GPIO_WritePin (csGpioPort_, csPin_ , GPIO_PIN_SET);
7575 reset ();
7676 setLowNoiseMode ();
7777 calibrate ();
@@ -104,51 +104,51 @@ void ICM42688::setGyroFS(uint8_t fssel) {
104104
105105 writeRegister (0x4F , reg);
106106
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;
109109}
110110
111111void ICM42688::calibrate () {
112112 // Set at a lower range (more resolution since IMU not moving)
113- const uint8_t current_fssel = gyroFS ;
113+ const uint8_t currentFSSel_ = gyroFS_ ;
114114 setGyroFS (GYRO_SENSITIVITY_250DPS); // Set to 250 dps
115115
116116 // 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 ;
120120
121121 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_ );
123123
124124 // Combine raw bytes into 16-bit values
125125 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 ];
127127 }
128128
129129 // Process gyro data
130130 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;
132132 }
133133
134134 /*
135135 Calculate bias by collecting samples and considering pre-existing bias
136136 For each iteration, add the existing bias with the new bias and divide by the sample size
137137 to get an average bias over a specified number of gyro samples
138138 */
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;
142142
143143 HAL_Delay (1 );
144144 }
145145
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 ];
149149
150150 // Recover the full scale setting
151- setGyroFS (current_fssel );
151+ setGyroFS (currentFSSel_ );
152152}
153153
154154IMUData_t ICM42688::getResult (uint8_t * data_buffer) {
@@ -157,18 +157,18 @@ IMUData_t ICM42688::getResult(uint8_t * data_buffer) {
157157
158158 // Formatting raw data
159159 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 ];
161161 }
162162
163163 IMUData_t IMUData {};
164164
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;
168168
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;
172172
173173 return IMUData;
174174}
0 commit comments