Skip to content

Commit 57286f0

Browse files
benjibstbenjibst
committed
format and rebase
1 parent b333143 commit 57286f0

File tree

1 file changed

+37
-46
lines changed

1 file changed

+37
-46
lines changed

‎components/sensors/icm42670/icm42670.c‎

Lines changed: 37 additions & 46 deletions
Original file line numberDiff line numberDiff line change
@@ -19,40 +19,39 @@
1919
#define ICM42670_I2C_CLK_SPEED 400000
2020
#define ICM42670_SPI_CLK_SPEED 1000000
2121

22-
#define ALPHA 0.97f /*!< Weight of gyroscope */
23-
#define RAD_TO_DEG 57.27272727f /*!< Radians to degrees */
22+
#define ALPHA 0.97f /*!< Weight of gyroscope */
23+
#define RAD_TO_DEG 57.27272727f /*!< Radians to degrees */
2424

2525
#define ICM42607_ID 0x60
2626
#define ICM42670_ID 0x67
2727

2828
/* ICM42670 register */
29-
#define ICM42670_WHOAMI 0x75
30-
#define ICM42670_GYRO_CONFIG0 0x20
31-
#define ICM42670_ACCEL_CONFIG0 0x21
32-
#define ICM42670_TEMP_CONFIG 0x22
33-
#define ICM42670_PWR_MGMT0 0x1F
34-
#define ICM42670_TEMP_DATA 0x09
35-
#define ICM42670_ACCEL_DATA 0x0B
36-
#define ICM42670_GYRO_DATA 0x11
29+
#define ICM42670_WHOAMI 0x75
30+
#define ICM42670_GYRO_CONFIG0 0x20
31+
#define ICM42670_ACCEL_CONFIG0 0x21
32+
#define ICM42670_TEMP_CONFIG 0x22
33+
#define ICM42670_PWR_MGMT0 0x1F
34+
#define ICM42670_TEMP_DATA 0x09
35+
#define ICM42670_ACCEL_DATA 0x0B
36+
#define ICM42670_GYRO_DATA 0x11
3737

3838
/* Sensitivity of the gyroscope */
3939
#define GYRO_FS_2000_SENSITIVITY (16.4)
4040
#define GYRO_FS_1000_SENSITIVITY (32.8)
41-
#define GYRO_FS_500_SENSITIVITY (65.5)
42-
#define GYRO_FS_250_SENSITIVITY (131.0)
41+
#define GYRO_FS_500_SENSITIVITY (65.5)
42+
#define GYRO_FS_250_SENSITIVITY (131.0)
4343

4444
/* Sensitivity of the accelerometer */
4545
#define ACCE_FS_16G_SENSITIVITY (2048)
46-
#define ACCE_FS_8G_SENSITIVITY (4096)
47-
#define ACCE_FS_4G_SENSITIVITY (8192)
48-
#define ACCE_FS_2G_SENSITIVITY (16384)
46+
#define ACCE_FS_8G_SENSITIVITY (4096)
47+
#define ACCE_FS_4G_SENSITIVITY (8192)
48+
#define ACCE_FS_2G_SENSITIVITY (16384)
4949

5050
/*******************************************************************************
5151
* Types definitions
5252
*******************************************************************************/
5353

