控制mpu6050失败！

一、描述你遇到的问题

是硬件问题还是程序问题，求解答

二、你具体做的所有步骤结果截图

#include "pinctrl.h"
#include "gpio.h"
#include "pwm.h"
#include "osal_debug.h"
#include "cmsis_os2.h"
#include "app_init.h"
#include "tcxo.h"
#include "watchdog.h"
#include "i2c.h"
#include <stddef.h>

/* ----- 引脚定义 ----- */
#define PWMA_PIN 0
#define AIN1_PIN 10
#define AIN2_PIN 9
#define PWMB_PIN 1
#define BIN1_PIN 11
#define BIN2_PIN 12

#define LEFT_ENCODER_A_PIN 2
#define LEFT_ENCODER_B_PIN 3
#define RIGHT_ENCODER_A_PIN 4
#define RIGHT_ENCODER_B_PIN 5

#define MPU_SCL_PIN 16
#define MPU_SDA_PIN 15
#define MPU_I2C_BUS 1
#define MPU_ADDR 0x68

/* ----- PWM 配置 (30% 占空比) ----- */
#define PWM_CHANNEL_A 0
#define PWM_CHANNEL_B 1
#define PWM_GROUP_ID 0
#define PWM_PERIOD 400
#define PWM_MAX 380

pwm_config_t pwm_cfg_a = { 280, 120, 0, 0, false };
pwm_config_t pwm_cfg_b = { 280, 120, 0, 0, false };

/* ----- 任务参数 ----- */
#define MOTOR_TASK_STACK 0x1000
#define MOTOR_TASK_PRIO (osPriority_t)(17)
#define ENCODER_TASK_STACK 0x2000
#define ENCODER_TASK_PRIO (osPriority_t)(17)
#define I2C_CHECK_TASK_STACK 0x800
#define I2C_CHECK_TASK_PRIO (osPriority_t)(osPriorityLow)
#define IMU_TASK_STACK 0x1000
#define IMU_TASK_PRIO (osPriority_t)(osPriorityAboveNormal)
#define CONTROL_TASK_STACK 0x1000
#define CONTROL_TASK_PRIO (osPriority_t)(osPriorityNormal)

/* ----- 编码器参数 ----- */
#define PULSES_PER_REV 11
#define WHL_DIAM_CM 65
#define PI_VAL 314159
#define WHEEL_CIRC ((PI_VAL * WHL_DIAM_CM) / 100000)
#define SPEED_CALC_MS 500
#define DEBOUNCE_US 5000

/* ----- 整数姿态参数 ----- /
#define GYRO_SCALE 164 / LSB/(deg/s) * 10 /
#define ACCEL_SENS 4096 / LSB/g /
#define ALPHA_FIX 98 / 互补滤波系数 (0.98 * 100) /
#define DT_MS 10 / 姿态更新周期 ms */

/* ----- PID 参数 (初始值，可调整) ----- /
#define ANGLE_KP 20
#define ANGLE_KD 1
#define SPEED_KP 5
#define SPEED_KI 1
#define TARGET_ANGLE 0 / 平衡角度，度（整数） */

/* ----- 全局变量 ----- /
/ 编码器 /
static volatile int32_t g_left_pulses = 0;
static volatile int32_t g_right_pulses = 0;
static int32_t g_left_last = 0, g_right_last = 0;
static int32_t g_left_spd = 0, g_right_spd = 0; / cm/s */
static volatile uint64_t g_left_last_us = 0;
static volatile uint64_t g_right_last_us = 0;

/* 姿态 /
static volatile int32_t g_angle_roll = 0; / 放大 100 倍 /
static volatile int32_t g_angle_pitch = 0;
static volatile int32_t g_gyro_y = 0; / 放大 10 倍 */
static volatile bool g_imu_ready = false;

/* PID 控制器 (简单结构，使用整数) */
typedef struct {
int32_t Kp, Ki, Kd;
int32_t integral;
int32_t last_error;
int32_t out_max;
} pid_t;

