first

2025-10-29 13:10:02 +08:00
commit 49a07fa419
2284 changed files with 642060 additions and 0 deletions
--- a/apps/common/device/gSensor/mpu6050.c
+++ b/apps/common/device/gSensor/mpu6050.c
@ -0,0 +1,134 @@
+#include "gSensor/mpu6050.h"
+#include "gSensor/gSensor_manage.h"
+#include "app_config.h"
+#include "imuSensor_manage.h"
+
+#if TCFG_MPU6050_EN
+
+int gsensorlen = 32;
+OS_MUTEX SENSOR_IIC_MUTEX;
+spinlock_t sensor_iic;
+u8 sensor_iic_init_status = 0;
+
+#define ACCEL_ONLY_LOW_POWER    0
+
+u8 mpu6050_register_read(u8 addr, u8 *data)
+{
+    _gravity_sensor_get_ndata(I2C_ADDR_MPU6050_R, addr, data, 1);
+    return 0;
+}
+
+u8 mpu6050_register_write(u8 addr, u8 data)
+{
+    gravity_sensor_command(I2C_ADDR_MPU6050_W, addr, data);
+    return 0;
+}
+
+u8 mpu6050_read_nbyte_data(u8 addr, u8 *data, u8 len)
+{
+    return _gravity_sensor_get_ndata(I2C_ADDR_MPU6050_R, addr, data, len);
+}
+
+u8 mpu6050_get_xyz_data(u8 addr, void *xyz_data)
+{
+    u8 buf[6], read_len;
+    axis_info_t *data = (axis_info_t *)xyz_data;
+    read_len = mpu6050_read_nbyte_data(addr, buf, 6);
+    if (read_len == 6) {
+        data->x = ((u16)buf[0] << 8) | buf[1];
+        data->y = ((u16)buf[2] << 8) | buf[3];
+        data->z = ((u16)buf[4] << 8) | buf[5];
+
+        data->x = 2 * ACCEL_OF_GRAVITY * ACCEL_DATA_GAIN * data->x / 32768;
+        data->y = 2 * ACCEL_OF_GRAVITY * ACCEL_DATA_GAIN * data->y / 32768;
+        data->z = 2 * ACCEL_OF_GRAVITY * ACCEL_DATA_GAIN * data->z / 32768;
+    }
+    return read_len;
+}
+
+extern void step_cal_init();
+extern int step_cal();
+u8 mpu6050_init()
+{
+    u8 res = 0;
+    u8 data;
+    res = mpu6050_register_read(MPU6050_RA_WHO_AM_I, &data);               //读ID
+    if (data == MPU_ADDR) {
+        g_printf("read MPU6050 ID suss");
+    } else {
+        g_printf("read MPU6050 ID err");
+        return -1;
+    }
+
+    data = 0x80;
+    mpu6050_register_write(MPU6050_RA_PWR_MGMT_1, data);                   //复位
+    os_time_dly(10);
+
+    data = 0x00;
+    data |= BIT(3);                                                        //关闭温度传感器
+#if ACCEL_ONLY_LOW_POWER
+    data |= BIT(5);                                                        //设置accel-only低功耗模式
+#endif
+    mpu6050_register_write(MPU6050_RA_PWR_MGMT_1, data);                   //退出休眠
+
+    data = 0x00;
+    data |= (BIT(0) | BIT(1) | BIT(2));                                    //关闭陀螺仪
+#if ACCEL_ONLY_LOW_POWER
+    data |= (3 << 6);                                                      //accel-only低功耗模式下，唤醒频率为40Hz
+#endif
+    /*mpu6050_register_write(MPU6050_RA_PWR_MGMT_2, data);*/
+
+    data = ACCEL_RANGE_2G << 3;
+    mpu6050_register_write(MPU6050_RA_ACCEL_CONFIG, data);                 //加速度计量程
+
+#if (!ACCEL_ONLY_LOW_POWER)
+    data = 0x06;
+    mpu6050_register_write(MPU6050_RA_CONFIG, data);                       //设置陀螺仪输出速率
+
+    data = MPU6050_GYRO_OUT_RATE / MPU6050_SAMPLE_RATE - 1;
+    mpu6050_register_write(MPU6050_RA_SMPLRT_DIV, data);                   //设置采样率, 采样率=陀螺仪输出速率/(1+SMPLRT_DIV)
+
+    data = 0x00;
+    mpu6050_register_write(MPU6050_RA_INT_ENABLE, data);                   //关闭中断
+
+    data = 0x00;
+    mpu6050_register_write(MPU6050_RA_USER_CTRL, data);                    //关闭主机IIC
+#endif
+
+    /*step_cal_init();*/
+    /*sys_timer_add(NULL, step_cal, 50);*/
+    return 0;
+}
+
+void mpu6050_ctl(u8 cmd, void *arg)
+{
+    switch (cmd) {
+    case GET_ACCEL_DATA:
+    case IMU_GET_ACCEL_DATA:
+        mpu6050_get_xyz_data(MPU6050_RA_ACCEL_XOUT_H, arg);
+        break;
+    case IMU_GET_GYRO_DATA:
+        mpu6050_get_xyz_data(MPU6050_RA_GYRO_XOUT_H, arg);
+        break;
+    case READ_GSENSOR_DATA:
+
+        break;
+    }
+}
+
+REGISTER_GRAVITY_SENSOR(gSensor) = {
+    .logo = "mpu6050",
+    .gravity_sensor_init  = mpu6050_init,
+    .gravity_sensor_check = NULL,
+    .gravity_sensor_ctl   = mpu6050_ctl,
+};
+
+//未合到imu: init:gravity_sensor_init(&motion_sensor_data);
+REGISTER_IMU_SENSOR(mpu6050_sensor) = {
+    .logo = "mpu6050",
+    .imu_sensor_init = mpu6050_init,
+    .imu_sensor_check = NULL,
+    .imu_sensor_ctl = mpu6050_ctl,
+};
+
+#endif //TCFG_MPU6050_EN