|
13 | 13 | #include "esp_check.h" |
14 | 14 | #include "esp_rom_sys.h" |
15 | 15 | #include "esp_timer.h" |
| 16 | +#include "iot_sensor_hub.h" |
16 | 17 | #include "icm42670.h" |
17 | 18 |
|
18 | 19 | #define I2C_CLK_SPEED 400000 |
@@ -482,3 +483,76 @@ esp_err_t icm42670_write_mreg_register(icm42670_handle_t sensor, uint8_t mreg, u |
482 | 483 |
|
483 | 484 | return ESP_OK; |
484 | 485 | } |
| 486 | + |
| 487 | +static icm42670_handle_t sensor_hub_icm42670_handle; |
| 488 | + |
| 489 | +esp_err_t icm42670_impl_init(bus_handle_t bus_handle, uint8_t addr) |
| 490 | +{ |
| 491 | + ESP_RETURN_ON_ERROR(icm42670_create(bus_handle, addr, &sensor_hub_icm42670_handle), TAG, |
| 492 | + "Failed to initialize ICM42670"); |
| 493 | + if (sensor_hub_icm42670_handle) { |
| 494 | + /* Configuration of the accelerometer and gyroscope */ |
| 495 | + const icm42670_cfg_t icm42670_cfg = { |
| 496 | + .acce_fs = ACCE_FS_2G, |
| 497 | + .acce_odr = ACCE_ODR_400HZ, |
| 498 | + .gyro_fs = GYRO_FS_2000DPS, |
| 499 | + .gyro_odr = GYRO_ODR_400HZ, |
| 500 | + }; |
| 501 | + ESP_RETURN_ON_ERROR(icm42670_config(sensor_hub_icm42670_handle, &icm42670_cfg), TAG, "Failed to initialize ICM42670"); |
| 502 | + |
| 503 | + /* Set accelerometer and gyroscope to ON */ |
| 504 | + ESP_RETURN_ON_ERROR(icm42670_acce_set_pwr(sensor_hub_icm42670_handle, ACCE_PWR_LOWNOISE), TAG, |
| 505 | + "Failed to set ICM42670 accelerometer power setting"); |
| 506 | + ESP_RETURN_ON_ERROR(icm42670_gyro_set_pwr(sensor_hub_icm42670_handle, GYRO_PWR_LOWNOISE), TAG, |
| 507 | + "Failed to set ICM42670 gyroscope power setting"); |
| 508 | + return ESP_OK; |
| 509 | + } |
| 510 | + return ESP_FAIL; |
| 511 | +} |
| 512 | + |
| 513 | +esp_err_t icm42670_impl_deinit(void) |
| 514 | +{ |
| 515 | + ESP_RETURN_ON_FALSE(sensor_hub_icm42670_handle != NULL, ESP_ERR_INVALID_STATE, TAG, |
| 516 | + "Failed to delete uninitialized ICM42670"); |
| 517 | + icm42670_delete(sensor_hub_icm42670_handle); |
| 518 | + sensor_hub_icm42670_handle = NULL; |
| 519 | + return ESP_OK; |
| 520 | +} |
| 521 | + |
| 522 | +esp_err_t icm42670_impl_test (void) |
| 523 | +{ |
| 524 | + uint8_t device_id; |
| 525 | + return icm42670_get_deviceid(sensor_hub_icm42670_handle, &device_id); |
| 526 | +} |
| 527 | + |
| 528 | +esp_err_t icm42670_impl_acquire_acce (float *acce_x, float *acce_y, float *acce_z) |
| 529 | +{ |
| 530 | + icm42670_value_t value; |
| 531 | + ESP_RETURN_ON_ERROR(icm42670_get_acce_value(sensor_hub_icm42670_handle, &value), TAG, |
| 532 | + "Failed to read acceleration data"); |
| 533 | + *acce_x = value.x; |
| 534 | + *acce_y = value.y; |
| 535 | + *acce_z = value.z; |
| 536 | + return ESP_OK; |
| 537 | +} |
| 538 | + |
| 539 | +esp_err_t icm42670_impl_acquire_gyro(float *gyro_x, float *gyro_y, float *gyro_z) |
| 540 | +{ |
| 541 | + icm42670_value_t value; |
| 542 | + ESP_RETURN_ON_ERROR(icm42670_get_gyro_value(sensor_hub_icm42670_handle, &value), TAG, |
| 543 | + "Failed to read acceleration data"); |
| 544 | + *gyro_x = value.x; |
| 545 | + *gyro_y = value.y; |
| 546 | + *gyro_z = value.z; |
| 547 | + return ESP_OK; |
| 548 | +} |
| 549 | + |
| 550 | +static imu_impl_t icm42670_impl = { |
| 551 | + .init = icm42670_impl_init, |
| 552 | + .deinit = icm42670_impl_deinit, |
| 553 | + .test = icm42670_impl_test, |
| 554 | + .acquire_acce = icm42670_impl_acquire_acce, |
| 555 | + .acquire_gyro = icm42670_impl_acquire_gyro, |
| 556 | +}; |
| 557 | + |
| 558 | +SENSOR_HUB_DETECT_FN(IMU_ID, sensor_hub_icm42670, &icm42670_impl); |
0 commit comments