/* ----- 编码器中断服务函数 ----- */
static void left_encoder_isr(pin_t pin, uintptr_t param)
{
(void)pin; (void)param;
uint64_t now = uapi_tcxo_get_us();
if (now - g_left_last_us < DEBOUNCE_US) return;
g_left_last_us = now;
if (uapi_gpio_get_val(LEFT_ENCODER_B_PIN) == GPIO_LEVEL_LOW) g_left_pulses++;
else g_left_pulses--;
}

static void right_encoder_isr(pin_t pin, uintptr_t param)
{
(void)pin; (void)param;
uint64_t now = uapi_tcxo_get_us();
if (now - g_right_last_us < DEBOUNCE_US) return;
g_right_last_us = now;
if (uapi_gpio_get_val(RIGHT_ENCODER_B_PIN) == GPIO_LEVEL_LOW) g_right_pulses--;
else g_right_pulses++;
}

/* ----- 编码器初始化 ----- */
static void encoder_init(void)
{
uapi_pin_set_mode(LEFT_ENCODER_A_PIN, PIN_MODE_0);
uapi_pin_set_mode(LEFT_ENCODER_B_PIN, PIN_MODE_0);
uapi_pin_set_mode(RIGHT_ENCODER_A_PIN, PIN_MODE_2);
uapi_pin_set_mode(RIGHT_ENCODER_B_PIN, PIN_MODE_4);
uapi_gpio_set_dir(LEFT_ENCODER_A_PIN, GPIO_DIRECTION_INPUT);
uapi_gpio_set_dir(LEFT_ENCODER_B_PIN, GPIO_DIRECTION_INPUT);
uapi_gpio_set_dir(RIGHT_ENCODER_A_PIN, GPIO_DIRECTION_INPUT);
uapi_gpio_set_dir(RIGHT_ENCODER_B_PIN, GPIO_DIRECTION_INPUT);
uapi_gpio_register_isr_func(LEFT_ENCODER_A_PIN, GPIO_INTERRUPT_RISING_EDGE, left_encoder_isr);
uapi_gpio_register_isr_func(RIGHT_ENCODER_A_PIN, GPIO_INTERRUPT_RISING_EDGE, right_encoder_isr);
uapi_gpio_enable_interrupt(LEFT_ENCODER_A_PIN);
uapi_gpio_enable_interrupt(RIGHT_ENCODER_A_PIN);
}

/* ----- 速度计算 (整数) ----- */
static void calc_speeds(void)
{
int32_t ld = g_left_pulses - g_left_last;
int32_t rd = g_right_pulses - g_right_last;
g_left_last = g_left_pulses;
g_right_last = g_right_pulses;
g_left_spd = (ld * WHEEL_CIRC * 1000) / (PULSES_PER_REV * SPEED_CALC_MS);
g_right_spd = (rd * WHEEL_CIRC * 1000) / (PULSES_PER_REV * SPEED_CALC_MS);
}

/* ----- 电机控制 ----- */
static void motor_init(void)
{
uapi_pin_set_mode(AIN1_PIN, PIN_MODE_0);
uapi_pin_set_mode(AIN2_PIN, PIN_MODE_0);
uapi_pin_set_mode(BIN1_PIN, PIN_MODE_0);
uapi_pin_set_mode(BIN2_PIN, PIN_MODE_0);
uapi_gpio_set_dir(AIN1_PIN, GPIO_DIRECTION_OUTPUT);
uapi_gpio_set_dir(AIN2_PIN, GPIO_DIRECTION_OUTPUT);
uapi_gpio_set_dir(BIN1_PIN, GPIO_DIRECTION_OUTPUT);
uapi_gpio_set_dir(BIN2_PIN, GPIO_DIRECTION_OUTPUT);

uapi_pin_set_mode(PWMA_PIN, PIN_MODE_1);
uapi_pin_set_mode(PWMB_PIN, PIN_MODE_1);
uapi_pwm_init();
uapi_pwm_open(PWM_CHANNEL_A, &pwm_cfg_a);
uapi_pwm_open(PWM_CHANNEL_B, &pwm_cfg_b);

uapi_gpio_set_val(AIN1_PIN, GPIO_LEVEL_HIGH);
uapi_gpio_set_val(AIN2_PIN, GPIO_LEVEL_LOW);
uapi_gpio_set_val(BIN1_PIN, GPIO_LEVEL_HIGH);
uapi_gpio_set_val(BIN2_PIN, GPIO_LEVEL_LOW);
uint8_t ch[] = {PWM_CHANNEL_A, PWM_CHANNEL_B};
uapi_pwm_set_group(PWM_GROUP_ID, ch, 2);
uapi_pwm_start_group(PWM_GROUP_ID);

}

