亚洲春色中文字幕久久久-三上亚,一吻二脱三床四吻胸,国产真实伦对白视频全集,在线毛片观看,精品成品入口黄网,国产毛aⅴ片久久久,亚洲AV色香蕉一区二区三区老师,萧皇后A级艳片,色情日本视频更新,99久久亚洲精品日本无码

標題: 小白MPU6050求助 求指點，到底是哪出問題了 [打印本頁]
作者: test_011    時間: 2026-5-1 20:43
標題: 小白MPU6050求助 求指點，到底是哪出問題了
利用正點原子舊版的MPU6050程序做了一個掃頻程序，想要得到PWM與姿態角的關系，但是角度出來問題很大，直接從30度內跑到100+，后面有附帶情況，附件的圖片讀取格式是PWM偏移量-角度信息-角速度信息，現在已經知道的是MPU6050直接運行例程是沒有問題的，非常靈敏。但是想辦法加入控制舵機并且讀取姿態角就出問題，試過很多辦法了，修改IIC速率減少影響，提高主程序到100Hz匹配DMP速率，但是都會出現這種情況。下面是我的程序，請大佬們過目！
  1. #include "led.h"
  2. #include "delay.h"
  3. #include "key.h"
  4. #include "sys.h"
  5. #include "lcd.h"
  6. #include "usart.h"
  7. #include "mpu6050.h"
  8. #include "usmart.h"   
  9. #include "inv_mpu.h"
  10. #include "inv_mpu_dmp_motion_driver.h"
  11. #include "pwm.h"
  12. #include <math.h>

  14. /* ========== 通道選擇 ========== */
  15. //#define SWEEP_CHANNEL_PITCH
  16. #define SWEEP_CHANNEL_ROLL

  18. /* 各通道中位值 */
  19. #define PITCH_CENTER   160
  20. #define ROLL_CENTER    150

  22. /* 舵機有效范圍 */
  23. #define SERVO_MIN  50
  24. #define SERVO_MAX  250

  26. /* 掃頻參數 */
  27. #define SWEEP_TOTAL      45.0f
  28. #define SWEEP_F_START    0.1f
  29. #define SWEEP_F_END      1.0f
  30. #define SWEEP_AMPLITUDE  40.0f
  31. #define SAMPLE_PERIOD_MS 50

  33. /* 系統延時對齊參數 */
  34. #define SYSTEM_DELAY_MS   100
  35. #define DELAY_BUF_LEN     20
  36. #define DELAY_STEPS       (SYSTEM_DELAY_MS / SAMPLE_PERIOD_MS)

  38. /*  陀螺儀量程換算  */
  39. // 正點原子默認 MPU_Set_Gyro_Fsr(3) → ±2000°/s → 靈敏度 16.4 LSB/(°/s)
  40. #define GYRO_SCALE 16.4f

  42. int main(void)
  43. {         
  44.     u8 t = 0;
  45.     float pitch, roll, yaw;
  46.     short aacx, aacy, aacz;
  47.     short gyrox, gyroy, gyroz;
  48.     short temp;

  50.     /* 掃頻專用變量 */
  51.     float t_sweep = 0.0f;
  52.     float dt = SAMPLE_PERIOD_MS / 1000.0f;
  53.     float phase = 0.0f;
  54.     float f_current;
  55.     float sweep_ratio;
  56.     float sin_val;
  57.     u16 pwm_pitch, pwm_roll;
  58.     int32_t signed_offset, pwm;

  60.     /* 延時對齊緩存 */
  61.     int pwm_buf[DELAY_BUF_LEN];
  62.     int buf_index = 0;
  63.     int aligned_pwm;
  64.     int delayed_index;
  65.     int i;

  67.     /* 輸出變量 */
  68.     int pwm_offset;
  69.     float output_angle;
  70.     float output_rate;   // ★ 新增：輸出角速度

  72.     /* LCD背光 GPIO 初始化結構體 */
  73.     GPIO_InitTypeDef GPIO_InitStructure;

  75.     /* ---------- 初始化 PWM 延時緩存 ---------- */
  76.     for(i = 0; i < DELAY_BUF_LEN; i++) {
  77.         pwm_buf[i] = 0;
  78.     }

  80.     /* ---------- 系統初始化 ---------- */
  81.     NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
  82.     uart_init(500000);
  83.     delay_init();
  84.     usmart_dev.init(72);
  85.     LED_Init();
  86.     KEY_Init();
  87.     LCD_Init();
  88.     MPU_Init();

  90.     MPU_Set_Gyro_Fsr(3);   // 強制陀螺儀量程為 ±2000°/s, 與 GYRO_SCALE 一致

  92.     /* PWM初始化（50Hz） */
  93.     TIM3_PWM_Init(1999, 719);
  94.     TIM_SetCompare1(TIM3, ROLL_CENTER);
  95.     TIM_SetCompare2(TIM3, PITCH_CENTER);
  96.     TIM_SetCompare4(TIM3, 150);

  98.     /* LCD背光修復 */
  99.     RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
  100.     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
  101.     GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
  102.     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  103.     GPIO_Init(GPIOB, &GPIO_InitStructure);
  104.     GPIO_SetBits(GPIOB, GPIO_Pin_0);

  106.     /* LCD 顯示 */
  107.     POINT_COLOR = RED;
  108.     LCD_ShowString(30,50,200,16,16,"ELITE STM32");      
  109.     LCD_ShowString(30,70,200,16,16,"MPU6050 SWEEP");      
  110.     LCD_ShowString(30,90,200,16,16,"ATOM@ALIENTEK");
  111.     LCD_ShowString(30,110,200,16,16,"2026/4/28");

  113.     /* DMP初始化 */
  114.     while(mpu_dmp_init())
  115.     {
  116.         LCD_ShowString(30,130,200,16,16,"MPU6050 Error");
  117.         delay_ms(200);
  118.         LCD_Fill(30,130,239,130+16,WHITE);
  119.         delay_ms(200);
  120.     }  
  121.     LCD_ShowString(30,130,200,16,16,"MPU6050 OK");

  123. #ifdef SWEEP_CHANNEL_PITCH
  124.     LCD_ShowString(30,150,200,16,16,"Pitch Sweep");
  125. #elif defined(SWEEP_CHANNEL_ROLL)
  126.     LCD_ShowString(30,150,200,16,16,"Roll Sweep");
  127. #endif

  129.     POINT_COLOR = BLUE;
  130.     LCD_ShowString(30,170,200,16,16,"Sweeping...    ");         
  131.     LCD_ShowString(30,200,200,16,16," Temp:    . C");      
  132.     LCD_ShowString(30,220,200,16,16,"Pitch:    . C");      
  133.     LCD_ShowString(30,240,200,16,16," Roll:    . C");         
  134.     LCD_ShowString(30,260,200,16,16," Yaw :    . C");         

  136.     delay_ms(1000);

  138.     /* ---------- 主循環：相位連續掃頻 ---------- */
  139.     while(1)
  140.     {
  141.         /* 1. 固定非激活通道 */
  142. #ifdef SWEEP_CHANNEL_PITCH
  143.         TIM_SetCompare1(TIM3, ROLL_CENTER);
  144.         TIM_SetCompare4(TIM3, 150);
  145. #elif defined(SWEEP_CHANNEL_ROLL)
  146.         TIM_SetCompare2(TIM3, PITCH_CENTER);
  147.         TIM_SetCompare4(TIM3, 150);
  148. #endif

  150.         /* 2. 計算當前頻率 */
  151.         sweep_ratio = t_sweep / SWEEP_TOTAL;
  152.         if(sweep_ratio > 1.0f) sweep_ratio = 1.0f;
  153.         f_current = SWEEP_F_START + (SWEEP_F_END - SWEEP_F_START) * sweep_ratio;

  155.         /* 3. 相位連續的chirp信號 */
  156.         phase += 2.0f * 3.1415926f * f_current * dt;
  157.         sin_val = sinf(phase);
  158.         signed_offset = (int32_t)(SWEEP_AMPLITUDE * sin_val);

  160.         /* 4. PWM輸出 */
  161. #ifdef SWEEP_CHANNEL_PITCH
  162.         pwm = PITCH_CENTER + signed_offset;
  163.         if(pwm < SERVO_MIN) pwm = SERVO_MIN;
  164.         if(pwm > SERVO_MAX) pwm = SERVO_MAX;
  165.         TIM_SetCompare2(TIM3, (u16)pwm);
  166. #elif defined(SWEEP_CHANNEL_ROLL)
  167.         pwm = ROLL_CENTER + signed_offset;
  168.         if(pwm < SERVO_MIN) pwm = SERVO_MIN;
  169.         if(pwm > SERVO_MAX) pwm = SERVO_MAX;
  170.         TIM_SetCompare1(TIM3, (u16)pwm);
  171. #endif

  173.         /* 5. 存入PWM緩存（環形緩沖區） */
  174.         pwm_buf[buf_index] = (int)signed_offset;

  176.         /* 6. 獲取延時對齊后的PWM偏移量 */
  177.         delayed_index = buf_index - DELAY_STEPS;
  178.         if(delayed_index < 0) delayed_index += DELAY_BUF_LEN;
  179.         aligned_pwm = pwm_buf[delayed_index];

  181.         buf_index++;
  182.         if(buf_index >= DELAY_BUF_LEN) buf_index = 0;

  184.         /* 7. 讀取IMU并輸出對齊后的數據（包含角速度） */
  185.         if(mpu_dmp_get_data(&pitch, &roll, &yaw) == 0)
  186.         {
  187.             temp = MPU_Get_Temperature();
  188.             MPU_Get_Accelerometer(&aacx, &aacy, &aacz);
  189.             MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz);

  191.             // 根據實際軸映射，Pitch 用 gyro_y，Roll 用 gyro_x
  192. #ifdef SWEEP_CHANNEL_PITCH
  193.             output_angle = pitch;
  194.             output_rate  = gyroy / GYRO_SCALE;   // 對應 Pitch 軸角速度
  195. #elif defined(SWEEP_CHANNEL_ROLL)
  196.             output_angle = roll;
  197.             output_rate  = gyrox / GYRO_SCALE;   // 對應 Roll 軸角速度
  198. #endif

  200.             /* ? 核心輸出：對齊后的 PWM偏移, 角度, 角速度 */
  201.             printf("%d, %.4f, %.2f\r\n", aligned_pwm, output_angle, output_rate);

  203.             /* LCD 刷新（每10次循環一次） */
  204.             if((t % 10) == 0)
  205.             {
  206.                 if(temp < 0) { LCD_ShowChar(30+48,200,'-',16,0); temp = -temp; }
  207.                 else LCD_ShowChar(30+48,200,' ',16,0);
  208.                 LCD_ShowNum(30+48+8,200,temp/100,3,16);
  209.                 LCD_ShowNum(30+48+40,200,temp%10,1,16);

  211.                 if(pitch < 0) { LCD_ShowChar(30+48,220,'-',16,0); pitch = -pitch; }
  212.                 else LCD_ShowChar(30+48,220,' ',16,0);
  213.                 LCD_ShowNum(30+48+8,220,(int)(pitch*10)/10,3,16);
  214.                 LCD_ShowNum(30+48+40,220,(int)(pitch*10)%10,1,16);

  216.                 if(roll < 0) { LCD_ShowChar(30+48,240,'-',16,0); roll = -roll; }
  217.                 else LCD_ShowChar(30+48,240,' ',16,0);
  218.                 LCD_ShowNum(30+48+8,240,(int)(roll*10)/10,3,16);
  219.                 LCD_ShowNum(30+48+40,240,(int)(roll*10)%10,1,16);

  221.                 if(yaw < 0) { LCD_ShowChar(30+48,260,'-',16,0); yaw = -yaw; }
  222.                 else LCD_ShowChar(30+48,260,' ',16,0);
  223.                 LCD_ShowNum(30+48+8,260,(int)(yaw*10)/10,3,16);
  224.                 LCD_ShowNum(30+48+40,260,(int)(yaw*10)%10,1,16);

  226.                 t = 0;
  227.                 LED0 = !LED0;
  228.             }
  229.         }
  230.         t++;

  232.         delay_ms(SAMPLE_PERIOD_MS);
  233.         t_sweep += dt;

  235.         /* 掃頻結束，舵機歸中 */
  236.         if(t_sweep >= SWEEP_TOTAL)
  237.         {
  238. #ifdef SWEEP_CHANNEL_PITCH
  239.             TIM_SetCompare2(TIM3, PITCH_CENTER);
  240. #elif defined(SWEEP_CHANNEL_ROLL)
  241.             TIM_SetCompare1(TIM3, ROLL_CENTER);
  242. #endif
  243.             LCD_ShowString(30,170,200,16,16,"Sweep Finished!");
  244.             while(1) { LED0 = !LED0; delay_ms(200); }
  245.         }
  246.     }        
  247. }
復制代碼

90CF34F9C4BF223B0E9DE8CAAAADBF73.png (32.22 KB, 下載次數: 0)

90CF34F9C4BF223B0E9DE8CAAAADBF73.png

AF1E4A097B731E8874CA16F68938AEF4.png (40.68 KB, 下載次數: 0)

AF1E4A097B731E8874CA16F68938AEF4.png




歡迎光臨 (http://www.denmoz.com/bbs/) Powered by Discuz! X3.1