54-
typedef struct
55-
{
54+
typedef struct {
5655
bool using_spi;
5756
spi_device_handle_t spi_handle;
5857
i2c_master_dev_handle_t i2c_handle;
@@ -136,12 +135,9 @@ esp_err_t icm42670_create_spi(spi_host_device_t spi_bus, gpio_num_t cs_pin, icm4
136135
void icm42670_delete(icm42670_handle_t sensor)
137136
{
138137
icm42670_dev_t *sens = (icm42670_dev_t *)sensor;
139-
if (sens->using_spi && sens->spi_handle)
140-
{
138+
if (sens->using_spi && sens->spi_handle) {
141139
spi_bus_remove_device(sens->spi_handle);
142-
}
143-
else if (sens->i2c_handle)
144-
{
140+
} else if (sens->i2c_handle) {
145141
i2c_master_bus_rm_device(sens->i2c_handle);
146142
}
147143

@@ -369,12 +365,10 @@ static esp_err_t check_device_present(icm42670_handle_t sensor)
369365
{
370366
uint8_t dev_id = 0;
371367
esp_err_t ret = icm42670_get_deviceid(sensor, &dev_id);
372-
if (ret != ESP_OK)
373-
{
368+
if (ret != ESP_OK) {
374369
return ret;
375370
}
376-
if (dev_id != ICM42607_ID && dev_id != ICM42670_ID)
377-
{
371+
if (dev_id != ICM42607_ID && dev_id != ICM42670_ID) {
378372
ESP_LOGD(TAG, "Incorrect Device ID (0x%02x).", dev_id);
379373
return ESP_ERR_NOT_FOUND;
380374
}
@@ -405,22 +399,19 @@ static esp_err_t icm42670_get_raw_value(icm42670_handle_t sensor, uint8_t reg, i
405399
static esp_err_t icm42670_write(icm42670_handle_t sensor, const uint8_t reg_start_addr, const uint8_t *data_buf,
406400
const uint8_t data_len)
407401
{
408-
icm42670_dev_t *sens = (icm42670_dev_t *) sensor;
402+
icm42670_dev_t *sens = (icm42670_dev_t *)sensor;
409403
assert(sens);
410404

411405
assert(data_len < 5);
412406
uint8_t write_buff[5] = {reg_start_addr};
413407
memcpy(&write_buff[1], data_buf, data_len);
414-
if (sens->using_spi)
415-
{
408+
if (sens->using_spi) {
416409
spi_transaction_t trans = {
417410
.length = (data_len + 1) * 8, // Total length in bits
418411
.tx_buffer = write_buff,
419412
};
420413
return spi_device_transmit(sens->spi_handle, &trans);
421-
}
422-
else
423-
{
414+
} else {
424415
return i2c_master_transmit(sens->i2c_handle, write_buff, data_len + 1, -1);
425416
}
426417
}
@@ -430,8 +421,7 @@ static esp_err_t icm42670_read(icm42670_handle_t sensor, const uint8_t reg_start
430421
icm42670_dev_t *sens = (icm42670_dev_t *)sensor;
431422
assert(sens && data_buf && data_len <= 6);
432423
esp_err_t ret;
433-
if (sens->using_spi)
434-
{
424+
if (sens->using_spi) {
435425
uint8_t tx_buf[7] = {reg_start_addr | 0x80}; // Set MSB for read operation
436426
uint8_t rx_buf[7] = {0}; // Buffer to receive data (including the register address)
437427
spi_transaction_t trans = {
@@ -440,13 +430,10 @@ static esp_err_t icm42670_read(icm42670_handle_t sensor, const uint8_t reg_start
440430
.rx_buffer = rx_buf,
441431
};
442432
ret = spi_device_transmit(sens->spi_handle, &trans);
443-
if (ret == ESP_OK)
444-
{
433+
if (ret == ESP_OK) {
445434
memcpy(data_buf, &rx_buf[1], data_len); // Skip the first byte (register address)
446435
}
447-
}
448-
else
449-
{
436+
} else {
450437
uint8_t reg_buff[] = {reg_start_addr};
451438
/* Write register number and read data */
452439
ret = i2c_master_transmit_receive(sens->i2c_handle, reg_buff, sizeof(reg_buff), data_buf, data_len, -1);
@@ -457,7 +444,7 @@ static esp_err_t icm42670_read(icm42670_handle_t sensor, const uint8_t reg_start
457444
esp_err_t icm42670_complimentory_filter(icm42670_handle_t sensor, const icm42670_value_t *const acce_value,
458445
const icm42670_value_t *const gyro_value, complimentary_angle_t *const complimentary_angle)
459446
{
460-
icm42670_dev_t *sens = (icm42670_dev_t *) sensor;
447+
icm42670_dev_t *sens = (icm42670_dev_t *)sensor;
461448
float measurement_delta;
462449
uint64_t current_time_us;
463450
float acc_roll_angle;
@@ -466,9 +453,11 @@ esp_err_t icm42670_complimentory_filter(icm42670_handle_t sensor, const icm42670
466453
float gyro_pitch_angle;
467454

468455
acc_roll_angle = (atan2(acce_value->y,
469-
sqrt(acce_value->x * acce_value->x + acce_value->z * acce_value->z)) * RAD_TO_DEG);
456+
sqrt(acce_value->x * acce_value->x + acce_value->z * acce_value->z)) *
457+
RAD_TO_DEG);
470458
acc_pitch_angle = (atan2(-acce_value->x,
471-
sqrt(acce_value->y * acce_value->y + acce_value->z * acce_value->z)) * RAD_TO_DEG);
459+
sqrt(acce_value->y * acce_value->y + acce_value->z * acce_value->z)) *
460+
RAD_TO_DEG);
472461

473462
if (!sens->initialized_filter) {
474463
sens->initialized_filter = true;
@@ -485,9 +474,11 @@ esp_err_t icm42670_complimentory_filter(icm42670_handle_t sensor, const icm42670
485474
gyro_pitch_angle = gyro_value->y * measurement_delta;
486475

487476
complimentary_angle->roll = (ALPHA * (sens->previous_measurement.roll + gyro_roll_angle)) + ((
488-
1 - ALPHA) * acc_roll_angle);
477+
1 - ALPHA) *
478+
acc_roll_angle);
489479
complimentary_angle->pitch = (ALPHA * (sens->previous_measurement.pitch + gyro_pitch_angle)) + ((
490-
1 - ALPHA) * acc_pitch_angle);
480+
1 - ALPHA) *
481+
acc_pitch_angle);
491482

492483
sens->previous_measurement.roll = complimentary_angle->roll;
493484
sens->previous_measurement.pitch = complimentary_angle->pitch;
@@ -593,13 +584,13 @@ esp_err_t icm42670_impl_deinit(void)
593584
return ESP_OK;
594585
}
595586

596-
esp_err_t icm42670_impl_test (void)
587+
esp_err_t icm42670_impl_test(void)
597588
{
598589
uint8_t device_id;
599590
return icm42670_get_deviceid(sensor_hub_icm42670_handle, &device_id);
600591
}
601592

602-
esp_err_t icm42670_impl_acquire_acce (float *acce_x, float *acce_y, float *acce_z)
593+
esp_err_t icm42670_impl_acquire_acce(float *acce_x, float *acce_y, float *acce_z)
603594
{
604595
icm42670_value_t value;
605596
ESP_RETURN_ON_ERROR(icm42670_get_acce_value(sensor_hub_icm42670_handle, &value), TAG,

0 commit comments

Comments
 (0)
⚡