/* 设置电机 PWM (整数值) */
static void set_motor_pwm(int32_t left, int32_t right)
{
if (left > PWM_MAX) left = PWM_MAX;
if (left < -PWM_MAX) left = -PWM_MAX;
if (right > PWM_MAX) right = PWM_MAX;
if (right < -PWM_MAX) right = -PWM_MAX;

if (left >= 0) {
    uapi_gpio_set_val(AIN1_PIN, GPIO_LEVEL_HIGH);
    uapi_gpio_set_val(AIN2_PIN, GPIO_LEVEL_LOW);
    pwm_cfg_a.low_time  = PWM_PERIOD - left;
    pwm_cfg_a.high_time = left;
} else {
    uapi_gpio_set_val(AIN1_PIN, GPIO_LEVEL_LOW);
    uapi_gpio_set_val(AIN2_PIN, GPIO_LEVEL_HIGH);
    left = -left;
    pwm_cfg_a.low_time  = PWM_PERIOD - left;
    pwm_cfg_a.high_time = left;
}
if (right >= 0) {
    uapi_gpio_set_val(BIN1_PIN, GPIO_LEVEL_HIGH);
    uapi_gpio_set_val(BIN2_PIN, GPIO_LEVEL_LOW);
    pwm_cfg_b.low_time  = PWM_PERIOD - right;
    pwm_cfg_b.high_time = right;
} else {
    uapi_gpio_set_val(BIN1_PIN, GPIO_LEVEL_LOW);
    uapi_gpio_set_val(BIN2_PIN, GPIO_LEVEL_HIGH);
    right = -right;
    pwm_cfg_b.low_time  = PWM_PERIOD - right;
    pwm_cfg_b.high_time = right;
}
uapi_pwm_close(PWM_CHANNEL_A);
uapi_pwm_close(PWM_CHANNEL_B);
uapi_pwm_open(PWM_CHANNEL_A, &pwm_cfg_a);
uapi_pwm_open(PWM_CHANNEL_B, &pwm_cfg_b);
uint8_t ch[] = {PWM_CHANNEL_A, PWM_CHANNEL_B};
uapi_pwm_set_group(PWM_GROUP_ID, ch, 2);
uapi_pwm_start_group(PWM_GROUP_ID);

}

/* ----- PID 控制器 (整数) ----- */
static void pid_init(pid_t *p, int32_t kp, int32_t ki, int32_t kd, int32_t out_max)
{
p->Kp = kp; p->Ki = ki; p->Kd = kd;
p->integral = 0; p->last_error = 0;
p->out_max = out_max;
}

static int32_t pid_compute(pid_t *p, int32_t target, int32_t current)
{
int32_t error = target - current;
p->integral += error;
if (p->integral > p->out_max) p->integral = p->out_max;
if (p->integral < -p->out_max) p->integral = -p->out_max;
int32_t derivative = error - p->last_error;
p->last_error = error;
int32_t out = (p->Kp * error + p->Ki * p->integral + p->Kd * derivative) / 100;
if (out > p->out_max) out = p->out_max;
if (out < -p->out_max) out = -p->out_max;
return out;
}

/* ----- 任务函数 ----- /
/ 电机任务 (保持运行并喂狗) */
static void motor_task(void *arg)
{
(void)arg;
motor_init();
osal_printk("[Motor] Running\r\n");
while (1) {
uapi_tcxo_delay_ms(1000);
uapi_watchdog_kick();
}
}

/* 编码器任务 */
static void encoder_task(void *arg)
{
(void)arg;
encoder_init();
osal_printk("[Encoder] Started\r\n");
while (1) {
uapi_tcxo_delay_ms(SPEED_CALC_MS);
calc_speeds();
osal_printk("[Speed] L=%d cm/s, R=%d cm/s | P L=%d R=%d\r\n",
g_left_spd, g_right_spd, g_left_pulses, g_right_pulses);
}
}

