|
4 | 4 | #include "board_com_api.h" |
5 | 5 | #include "board_pixel_api.h" |
6 | 6 | #include "board_buzzer_api.h" |
| 7 | +#include "board_bmi220_api.h" |
7 | 8 | #include "board_bmi270_api.h" |
8 | 9 | #include "tdl_button_manage.h" |
9 | 10 | #include <string.h> |
@@ -70,9 +71,69 @@ typedef struct { |
70 | 71 |
|
71 | 72 | static sand_particle_t g_sand_particles[MAX_SAND_PARTICLES]; |
72 | 73 | static uint32_t g_last_sand_spawn_time = 0; |
| 74 | +/* IMU sensor abstraction: supports BMI220 (default) and BMI270 */ |
| 75 | +typedef enum { |
| 76 | + IMU_TYPE_NONE = 0, |
| 77 | + IMU_TYPE_BMI220, |
| 78 | + IMU_TYPE_BMI270, |
| 79 | +} imu_type_t; |
| 80 | + |
| 81 | +typedef struct { |
| 82 | + float acc_x, acc_y, acc_z; |
| 83 | + float gyr_x, gyr_y, gyr_z; |
| 84 | +} imu_sensor_data_t; |
| 85 | + |
| 86 | +static imu_type_t g_imu_type = IMU_TYPE_NONE; |
| 87 | +static bmi220_dev_t *g_bmi220_dev = NULL; |
73 | 88 | static bmi270_dev_t *g_bmi270_dev = NULL; |
74 | 89 | static bool g_sand_initialized = false; |
75 | 90 |
|
| 91 | +/** |
| 92 | + * @brief Read IMU sensor data (works with either BMI220 or BMI270) |
| 93 | + */ |
| 94 | +static OPERATE_RET imu_read_data(imu_sensor_data_t *data) |
| 95 | +{ |
| 96 | + if (!data) { |
| 97 | + return OPRT_INVALID_PARM; |
| 98 | + } |
| 99 | + |
| 100 | + if (g_imu_type == IMU_TYPE_BMI220 && g_bmi220_dev != NULL && board_bmi220_is_ready(g_bmi220_dev)) { |
| 101 | + bmi220_sensor_data_t raw = {0}; |
| 102 | + OPERATE_RET ret = board_bmi220_read_data(g_bmi220_dev, &raw); |
| 103 | + if (ret == OPRT_OK) { |
| 104 | + data->acc_x = raw.acc_x; |
| 105 | + data->acc_y = raw.acc_y; |
| 106 | + data->acc_z = raw.acc_z; |
| 107 | + data->gyr_x = raw.gyr_x; |
| 108 | + data->gyr_y = raw.gyr_y; |
| 109 | + data->gyr_z = raw.gyr_z; |
| 110 | + } |
| 111 | + return ret; |
| 112 | + } else if (g_imu_type == IMU_TYPE_BMI270 && g_bmi270_dev != NULL && board_bmi270_is_ready(g_bmi270_dev)) { |
| 113 | + bmi270_sensor_data_t raw = {0}; |
| 114 | + OPERATE_RET ret = board_bmi270_read_data(g_bmi270_dev, &raw); |
| 115 | + if (ret == OPRT_OK) { |
| 116 | + data->acc_x = raw.acc_x; |
| 117 | + data->acc_y = raw.acc_y; |
| 118 | + data->acc_z = raw.acc_z; |
| 119 | + data->gyr_x = raw.gyr_x; |
| 120 | + data->gyr_y = raw.gyr_y; |
| 121 | + data->gyr_z = raw.gyr_z; |
| 122 | + } |
| 123 | + return ret; |
| 124 | + } |
| 125 | + |
| 126 | + return OPRT_COM_ERROR; |
| 127 | +} |
| 128 | + |
| 129 | +/** |
| 130 | + * @brief Check if any IMU sensor is ready |
| 131 | + */ |
| 132 | +static bool imu_is_ready(void) |
| 133 | +{ |
| 134 | + return g_imu_type != IMU_TYPE_NONE; |
| 135 | +} |
| 136 | + |
76 | 137 | /*********************************************************** |
77 | 138 | ********************function declaration******************** |
78 | 139 | ***********************************************************/ |
@@ -965,23 +1026,38 @@ static void sand_init_particle(sand_particle_t *particle) |
965 | 1026 | } |
966 | 1027 |
|
967 | 1028 | /** |
968 | | - * @brief Update sand particle physics based on BMI270 sensor data |
| 1029 | + * @brief Update sand particle physics based on IMU sensor data |
969 | 1030 | */ |
970 | 1031 | static void sand_update_physics(void) |
971 | 1032 | { |
972 | | - bmi270_sensor_data_t sensor_data = {0}; |
| 1033 | + imu_sensor_data_t sensor_data = {0}; |
973 | 1034 | float acc_x = 0.0f, acc_y = 0.0f; |
974 | 1035 | float gyr_x = 0.0f, gyr_y = 0.0f; |
975 | 1036 |
|
976 | 1037 | // Read sensor data if available |
977 | | - if (g_bmi270_dev != NULL && board_bmi270_is_ready(g_bmi270_dev)) { |
978 | | - if (board_bmi270_read_data(g_bmi270_dev, &sensor_data) == OPRT_OK) { |
| 1038 | + static uint32_t imu_dbg_cnt = 0; |
| 1039 | + if (imu_is_ready()) { |
| 1040 | + OPERATE_RET imu_ret = imu_read_data(&sensor_data); |
| 1041 | + if (imu_ret == OPRT_OK) { |
979 | 1042 | acc_x = sensor_data.acc_x; |
980 | 1043 | acc_y = sensor_data.acc_y; |
981 | 1044 | // acc_z = sensor_data.acc_z; |
982 | 1045 | gyr_x = sensor_data.gyr_x; |
983 | 1046 | gyr_y = sensor_data.gyr_y; |
984 | 1047 | // gyr_z = sensor_data.gyr_z; |
| 1048 | + if (imu_dbg_cnt++ % 500 == 0) { |
| 1049 | + PR_NOTICE("IMU data: acc(%.2f, %.2f, %.2f) gyr(%.2f, %.2f, %.2f)", |
| 1050 | + sensor_data.acc_x, sensor_data.acc_y, sensor_data.acc_z, |
| 1051 | + sensor_data.gyr_x, sensor_data.gyr_y, sensor_data.gyr_z); |
| 1052 | + } |
| 1053 | + } else { |
| 1054 | + if (imu_dbg_cnt++ % 500 == 0) { |
| 1055 | + PR_ERR("IMU read failed: %d", imu_ret); |
| 1056 | + } |
| 1057 | + } |
| 1058 | + } else { |
| 1059 | + if (imu_dbg_cnt++ % 500 == 0) { |
| 1060 | + PR_WARN("IMU not ready, imu_type=%d", g_imu_type); |
985 | 1061 | } |
986 | 1062 | } |
987 | 1063 |
|
@@ -1441,28 +1517,54 @@ static void user_main(void) |
1441 | 1517 | } |
1442 | 1518 | PR_NOTICE("Hardware initialized"); |
1443 | 1519 |
|
1444 | | - // Initialize BMI270 sensor |
1445 | | - g_bmi270_dev = board_bmi270_get_handle(); |
1446 | | - if (g_bmi270_dev != NULL) { |
1447 | | - PR_NOTICE("BMI270 sensor handle obtained"); |
1448 | | - // Wait a bit for hardware registration to complete |
1449 | | - tal_system_sleep(200); |
1450 | | - |
1451 | | - // Check if sensor is ready |
1452 | | - if (!board_bmi270_is_ready(g_bmi270_dev)) { |
1453 | | - PR_NOTICE("Initializing BMI270 sensor..."); |
1454 | | - rt = board_bmi270_init(g_bmi270_dev); |
| 1520 | + // Initialize IMU sensor: try BMI220 first (no firmware upload), fallback to BMI270 |
| 1521 | + PR_NOTICE("Initializing IMU sensor..."); |
| 1522 | + tal_system_sleep(200); |
| 1523 | + |
| 1524 | + // Try BMI220 first (direct register access, no firmware upload needed) |
| 1525 | + g_bmi220_dev = board_bmi220_get_handle(); |
| 1526 | + if (g_bmi220_dev != NULL) { |
| 1527 | + PR_NOTICE("Trying BMI220 sensor..."); |
| 1528 | + if (!board_bmi220_is_ready(g_bmi220_dev)) { |
| 1529 | + rt = board_bmi220_init(g_bmi220_dev); |
1455 | 1530 | if (OPRT_OK == rt) { |
1456 | | - PR_NOTICE("BMI270 sensor initialized successfully"); |
| 1531 | + g_imu_type = IMU_TYPE_BMI220; |
| 1532 | + PR_NOTICE("BMI220 sensor initialized successfully"); |
1457 | 1533 | } else { |
1458 | | - PR_WARN("BMI270 sensor initialization failed: %d (will continue without sensor)", rt); |
1459 | | - g_bmi270_dev = NULL; |
| 1534 | + PR_WARN("BMI220 init failed: %d, trying BMI270...", rt); |
| 1535 | + g_bmi220_dev = NULL; |
1460 | 1536 | } |
1461 | 1537 | } else { |
1462 | | - PR_NOTICE("BMI270 sensor already initialized"); |
| 1538 | + g_imu_type = IMU_TYPE_BMI220; |
| 1539 | + PR_NOTICE("BMI220 sensor already initialized"); |
| 1540 | + } |
| 1541 | + } |
| 1542 | + |
| 1543 | + // Fallback to BMI270 if BMI220 failed |
| 1544 | + if (g_imu_type == IMU_TYPE_NONE) { |
| 1545 | + g_bmi270_dev = board_bmi270_get_handle(); |
| 1546 | + if (g_bmi270_dev != NULL) { |
| 1547 | + PR_NOTICE("Trying BMI270 sensor..."); |
| 1548 | + if (!board_bmi270_is_ready(g_bmi270_dev)) { |
| 1549 | + rt = board_bmi270_init(g_bmi270_dev); |
| 1550 | + if (OPRT_OK == rt) { |
| 1551 | + g_imu_type = IMU_TYPE_BMI270; |
| 1552 | + PR_NOTICE("BMI270 sensor initialized successfully"); |
| 1553 | + } else { |
| 1554 | + PR_WARN("BMI270 init failed: %d (continuing without IMU)", rt); |
| 1555 | + g_bmi270_dev = NULL; |
| 1556 | + } |
| 1557 | + } else { |
| 1558 | + g_imu_type = IMU_TYPE_BMI270; |
| 1559 | + PR_NOTICE("BMI270 sensor already initialized"); |
| 1560 | + } |
1463 | 1561 | } |
| 1562 | + } |
| 1563 | + |
| 1564 | + if (g_imu_type == IMU_TYPE_NONE) { |
| 1565 | + PR_WARN("No IMU sensor available (continuing without sensor)"); |
1464 | 1566 | } else { |
1465 | | - PR_WARN("BMI270 sensor not available (will continue without sensor)"); |
| 1567 | + PR_NOTICE("IMU active: %s", g_imu_type == IMU_TYPE_BMI220 ? "BMI220" : "BMI270"); |
1466 | 1568 | } |
1467 | 1569 |
|
1468 | 1570 | // Initialize buzzer |
|
0 commit comments