diff --git a/air_mouse/air_mouse_app.c b/air_mouse/air_mouse_app.c new file mode 100644 index 00000000..f0f90dcd --- /dev/null +++ b/air_mouse/air_mouse_app.c @@ -0,0 +1,129 @@ +#include +#include +#include +#include +#include "imu_mouse.h" +#include "air_mouse_icons.h" + +#define TAG "SensorModule" + +typedef struct { + Gui* gui; + ViewPort* view_port; + FuriMessageQueue* input_queue; + + FuriHalSpiBusHandle* icm42688p_device; + ICM42688P* icm42688p; + bool icm42688p_valid; + + ImuThread* imu_thread; +} SensorModuleApp; + +static void render_callback(Canvas* canvas, void* ctx) { + UNUSED(ctx); + canvas_clear(canvas); + canvas_set_color(canvas, ColorBlack); + + canvas_draw_icon(canvas, 64 + 14, 8, &I_Circles_47x47); + canvas_draw_icon(canvas, 83 + 14, 27, &I_Left_mouse_icon_9x9); + canvas_draw_icon(canvas, 83 + 14, 11, &I_Right_mouse_icon_9x9); + + canvas_set_font(canvas, FontPrimary); + canvas_draw_str(canvas, 0, 14, "Air Mouse"); + canvas_set_font(canvas, FontSecondary); + canvas_draw_str(canvas, 0, 56, "Press Back to exit"); +} + +static void input_callback(InputEvent* input_event, void* ctx) { + SensorModuleApp* app = ctx; + furi_message_queue_put(app->input_queue, input_event, 0); +} + +static SensorModuleApp* sensor_module_alloc(void) { + SensorModuleApp* app = malloc(sizeof(SensorModuleApp)); + + app->icm42688p_device = malloc(sizeof(FuriHalSpiBusHandle)); + memcpy(app->icm42688p_device, &furi_hal_spi_bus_handle_external, sizeof(FuriHalSpiBusHandle)); + app->icm42688p_device->cs = &gpio_ext_pc3; + app->icm42688p = icm42688p_alloc(app->icm42688p_device, &gpio_ext_pb2); + app->icm42688p_valid = icm42688p_init(app->icm42688p); + + app->input_queue = furi_message_queue_alloc(8, sizeof(InputEvent)); + + app->view_port = view_port_alloc(); + view_port_draw_callback_set(app->view_port, render_callback, app); + view_port_input_callback_set(app->view_port, input_callback, app); + + app->gui = furi_record_open(RECORD_GUI); + gui_add_view_port(app->gui, app->view_port, GuiLayerFullscreen); + + return app; +} + +static void sensor_module_free(SensorModuleApp* app) { + gui_remove_view_port(app->gui, app->view_port); + furi_record_close(RECORD_GUI); + view_port_free(app->view_port); + + furi_message_queue_free(app->input_queue); + + if(app->imu_thread) { + imu_stop(app->imu_thread); + app->imu_thread = NULL; + } + + if(!icm42688p_deinit(app->icm42688p)) { + FURI_LOG_E(TAG, "Failed to deinitialize ICM42688P"); + } + + icm42688p_free(app->icm42688p); + free(app->icm42688p_device); + + free(app); +} + +int32_t air_mouse_app(void* arg) { + UNUSED(arg); + SensorModuleApp* app = sensor_module_alloc(); + + if(!app->icm42688p_valid) { + DialogsApp* dialogs = furi_record_open(RECORD_DIALOGS); + DialogMessage* message = dialog_message_alloc(); + dialog_message_set_header(message, "Sensor Module error", 63, 0, AlignCenter, AlignTop); + + dialog_message_set_text(message, "Module not conntected", 63, 30, AlignCenter, AlignTop); + dialog_message_show(dialogs, message); + dialog_message_free(message); + furi_record_close(RECORD_DIALOGS); + + sensor_module_free(app); + return 0; + } + + view_port_update(app->view_port); + app->imu_thread = imu_start(app->icm42688p); + + while(1) { + InputEvent input; + if(furi_message_queue_get(app->input_queue, &input, FuriWaitForever) == FuriStatusOk) { + if((input.key == InputKeyBack) && (input.type == InputTypeShort)) { + break; + } else if(input.key == InputKeyOk) { + if(input.type == InputTypePress) { + imu_mouse_key_press(app->imu_thread, ImuMouseKeyLeft, true); + } else if(input.type == InputTypeRelease) { + imu_mouse_key_press(app->imu_thread, ImuMouseKeyLeft, false); + } + } else if(input.key == InputKeyUp) { + if(input.type == InputTypePress) { + imu_mouse_key_press(app->imu_thread, ImuMouseKeyRight, true); + } else if(input.type == InputTypeRelease) { + imu_mouse_key_press(app->imu_thread, ImuMouseKeyRight, false); + } + } + } + } + + sensor_module_free(app); + return 0; +} diff --git a/air_mouse/airmouse_10x10.png b/air_mouse/airmouse_10x10.png new file mode 100644 index 00000000..a61d5259 Binary files /dev/null and b/air_mouse/airmouse_10x10.png differ diff --git a/air_mouse/application.fam b/air_mouse/application.fam new file mode 100644 index 00000000..f17ff429 --- /dev/null +++ b/air_mouse/application.fam @@ -0,0 +1,10 @@ +App( + appid="air_mouse", + name="Air Mouse", + apptype=FlipperAppType.EXTERNAL, + entry_point="air_mouse_app", + stack_size=4 * 1024, + fap_icon="airmouse_10x10.png", + fap_icon_assets="assets", + fap_category="GPIO", +) diff --git a/air_mouse/assets/Circles_47x47.png b/air_mouse/assets/Circles_47x47.png new file mode 100644 index 00000000..6a16ebf7 Binary files /dev/null and b/air_mouse/assets/Circles_47x47.png differ diff --git a/air_mouse/assets/Left_mouse_icon_9x9.png b/air_mouse/assets/Left_mouse_icon_9x9.png new file mode 100644 index 00000000..c533d857 Binary files /dev/null and b/air_mouse/assets/Left_mouse_icon_9x9.png differ diff --git a/air_mouse/assets/Right_mouse_icon_9x9.png b/air_mouse/assets/Right_mouse_icon_9x9.png new file mode 100644 index 00000000..446d7176 Binary files /dev/null and b/air_mouse/assets/Right_mouse_icon_9x9.png differ diff --git a/air_mouse/imu_mouse.c b/air_mouse/imu_mouse.c new file mode 100644 index 00000000..afd97a95 --- /dev/null +++ b/air_mouse/imu_mouse.c @@ -0,0 +1,340 @@ +#include "imu_mouse.h" +#include +#include +#include "sensors/ICM42688P.h" + +#define TAG "IMU" + +#define ACCEL_GYRO_RATE DataRate1kHz + +#define FILTER_SAMPLE_FREQ 1000.f +#define FILTER_BETA 0.08f + +#define HID_RATE_DIV 5 + +#define SENSITIVITY_K 30.f +#define EXP_RATE 1.1f + +#define IMU_CALI_AVG 64 + +typedef enum { + ImuMouseStop = (1 << 0), + ImuMouseNewData = (1 << 1), + ImuMouseRightPress = (1 << 2), + ImuMouseRightRelease = (1 << 3), + ImuMouseLeftPress = (1 << 4), + ImuMouseLeftRelease = (1 << 5), +} ImuThreadFlags; + +#define FLAGS_ALL \ + (ImuMouseStop | ImuMouseNewData | ImuMouseRightPress | ImuMouseRightRelease | \ + ImuMouseLeftPress | ImuMouseLeftRelease) + +typedef struct { + float q0; + float q1; + float q2; + float q3; + float roll; + float pitch; + float yaw; +} ImuProcessedData; + +struct ImuThread { + FuriThread* thread; + ICM42688P* icm42688p; + ImuProcessedData processed_data; +}; + +static void imu_madgwick_filter( + ImuProcessedData* out, + ICM42688PScaledData* accel, + ICM42688PScaledData* gyro); + +static void imu_irq_callback(void* context) { + furi_assert(context); + ImuThread* imu = context; + furi_thread_flags_set(furi_thread_get_id(imu->thread), ImuMouseNewData); +} + +static void imu_process_data(ImuThread* imu, ICM42688PFifoPacket* in_data) { + ICM42688PScaledData accel_data; + ICM42688PScaledData gyro_data; + + // Get accel and gyro data in g and degrees/s + icm42688p_apply_scale_fifo(imu->icm42688p, in_data, &accel_data, &gyro_data); + + // Gyro: degrees/s to rads/s + gyro_data.x = gyro_data.x / 180.f * M_PI; + gyro_data.y = gyro_data.y / 180.f * M_PI; + gyro_data.z = gyro_data.z / 180.f * M_PI; + + // Sensor Fusion algorithm + ImuProcessedData* out = &imu->processed_data; + imu_madgwick_filter(out, &accel_data, &gyro_data); + + // Quaternion to euler angles + float roll = atan2f( + out->q0 * out->q1 + out->q2 * out->q3, 0.5f - out->q1 * out->q1 - out->q2 * out->q2); + float pitch = asinf(-2.0f * (out->q1 * out->q3 - out->q0 * out->q2)); + float yaw = atan2f( + out->q1 * out->q2 + out->q0 * out->q3, 0.5f - out->q2 * out->q2 - out->q3 * out->q3); + + // Euler angles: rads to degrees + out->roll = roll / M_PI * 180.f; + out->pitch = pitch / M_PI * 180.f; + out->yaw = yaw / M_PI * 180.f; +} + +static void calibrate_gyro(ImuThread* imu) { + ICM42688PRawData data; + ICM42688PScaledData offset_scaled = {.x = 0.f, .y = 0.f, .z = 0.f}; + + icm42688p_write_gyro_offset(imu->icm42688p, &offset_scaled); + furi_delay_ms(10); + + int32_t avg_x = 0; + int32_t avg_y = 0; + int32_t avg_z = 0; + + for(uint8_t i = 0; i < IMU_CALI_AVG; i++) { + icm42688p_read_gyro_raw(imu->icm42688p, &data); + avg_x += data.x; + avg_y += data.y; + avg_z += data.z; + furi_delay_ms(2); + } + + data.x = avg_x / IMU_CALI_AVG; + data.y = avg_y / IMU_CALI_AVG; + data.z = avg_z / IMU_CALI_AVG; + + icm42688p_apply_scale(&data, icm42688p_gyro_get_full_scale(imu->icm42688p), &offset_scaled); + FURI_LOG_I( + TAG, + "Offsets: x %f, y %f, z %f", + (double)offset_scaled.x, + (double)offset_scaled.y, + (double)offset_scaled.z); + icm42688p_write_gyro_offset(imu->icm42688p, &offset_scaled); +} + +static float imu_angle_diff(float a, float b) { + float diff = a - b; + if(diff > 180.f) + diff -= 360.f; + else if(diff < -180.f) + diff += 360.f; + + return diff; +} + +static int8_t mouse_exp_rate(float in) { + int8_t sign = (in < 0.f) ? (-1) : (1); + float val_in = (in * sign) / 127.f; + float val_out = powf(val_in, EXP_RATE) * 127.f; + return ((int8_t)val_out) * sign; +} + +static int32_t imu_thread(void* context) { + furi_assert(context); + ImuThread* imu = context; + + float yaw_last = 0.f; + float pitch_last = 0.f; + float diff_x = 0.f; + float diff_y = 0.f; + uint32_t sample_cnt = 0; + + FuriHalUsbInterface* usb_mode_prev = furi_hal_usb_get_config(); + furi_hal_usb_set_config(&usb_hid, NULL); + + calibrate_gyro(imu); + + icm42688p_accel_config(imu->icm42688p, AccelFullScale16G, ACCEL_GYRO_RATE); + icm42688p_gyro_config(imu->icm42688p, GyroFullScale2000DPS, ACCEL_GYRO_RATE); + + imu->processed_data.q0 = 1.f; + imu->processed_data.q1 = 0.f; + imu->processed_data.q2 = 0.f; + imu->processed_data.q3 = 0.f; + icm42688_fifo_enable(imu->icm42688p, imu_irq_callback, imu); + + while(1) { + uint32_t events = furi_thread_flags_wait(FLAGS_ALL, FuriFlagWaitAny, FuriWaitForever); + + if(events & ImuMouseStop) { + break; + } + + if(events & ImuMouseRightPress) { + furi_hal_hid_mouse_press(HID_MOUSE_BTN_RIGHT); + } + if(events & ImuMouseRightRelease) { + furi_hal_hid_mouse_release(HID_MOUSE_BTN_RIGHT); + } + if(events & ImuMouseLeftPress) { + furi_hal_hid_mouse_press(HID_MOUSE_BTN_LEFT); + } + if(events & ImuMouseLeftRelease) { + furi_hal_hid_mouse_release(HID_MOUSE_BTN_LEFT); + } + + if(events & ImuMouseNewData) { + uint16_t data_pending = icm42688_fifo_get_count(imu->icm42688p); + ICM42688PFifoPacket data; + while(data_pending--) { + icm42688_fifo_read(imu->icm42688p, &data); + imu_process_data(imu, &data); + + if((imu->processed_data.pitch > -75.f) && (imu->processed_data.pitch < 75.f) && + (isfinite(imu->processed_data.pitch) != 0)) { + diff_x += imu_angle_diff(yaw_last, imu->processed_data.yaw) * SENSITIVITY_K; + diff_y += + imu_angle_diff(pitch_last, -imu->processed_data.pitch) * SENSITIVITY_K; + + yaw_last = imu->processed_data.yaw; + pitch_last = -imu->processed_data.pitch; + + sample_cnt++; + if(sample_cnt >= HID_RATE_DIV) { + sample_cnt = 0; + + float mouse_x = CLAMP(diff_x, 127.f, -127.f); + float mouse_y = CLAMP(diff_y, 127.f, -127.f); + + furi_hal_hid_mouse_move(mouse_exp_rate(mouse_x), mouse_exp_rate(mouse_y)); + + diff_x -= (float)(int8_t)mouse_x; + diff_y -= (float)(int8_t)mouse_y; + } + } + } + } + } + + furi_hal_hid_mouse_release(HID_MOUSE_BTN_RIGHT | HID_MOUSE_BTN_LEFT); + furi_hal_usb_set_config(usb_mode_prev, NULL); + + icm42688_fifo_disable(imu->icm42688p); + + return 0; +} + +void imu_mouse_key_press(ImuThread* imu, ImuMouseKey key, bool state) { + furi_assert(imu); + uint32_t flag = 0; + if(key == ImuMouseKeyRight) { + flag = (state) ? (ImuMouseRightPress) : (ImuMouseRightRelease); + } else if(key == ImuMouseKeyLeft) { + flag = (state) ? (ImuMouseLeftPress) : (ImuMouseLeftRelease); + } + + furi_thread_flags_set(furi_thread_get_id(imu->thread), flag); +} + +ImuThread* imu_start(ICM42688P* icm42688p) { + ImuThread* imu = malloc(sizeof(ImuThread)); + imu->icm42688p = icm42688p; + imu->thread = furi_thread_alloc_ex("ImuThread", 4096, imu_thread, imu); + furi_thread_start(imu->thread); + + return imu; +} + +void imu_stop(ImuThread* imu) { + furi_assert(imu); + + furi_thread_flags_set(furi_thread_get_id(imu->thread), ImuMouseStop); + + furi_thread_join(imu->thread); + furi_thread_free(imu->thread); + + free(imu); +} + +static float imu_inv_sqrt(float number) { + union { + float f; + uint32_t i; + } conv = {.f = number}; + conv.i = 0x5F3759Df - (conv.i >> 1); + conv.f *= 1.5f - (number * 0.5f * conv.f * conv.f); + return conv.f; +} + +/* Simple madgwik filter, based on: https://github.com/arduino-libraries/MadgwickAHRS/ */ + +static void imu_madgwick_filter( + ImuProcessedData* out, + ICM42688PScaledData* accel, + ICM42688PScaledData* gyro) { + float recipNorm; + float s0, s1, s2, s3; + float qDot1, qDot2, qDot3, qDot4; + float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2, q3q3; + + // Rate of change of quaternion from gyroscope + qDot1 = 0.5f * (-out->q1 * gyro->x - out->q2 * gyro->y - out->q3 * gyro->z); + qDot2 = 0.5f * (out->q0 * gyro->x + out->q2 * gyro->z - out->q3 * gyro->y); + qDot3 = 0.5f * (out->q0 * gyro->y - out->q1 * gyro->z + out->q3 * gyro->x); + qDot4 = 0.5f * (out->q0 * gyro->z + out->q1 * gyro->y - out->q2 * gyro->x); + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if(!((accel->x == 0.0f) && (accel->y == 0.0f) && (accel->z == 0.0f))) { + // Normalise accelerometer measurement + recipNorm = imu_inv_sqrt(accel->x * accel->x + accel->y * accel->y + accel->z * accel->z); + accel->x *= recipNorm; + accel->y *= recipNorm; + accel->z *= recipNorm; + + // Auxiliary variables to avoid repeated arithmetic + _2q0 = 2.0f * out->q0; + _2q1 = 2.0f * out->q1; + _2q2 = 2.0f * out->q2; + _2q3 = 2.0f * out->q3; + _4q0 = 4.0f * out->q0; + _4q1 = 4.0f * out->q1; + _4q2 = 4.0f * out->q2; + _8q1 = 8.0f * out->q1; + _8q2 = 8.0f * out->q2; + q0q0 = out->q0 * out->q0; + q1q1 = out->q1 * out->q1; + q2q2 = out->q2 * out->q2; + q3q3 = out->q3 * out->q3; + + // Gradient decent algorithm corrective step + s0 = _4q0 * q2q2 + _2q2 * accel->x + _4q0 * q1q1 - _2q1 * accel->y; + s1 = _4q1 * q3q3 - _2q3 * accel->x + 4.0f * q0q0 * out->q1 - _2q0 * accel->y - _4q1 + + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * accel->z; + s2 = 4.0f * q0q0 * out->q2 + _2q0 * accel->x + _4q2 * q3q3 - _2q3 * accel->y - _4q2 + + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * accel->z; + s3 = 4.0f * q1q1 * out->q3 - _2q1 * accel->x + 4.0f * q2q2 * out->q3 - _2q2 * accel->y; + recipNorm = + imu_inv_sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + // Apply feedback step + qDot1 -= FILTER_BETA * s0; + qDot2 -= FILTER_BETA * s1; + qDot3 -= FILTER_BETA * s2; + qDot4 -= FILTER_BETA * s3; + } + + // Integrate rate of change of quaternion to yield quaternion + out->q0 += qDot1 * (1.0f / FILTER_SAMPLE_FREQ); + out->q1 += qDot2 * (1.0f / FILTER_SAMPLE_FREQ); + out->q2 += qDot3 * (1.0f / FILTER_SAMPLE_FREQ); + out->q3 += qDot4 * (1.0f / FILTER_SAMPLE_FREQ); + + // Normalise quaternion + recipNorm = imu_inv_sqrt( + out->q0 * out->q0 + out->q1 * out->q1 + out->q2 * out->q2 + out->q3 * out->q3); + out->q0 *= recipNorm; + out->q1 *= recipNorm; + out->q2 *= recipNorm; + out->q3 *= recipNorm; +} diff --git a/air_mouse/imu_mouse.h b/air_mouse/imu_mouse.h new file mode 100644 index 00000000..b2eebfb7 --- /dev/null +++ b/air_mouse/imu_mouse.h @@ -0,0 +1,16 @@ +#pragma once + +#include "sensors/ICM42688P.h" + +typedef enum { + ImuMouseKeyRight, + ImuMouseKeyLeft, +} ImuMouseKey; + +typedef struct ImuThread ImuThread; + +ImuThread* imu_start(ICM42688P* icm42688p); + +void imu_stop(ImuThread* imu); + +void imu_mouse_key_press(ImuThread* imu, ImuMouseKey key, bool state); diff --git a/air_mouse/sensors/ICM42688P.c b/air_mouse/sensors/ICM42688P.c new file mode 100644 index 00000000..9f8e9a0a --- /dev/null +++ b/air_mouse/sensors/ICM42688P.c @@ -0,0 +1,297 @@ +#include "ICM42688P_regs.h" +#include "ICM42688P.h" + +#define TAG "ICM42688P" + +#define ICM42688P_TIMEOUT 100 + +struct ICM42688P { + FuriHalSpiBusHandle* spi_bus; + const GpioPin* irq_pin; + float accel_scale; + float gyro_scale; +}; + +static const struct AccelFullScale { + float value; + uint8_t reg_mask; +} accel_fs_modes[] = { + [AccelFullScale16G] = {16.f, ICM42688_AFS_16G}, + [AccelFullScale8G] = {8.f, ICM42688_AFS_8G}, + [AccelFullScale4G] = {4.f, ICM42688_AFS_4G}, + [AccelFullScale2G] = {2.f, ICM42688_AFS_2G}, +}; + +static const struct GyroFullScale { + float value; + uint8_t reg_mask; +} gyro_fs_modes[] = { + [GyroFullScale2000DPS] = {2000.f, ICM42688_GFS_2000DPS}, + [GyroFullScale1000DPS] = {1000.f, ICM42688_GFS_1000DPS}, + [GyroFullScale500DPS] = {500.f, ICM42688_GFS_500DPS}, + [GyroFullScale250DPS] = {250.f, ICM42688_GFS_250DPS}, + [GyroFullScale125DPS] = {125.f, ICM42688_GFS_125DPS}, + [GyroFullScale62_5DPS] = {62.5f, ICM42688_GFS_62_5DPS}, + [GyroFullScale31_25DPS] = {31.25f, ICM42688_GFS_31_25DPS}, + [GyroFullScale15_625DPS] = {15.625f, ICM42688_GFS_15_625DPS}, +}; + +static bool icm42688p_write_reg(FuriHalSpiBusHandle* spi_bus, uint8_t addr, uint8_t value) { + bool res = false; + furi_hal_spi_acquire(spi_bus); + do { + uint8_t cmd_data[2] = {addr & 0x7F, value}; + if(!furi_hal_spi_bus_tx(spi_bus, cmd_data, 2, ICM42688P_TIMEOUT)) break; + res = true; + } while(0); + furi_hal_spi_release(spi_bus); + return res; +} + +static bool icm42688p_read_reg(FuriHalSpiBusHandle* spi_bus, uint8_t addr, uint8_t* value) { + bool res = false; + furi_hal_spi_acquire(spi_bus); + do { + uint8_t cmd_byte = addr | (1 << 7); + if(!furi_hal_spi_bus_tx(spi_bus, &cmd_byte, 1, ICM42688P_TIMEOUT)) break; + if(!furi_hal_spi_bus_rx(spi_bus, value, 1, ICM42688P_TIMEOUT)) break; + res = true; + } while(0); + furi_hal_spi_release(spi_bus); + return res; +} + +static bool + icm42688p_read_mem(FuriHalSpiBusHandle* spi_bus, uint8_t addr, uint8_t* data, uint8_t len) { + bool res = false; + furi_hal_spi_acquire(spi_bus); + do { + uint8_t cmd_byte = addr | (1 << 7); + if(!furi_hal_spi_bus_tx(spi_bus, &cmd_byte, 1, ICM42688P_TIMEOUT)) break; + if(!furi_hal_spi_bus_rx(spi_bus, data, len, ICM42688P_TIMEOUT)) break; + res = true; + } while(0); + furi_hal_spi_release(spi_bus); + return res; +} + +bool icm42688p_accel_config( + ICM42688P* icm42688p, + ICM42688PAccelFullScale full_scale, + ICM42688PDataRate rate) { + icm42688p->accel_scale = accel_fs_modes[full_scale].value; + uint8_t reg_value = accel_fs_modes[full_scale].reg_mask | rate; + return icm42688p_write_reg(icm42688p->spi_bus, ICM42688_ACCEL_CONFIG0, reg_value); +} + +float icm42688p_accel_get_full_scale(ICM42688P* icm42688p) { + return icm42688p->accel_scale; +} + +bool icm42688p_gyro_config( + ICM42688P* icm42688p, + ICM42688PGyroFullScale full_scale, + ICM42688PDataRate rate) { + icm42688p->gyro_scale = gyro_fs_modes[full_scale].value; + uint8_t reg_value = gyro_fs_modes[full_scale].reg_mask | rate; + return icm42688p_write_reg(icm42688p->spi_bus, ICM42688_GYRO_CONFIG0, reg_value); +} + +float icm42688p_gyro_get_full_scale(ICM42688P* icm42688p) { + return icm42688p->gyro_scale; +} + +bool icm42688p_read_accel_raw(ICM42688P* icm42688p, ICM42688PRawData* data) { + bool ret = icm42688p_read_mem( + icm42688p->spi_bus, ICM42688_ACCEL_DATA_X1, (uint8_t*)data, sizeof(ICM42688PRawData)); + return ret; +} + +bool icm42688p_read_gyro_raw(ICM42688P* icm42688p, ICM42688PRawData* data) { + bool ret = icm42688p_read_mem( + icm42688p->spi_bus, ICM42688_GYRO_DATA_X1, (uint8_t*)data, sizeof(ICM42688PRawData)); + return ret; +} + +bool icm42688p_write_gyro_offset(ICM42688P* icm42688p, ICM42688PScaledData* scaled_data) { + if((fabsf(scaled_data->x) > 64.f) || (fabsf(scaled_data->y) > 64.f) || + (fabsf(scaled_data->z) > 64.f)) { + return false; + } + + uint16_t offset_x = (uint16_t)(-(int16_t)(scaled_data->x * 32.f) * 16) >> 4; + uint16_t offset_y = (uint16_t)(-(int16_t)(scaled_data->y * 32.f) * 16) >> 4; + uint16_t offset_z = (uint16_t)(-(int16_t)(scaled_data->z * 32.f) * 16) >> 4; + + uint8_t offset_regs[9]; + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_REG_BANK_SEL, 4); + icm42688p_read_mem(icm42688p->spi_bus, ICM42688_OFFSET_USER0, offset_regs, 5); + + offset_regs[0] = offset_x & 0xFF; + offset_regs[1] = (offset_x & 0xF00) >> 8; + offset_regs[1] |= (offset_y & 0xF00) >> 4; + offset_regs[2] = offset_y & 0xFF; + offset_regs[3] = offset_z & 0xFF; + offset_regs[4] &= 0xF0; + offset_regs[4] |= (offset_z & 0x0F00) >> 8; + + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_OFFSET_USER0, offset_regs[0]); + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_OFFSET_USER1, offset_regs[1]); + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_OFFSET_USER2, offset_regs[2]); + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_OFFSET_USER3, offset_regs[3]); + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_OFFSET_USER4, offset_regs[4]); + + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_REG_BANK_SEL, 0); + return true; +} + +void icm42688p_apply_scale(ICM42688PRawData* raw_data, float full_scale, ICM42688PScaledData* data) { + data->x = ((float)(raw_data->x)) / 32768.f * full_scale; + data->y = ((float)(raw_data->y)) / 32768.f * full_scale; + data->z = ((float)(raw_data->z)) / 32768.f * full_scale; +} + +void icm42688p_apply_scale_fifo( + ICM42688P* icm42688p, + ICM42688PFifoPacket* fifo_data, + ICM42688PScaledData* accel_data, + ICM42688PScaledData* gyro_data) { + float full_scale = icm42688p->accel_scale; + accel_data->x = ((float)(fifo_data->a_x)) / 32768.f * full_scale; + accel_data->y = ((float)(fifo_data->a_y)) / 32768.f * full_scale; + accel_data->z = ((float)(fifo_data->a_z)) / 32768.f * full_scale; + + full_scale = icm42688p->gyro_scale; + gyro_data->x = ((float)(fifo_data->g_x)) / 32768.f * full_scale; + gyro_data->y = ((float)(fifo_data->g_y)) / 32768.f * full_scale; + gyro_data->z = ((float)(fifo_data->g_z)) / 32768.f * full_scale; +} + +float icm42688p_read_temp(ICM42688P* icm42688p) { + uint8_t reg_val[2]; + + icm42688p_read_mem(icm42688p->spi_bus, ICM42688_TEMP_DATA1, reg_val, 2); + int16_t temp_int = (reg_val[0] << 8) | reg_val[1]; + return ((float)temp_int / 132.48f) + 25.f; +} + +void icm42688_fifo_enable( + ICM42688P* icm42688p, + ICM42688PIrqCallback irq_callback, + void* irq_context) { + // FIFO mode: stream + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_FIFO_CONFIG, (1 << 6)); + // Little-endian data, FIFO count in records + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INTF_CONFIG0, (1 << 7) | (1 << 6)); + // FIFO partial read, FIFO packet: gyro + accel TODO: 20bit + icm42688p_write_reg( + icm42688p->spi_bus, ICM42688_FIFO_CONFIG1, (1 << 6) | (1 << 5) | (1 << 1) | (1 << 0)); + // FIFO irq watermark + uint16_t fifo_watermark = 1; + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_FIFO_CONFIG2, fifo_watermark & 0xFF); + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_FIFO_CONFIG3, fifo_watermark >> 8); + + // IRQ1: push-pull, active high + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_CONFIG, (1 << 1) | (1 << 0)); + // Clear IRQ on status read + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_CONFIG0, 0); + // IRQ pulse duration + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_CONFIG1, (1 << 6) | (1 << 5)); + + uint8_t reg_data = 0; + icm42688p_read_reg(icm42688p->spi_bus, ICM42688_INT_STATUS, ®_data); + + furi_hal_gpio_init(icm42688p->irq_pin, GpioModeInterruptRise, GpioPullDown, GpioSpeedVeryHigh); + furi_hal_gpio_remove_int_callback(icm42688p->irq_pin); + furi_hal_gpio_add_int_callback(icm42688p->irq_pin, irq_callback, irq_context); + + // IRQ1 source: FIFO threshold + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_SOURCE0, (1 << 2)); +} + +void icm42688_fifo_disable(ICM42688P* icm42688p) { + furi_hal_gpio_remove_int_callback(icm42688p->irq_pin); + furi_hal_gpio_init(icm42688p->irq_pin, GpioModeAnalog, GpioPullNo, GpioSpeedLow); + + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_SOURCE0, 0); + + // FIFO mode: bypass + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_FIFO_CONFIG, 0); +} + +uint16_t icm42688_fifo_get_count(ICM42688P* icm42688p) { + uint16_t reg_val = 0; + icm42688p_read_mem(icm42688p->spi_bus, ICM42688_FIFO_COUNTH, (uint8_t*)®_val, 2); + return reg_val; +} + +bool icm42688_fifo_read(ICM42688P* icm42688p, ICM42688PFifoPacket* data) { + icm42688p_read_mem( + icm42688p->spi_bus, ICM42688_FIFO_DATA, (uint8_t*)data, sizeof(ICM42688PFifoPacket)); + return (data->header) & (1 << 7); +} + +ICM42688P* icm42688p_alloc(FuriHalSpiBusHandle* spi_bus, const GpioPin* irq_pin) { + ICM42688P* icm42688p = malloc(sizeof(ICM42688P)); + icm42688p->spi_bus = spi_bus; + icm42688p->irq_pin = irq_pin; + return icm42688p; +} + +void icm42688p_free(ICM42688P* icm42688p) { + free(icm42688p); +} + +bool icm42688p_init(ICM42688P* icm42688p) { + furi_hal_spi_bus_handle_init(icm42688p->spi_bus); + + // Software reset + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_REG_BANK_SEL, 0); // Set reg bank to 0 + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_DEVICE_CONFIG, 0x01); // SPI Mode 0, SW reset + furi_delay_ms(1); + + uint8_t reg_value = 0; + bool read_ok = icm42688p_read_reg(icm42688p->spi_bus, ICM42688_WHO_AM_I, ®_value); + if(!read_ok) { + FURI_LOG_E(TAG, "Chip ID read failed"); + return false; + } else if(reg_value != ICM42688_WHOAMI) { + FURI_LOG_E( + TAG, "Sensor returned wrong ID 0x%02X, expected 0x%02X", reg_value, ICM42688_WHOAMI); + return false; + } + + // Disable all interrupts + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_SOURCE0, 0); + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_SOURCE1, 0); + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_SOURCE3, 0); + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_SOURCE4, 0); + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_REG_BANK_SEL, 4); + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_SOURCE6, 0); + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_SOURCE7, 0); + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_REG_BANK_SEL, 0); + + // Data format: little endian + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INTF_CONFIG0, 0); + + // Enable all sensors + icm42688p_write_reg( + icm42688p->spi_bus, + ICM42688_PWR_MGMT0, + ICM42688_PWR_TEMP_ON | ICM42688_PWR_GYRO_MODE_LN | ICM42688_PWR_ACCEL_MODE_LN); + furi_delay_ms(45); + + icm42688p_accel_config(icm42688p, AccelFullScale16G, DataRate1kHz); + icm42688p_gyro_config(icm42688p, GyroFullScale2000DPS, DataRate1kHz); + + return true; +} + +bool icm42688p_deinit(ICM42688P* icm42688p) { + // Software reset + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_REG_BANK_SEL, 0); // Set reg bank to 0 + icm42688p_write_reg(icm42688p->spi_bus, ICM42688_DEVICE_CONFIG, 0x01); // SPI Mode 0, SW reset + + furi_hal_spi_bus_handle_deinit(icm42688p->spi_bus); + return true; +} diff --git a/air_mouse/sensors/ICM42688P.h b/air_mouse/sensors/ICM42688P.h new file mode 100644 index 00000000..b04fb980 --- /dev/null +++ b/air_mouse/sensors/ICM42688P.h @@ -0,0 +1,127 @@ +#pragma once + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +typedef enum { + DataRate32kHz = 0x01, + DataRate16kHz = 0x02, + DataRate8kHz = 0x03, + DataRate4kHz = 0x04, + DataRate2kHz = 0x05, + DataRate1kHz = 0x06, + DataRate200Hz = 0x07, + DataRate100Hz = 0x08, + DataRate50Hz = 0x09, + DataRate25Hz = 0x0A, + DataRate12_5Hz = 0x0B, + DataRate6_25Hz = 0x0C, // Accelerometer only + DataRate3_125Hz = 0x0D, // Accelerometer only + DataRate1_5625Hz = 0x0E, // Accelerometer only + DataRate500Hz = 0x0F, +} ICM42688PDataRate; + +typedef enum { + AccelFullScale16G = 0, + AccelFullScale8G, + AccelFullScale4G, + AccelFullScale2G, + AccelFullScaleTotal, +} ICM42688PAccelFullScale; + +typedef enum { + GyroFullScale2000DPS = 0, + GyroFullScale1000DPS, + GyroFullScale500DPS, + GyroFullScale250DPS, + GyroFullScale125DPS, + GyroFullScale62_5DPS, + GyroFullScale31_25DPS, + GyroFullScale15_625DPS, + GyroFullScaleTotal, +} ICM42688PGyroFullScale; + +typedef struct { + int16_t x; + int16_t y; + int16_t z; +} __attribute__((packed)) ICM42688PRawData; + +typedef struct { + uint8_t header; + int16_t a_x; + int16_t a_y; + int16_t a_z; + int16_t g_x; + int16_t g_y; + int16_t g_z; + uint8_t temp; + uint16_t ts; +} __attribute__((packed)) ICM42688PFifoPacket; + +typedef struct { + float x; + float y; + float z; +} ICM42688PScaledData; + +typedef struct ICM42688P ICM42688P; + +typedef void (*ICM42688PIrqCallback)(void* ctx); + +ICM42688P* icm42688p_alloc(FuriHalSpiBusHandle* spi_bus, const GpioPin* irq_pin); + +bool icm42688p_init(ICM42688P* icm42688p); + +bool icm42688p_deinit(ICM42688P* icm42688p); + +void icm42688p_free(ICM42688P* icm42688p); + +bool icm42688p_accel_config( + ICM42688P* icm42688p, + ICM42688PAccelFullScale full_scale, + ICM42688PDataRate rate); + +float icm42688p_accel_get_full_scale(ICM42688P* icm42688p); + +bool icm42688p_gyro_config( + ICM42688P* icm42688p, + ICM42688PGyroFullScale full_scale, + ICM42688PDataRate rate); + +float icm42688p_gyro_get_full_scale(ICM42688P* icm42688p); + +bool icm42688p_read_accel_raw(ICM42688P* icm42688p, ICM42688PRawData* data); + +bool icm42688p_read_gyro_raw(ICM42688P* icm42688p, ICM42688PRawData* data); + +bool icm42688p_write_gyro_offset(ICM42688P* icm42688p, ICM42688PScaledData* scaled_data); + +void icm42688p_apply_scale(ICM42688PRawData* raw_data, float full_scale, ICM42688PScaledData* data); + +void icm42688p_apply_scale_fifo( + ICM42688P* icm42688p, + ICM42688PFifoPacket* fifo_data, + ICM42688PScaledData* accel_data, + ICM42688PScaledData* gyro_data); + +float icm42688p_read_temp(ICM42688P* icm42688p); + +void icm42688_fifo_enable( + ICM42688P* icm42688p, + ICM42688PIrqCallback irq_callback, + void* irq_context); + +void icm42688_fifo_disable(ICM42688P* icm42688p); + +uint16_t icm42688_fifo_get_count(ICM42688P* icm42688p); + +bool icm42688_fifo_read(ICM42688P* icm42688p, ICM42688PFifoPacket* data); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/air_mouse/sensors/ICM42688P_regs.h b/air_mouse/sensors/ICM42688P_regs.h new file mode 100644 index 00000000..1967534d --- /dev/null +++ b/air_mouse/sensors/ICM42688P_regs.h @@ -0,0 +1,176 @@ +#pragma once + +#define ICM42688_WHOAMI 0x47 + +// Bank 0 +#define ICM42688_DEVICE_CONFIG 0x11 +#define ICM42688_DRIVE_CONFIG 0x13 +#define ICM42688_INT_CONFIG 0x14 +#define ICM42688_FIFO_CONFIG 0x16 +#define ICM42688_TEMP_DATA1 0x1D +#define ICM42688_TEMP_DATA0 0x1E +#define ICM42688_ACCEL_DATA_X1 0x1F +#define ICM42688_ACCEL_DATA_X0 0x20 +#define ICM42688_ACCEL_DATA_Y1 0x21 +#define ICM42688_ACCEL_DATA_Y0 0x22 +#define ICM42688_ACCEL_DATA_Z1 0x23 +#define ICM42688_ACCEL_DATA_Z0 0x24 +#define ICM42688_GYRO_DATA_X1 0x25 +#define ICM42688_GYRO_DATA_X0 0x26 +#define ICM42688_GYRO_DATA_Y1 0x27 +#define ICM42688_GYRO_DATA_Y0 0x28 +#define ICM42688_GYRO_DATA_Z1 0x29 +#define ICM42688_GYRO_DATA_Z0 0x2A +#define ICM42688_TMST_FSYNCH 0x2B +#define ICM42688_TMST_FSYNCL 0x2C +#define ICM42688_INT_STATUS 0x2D +#define ICM42688_FIFO_COUNTH 0x2E +#define ICM42688_FIFO_COUNTL 0x2F +#define ICM42688_FIFO_DATA 0x30 +#define ICM42688_APEX_DATA0 0x31 +#define ICM42688_APEX_DATA1 0x32 +#define ICM42688_APEX_DATA2 0x33 +#define ICM42688_APEX_DATA3 0x34 +#define ICM42688_APEX_DATA4 0x35 +#define ICM42688_APEX_DATA5 0x36 +#define ICM42688_INT_STATUS2 0x37 +#define ICM42688_INT_STATUS3 0x38 +#define ICM42688_SIGNAL_PATH_RESET 0x4B +#define ICM42688_INTF_CONFIG0 0x4C +#define ICM42688_INTF_CONFIG1 0x4D +#define ICM42688_PWR_MGMT0 0x4E +#define ICM42688_GYRO_CONFIG0 0x4F +#define ICM42688_ACCEL_CONFIG0 0x50 +#define ICM42688_GYRO_CONFIG1 0x51 +#define ICM42688_GYRO_ACCEL_CONFIG0 0x52 +#define ICM42688_ACCEL_CONFIG1 0x53 +#define ICM42688_TMST_CONFIG 0x54 +#define ICM42688_APEX_CONFIG0 0x56 +#define ICM42688_SMD_CONFIG 0x57 +#define ICM42688_FIFO_CONFIG1 0x5F +#define ICM42688_FIFO_CONFIG2 0x60 +#define ICM42688_FIFO_CONFIG3 0x61 +#define ICM42688_FSYNC_CONFIG 0x62 +#define ICM42688_INT_CONFIG0 0x63 +#define ICM42688_INT_CONFIG1 0x64 +#define ICM42688_INT_SOURCE0 0x65 +#define ICM42688_INT_SOURCE1 0x66 +#define ICM42688_INT_SOURCE3 0x68 +#define ICM42688_INT_SOURCE4 0x69 +#define ICM42688_FIFO_LOST_PKT0 0x6C +#define ICM42688_FIFO_LOST_PKT1 0x6D +#define ICM42688_SELF_TEST_CONFIG 0x70 +#define ICM42688_WHO_AM_I 0x75 +#define ICM42688_REG_BANK_SEL 0x76 + +// Bank 1 +#define ICM42688_SENSOR_CONFIG0 0x03 +#define ICM42688_GYRO_CONFIG_STATIC2 0x0B +#define ICM42688_GYRO_CONFIG_STATIC3 0x0C +#define ICM42688_GYRO_CONFIG_STATIC4 0x0D +#define ICM42688_GYRO_CONFIG_STATIC5 0x0E +#define ICM42688_GYRO_CONFIG_STATIC6 0x0F +#define ICM42688_GYRO_CONFIG_STATIC7 0x10 +#define ICM42688_GYRO_CONFIG_STATIC8 0x11 +#define ICM42688_GYRO_CONFIG_STATIC9 0x12 +#define ICM42688_GYRO_CONFIG_STATIC10 0x13 +#define ICM42688_XG_ST_DATA 0x5F +#define ICM42688_YG_ST_DATA 0x60 +#define ICM42688_ZG_ST_DATA 0x61 +#define ICM42688_TMSTVAL0 0x62 +#define ICM42688_TMSTVAL1 0x63 +#define ICM42688_TMSTVAL2 0x64 +#define ICM42688_INTF_CONFIG4 0x7A +#define ICM42688_INTF_CONFIG5 0x7B +#define ICM42688_INTF_CONFIG6 0x7C + +// Bank 2 +#define ICM42688_ACCEL_CONFIG_STATIC2 0x03 +#define ICM42688_ACCEL_CONFIG_STATIC3 0x04 +#define ICM42688_ACCEL_CONFIG_STATIC4 0x05 +#define ICM42688_XA_ST_DATA 0x3B +#define ICM42688_YA_ST_DATA 0x3C +#define ICM42688_ZA_ST_DATA 0x3D + +// Bank 4 +#define ICM42688_APEX_CONFIG1 0x40 +#define ICM42688_APEX_CONFIG2 0x41 +#define ICM42688_APEX_CONFIG3 0x42 +#define ICM42688_APEX_CONFIG4 0x43 +#define ICM42688_APEX_CONFIG5 0x44 +#define ICM42688_APEX_CONFIG6 0x45 +#define ICM42688_APEX_CONFIG7 0x46 +#define ICM42688_APEX_CONFIG8 0x47 +#define ICM42688_APEX_CONFIG9 0x48 +#define ICM42688_ACCEL_WOM_X_THR 0x4A +#define ICM42688_ACCEL_WOM_Y_THR 0x4B +#define ICM42688_ACCEL_WOM_Z_THR 0x4C +#define ICM42688_INT_SOURCE6 0x4D +#define ICM42688_INT_SOURCE7 0x4E +#define ICM42688_INT_SOURCE8 0x4F +#define ICM42688_INT_SOURCE9 0x50 +#define ICM42688_INT_SOURCE10 0x51 +#define ICM42688_OFFSET_USER0 0x77 +#define ICM42688_OFFSET_USER1 0x78 +#define ICM42688_OFFSET_USER2 0x79 +#define ICM42688_OFFSET_USER3 0x7A +#define ICM42688_OFFSET_USER4 0x7B +#define ICM42688_OFFSET_USER5 0x7C +#define ICM42688_OFFSET_USER6 0x7D +#define ICM42688_OFFSET_USER7 0x7E +#define ICM42688_OFFSET_USER8 0x7F + +// PWR_MGMT0 +#define ICM42688_PWR_TEMP_ON (0 << 5) +#define ICM42688_PWR_TEMP_OFF (1 << 5) +#define ICM42688_PWR_IDLE (1 << 4) +#define ICM42688_PWR_GYRO_MODE_OFF (0 << 2) +#define ICM42688_PWR_GYRO_MODE_LN (3 << 2) +#define ICM42688_PWR_ACCEL_MODE_OFF (0 << 0) +#define ICM42688_PWR_ACCEL_MODE_LP (2 << 0) +#define ICM42688_PWR_ACCEL_MODE_LN (3 << 0) + +// GYRO_CONFIG0 +#define ICM42688_GFS_2000DPS (0x00 << 5) +#define ICM42688_GFS_1000DPS (0x01 << 5) +#define ICM42688_GFS_500DPS (0x02 << 5) +#define ICM42688_GFS_250DPS (0x03 << 5) +#define ICM42688_GFS_125DPS (0x04 << 5) +#define ICM42688_GFS_62_5DPS (0x05 << 5) +#define ICM42688_GFS_31_25DPS (0x06 << 5) +#define ICM42688_GFS_15_625DPS (0x07 << 5) + +#define ICM42688_GODR_32kHz 0x01 +#define ICM42688_GODR_16kHz 0x02 +#define ICM42688_GODR_8kHz 0x03 +#define ICM42688_GODR_4kHz 0x04 +#define ICM42688_GODR_2kHz 0x05 +#define ICM42688_GODR_1kHz 0x06 +#define ICM42688_GODR_200Hz 0x07 +#define ICM42688_GODR_100Hz 0x08 +#define ICM42688_GODR_50Hz 0x09 +#define ICM42688_GODR_25Hz 0x0A +#define ICM42688_GODR_12_5Hz 0x0B +#define ICM42688_GODR_500Hz 0x0F + +// ACCEL_CONFIG0 +#define ICM42688_AFS_16G (0x00 << 5) +#define ICM42688_AFS_8G (0x01 << 5) +#define ICM42688_AFS_4G (0x02 << 5) +#define ICM42688_AFS_2G (0x03 << 5) + +#define ICM42688_AODR_32kHz 0x01 +#define ICM42688_AODR_16kHz 0x02 +#define ICM42688_AODR_8kHz 0x03 +#define ICM42688_AODR_4kHz 0x04 +#define ICM42688_AODR_2kHz 0x05 +#define ICM42688_AODR_1kHz 0x06 +#define ICM42688_AODR_200Hz 0x07 +#define ICM42688_AODR_100Hz 0x08 +#define ICM42688_AODR_50Hz 0x09 +#define ICM42688_AODR_25Hz 0x0A +#define ICM42688_AODR_12_5Hz 0x0B +#define ICM42688_AODR_6_25Hz 0x0C +#define ICM42688_AODR_3_125Hz 0x0D +#define ICM42688_AODR_1_5625Hz 0x0E +#define ICM42688_AODR_500Hz 0x0F