/* I2C 检测任务 (只读 WHO_AM_I，不阻塞系统) */
static void i2c_check_task(void *arg)
{
(void)arg;
uapi_pin_set_mode(MPU_SCL_PIN, 2);
uapi_pin_set_mode(MPU_SDA_PIN, 2);
uapi_pin_set_pull(MPU_SCL_PIN, PIN_PULL_TYPE_UP);
uapi_pin_set_pull(MPU_SDA_PIN, PIN_PULL_TYPE_UP);
uapi_i2c_master_init(MPU_I2C_BUS, 400000, 0x0);
uapi_tcxo_delay_ms(200);

uint8_t who = 0;
i2c_data_t data = { .send_buf = (uint8_t[]){0x75}, .send_len = 1,
                    .receive_buf = &who, .receive_len = 1 };
errcode_t ret = uapi_i2c_master_read(MPU_I2C_BUS, MPU_ADDR, &data);
if (ret == ERRCODE_SUCC && who == 0x68) {
    osal_printk("[I2C] MPU6050 found (WHO=0x68)\r\n");
    g_imu_ready = true;
} else {
    osal_printk("[I2C] MPU6050 not found (ret=%d, who=0x%02X)\r\n", ret, who);
}
while (1) {
    uapi_tcxo_delay_ms(5000);
    if (!g_imu_ready) {
        ret = uapi_i2c_master_read(MPU_I2C_BUS, MPU_ADDR, &data);
        if (ret == ERRCODE_SUCC && who == 0x68) {
            osal_printk("[I2C] MPU6050 recovered\r\n");
            g_imu_ready = true;
        }
    }
}

}

/* IMU 任务 (整数互补滤波) */
static void imu_task(void *arg)
{
(void)arg;
while (!g_imu_ready) { uapi_tcxo_delay_ms(100); }
osal_printk("[IMU] Started (integer fusion)\r\n");

int32_t ax_raw, ay_raw, az_raw, gx_raw, gy_raw;
uint8_t rx[14];
i2c_data_t data = { .send_buf = (uint8_t[]){0x3B}, .send_len = 1,
                    .receive_buf = rx, .receive_len = 14 };
int32_t ang_x = 0, ang_y = 0;
uint32_t cnt = 0;

while (1) {
    uapi_tcxo_delay_ms(DT_MS);
    if (uapi_i2c_master_read(MPU_I2C_BUS, MPU_ADDR, &data) != ERRCODE_SUCC) continue;

    ax_raw = (int16_t)(rx[0]<<8 | rx[1]);
    ay_raw = (int16_t)(rx[2]<<8 | rx[3]);
    az_raw = (int16_t)(rx[4]<<8 | rx[5]);
    gx_raw = (int16_t)(rx[8]<<8 | rx[9]);
    gy_raw = (int16_t)(rx[10]<<8 | rx[11]);

    /* 加速度计角度 (整数近似) */
    int32_t acc_x = (ay_raw * 100) / (az_raw + 1);
    int32_t acc_y = (-ax_raw * 100) / (az_raw + 1);

    /* 陀螺仪积分 (单位: 0.01 度) */
    ang_x += (gx_raw * DT_MS * 10) / GYRO_SCALE;
    ang_y += (gy_raw * DT_MS * 10) / GYRO_SCALE;

    /* 互补滤波 */
    ang_x = (ALPHA_FIX * ang_x + (100 - ALPHA_FIX) * acc_x) / 100;
    ang_y = (ALPHA_FIX * ang_y + (100 - ALPHA_FIX) * acc_y) / 100;

    g_angle_roll  = ang_x;
    g_angle_pitch = ang_y;
    g_gyro_y      = gy_raw * 10 / GYRO_SCALE;  /* 角速度 (0.1 deg/s) */

    if (++cnt >= (1000 / DT_MS)) { /* 每秒打印一次 */
        cnt = 0;
        osal_printk("[IMU] Roll=%d.%02d deg, Pitch=%d.%02d deg\r\n",
                    ang_x/100, abs(ang_x%100), ang_y/100, abs(ang_y%100));
    }
}

}

/* 控制任务 (PID) */
static void control_task(void *arg)
{
(void)arg;
while (!g_imu_ready) { uapi_tcxo_delay_ms(100); }
osal_printk("[Control] PID started\r\n");

pid_t pid_angle, pid_speed;
pid_init(&pid_angle, ANGLE_KP, 0, ANGLE_KD, PWM_MAX);
pid_init(&pid_speed, SPEED_KP, SPEED_KI, 0, PWM_MAX);

while (1) {
    uapi_tcxo_delay_ms(10);
    int32_t angle_cur = g_angle_roll / 100;  /* 转换为度 */
    int32_t spd_cur   = (g_left_spd + g_right_spd) / 2;

    int32_t angle_out = pid_compute(&pid_angle, TARGET_ANGLE, angle_cur);
    int32_t spd_out   = pid_compute(&pid_speed, 0, spd_cur);

    int32_t left_pwm  = spd_out + angle_out;
    int32_t right_pwm = spd_out + angle_out;

    set_motor_pwm(left_pwm, right_pwm);
}

}

/* ----- 应用入口 ----- */
static void car_entry(void)
{
osal_printk("\r\n========== Self-Balancing Car (Int) ==========\r\n");
osThreadAttr_t attr = {0};

attr.name = "MotorTask";
attr.stack_size = MOTOR_TASK_STACK;
attr.priority = MOTOR_TASK_PRIO;
osThreadNew(motor_task, NULL, &attr);

attr.name = "EncoderTask";
attr.stack_size = ENCODER_TASK_STACK;
attr.priority = ENCODER_TASK_PRIO;
osThreadNew(encoder_task, NULL, &attr);

attr.name = "I2CCheck";
attr.stack_size = I2C_CHECK_TASK_STACK;
attr.priority = I2C_CHECK_TASK_PRIO;
osThreadNew(i2c_check_task, NULL, &attr);

attr.name = "IMUTask";
attr.stack_size = IMU_TASK_STACK;
attr.priority = IMU_TASK_PRIO;
osThreadNew(imu_task, NULL, &attr);

attr.name = "ControlTask";
attr.stack_size = CONTROL_TASK_STACK;
attr.priority = CONTROL_TASK_PRIO;
osThreadNew(control_task, NULL, &attr);

osal_printk("System Ready\r\n");

}

app_run(car_entry);

三、当前开发板状态全景照片

请插入图片

四、开发板串口所有日志

``oxo update temp:3,diff:0,xo:0x30000
boot.
Flash Init Fail! ret = 0x80001341
verify_public_rootkey secure verify disable!
verify_params_key_area secure verify disable!
verify_params_area_info secure verify disable!
verify_image_key_area secure verify disable!
verify_image_code_info secure verify disable!
SSB Uart Init Succ!
SSB Flash Init Succ!
verify_image_key_area secure verify disable!
verify_image_code_info secure verify disable!
Flashboot Uart Init Succ!
Flashboot Malloc Init Succ!
Flash Init Succ!
No need to fix SR!
io level work in hw mode, level[chip]:0x33
flashboot version : 1.10.102
[UPG] upgrade init OK!
No need to upgrade...
flash_encrypt disable.
verify_image_key_area secure verify disable!
verify_image_code_info secure verify disable!
APP|dbg uart init ok.
[UPG] upgrade init OK!
APP|init_sle_mac, mac_addr:0x50,0x5c,0x11,0x b,0x**,0x**,
APP|init_dev_addr, mac_addr:0x50,0x5c,0x11,0x94,0x**,0x**,
xo_trim_temp_comp val:0 0
los_at_plt_cmd_register EXCUTE
APP|thread[11] func null

========== Self-Balancing Car (Int) ==========
[ERR] osThreadNew Fail, NOT in adapt priority range: [osPriorityLow3 : osPriorityHigh].
System Ready
cpu 0 entering scheduler
APP|btc open
xo update temp:3,diff:0,xo:0x30000