利用正点原子旧版的MPU6050程序做了一个扫频程序，想要得到PWM与姿态角的关系，但是角度出来问题很大，直接从30度内跑到100+，后面有附带情况，附件的图片读取格式是PWM偏移量-角度信息-角速度信息，现在已经知道的是MPU6050直接运行例程是没有问题的，非常灵敏。但是想办法加入控制舵机并且读取姿态角就出问题，试过很多办法了，修改IIC速率减少影响，提高主程序到100Hz匹配DMP速率，但是都会出现这种情况。下面是我的程序，请大佬们过目！#include "led.h"#include "delay.h"
#include "key.h"
#include "sys.h"
#include "lcd.h"
#include "usart.h"
#include "mpu6050.h"
#include "usmart.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "pwm.h"
#include <math.h>
/* ========== 通道选择 ========== */
//#define SWEEP_CHANNEL_PITCH
#define SWEEP_CHANNEL_ROLL
/* 各通道中位值 */
#define PITCH_CENTER 160
#define ROLL_CENTER 150
/* 舵机有效范围 */
#define SERVO_MIN 50
#define SERVO_MAX 250
/* 扫频参数 */
#define SWEEP_TOTAL 45.0f
#define SWEEP_F_START 0.1f
#define SWEEP_F_END 1.0f
#define SWEEP_AMPLITUDE 40.0f
#define SAMPLE_PERIOD_MS 50
/* 系统延时对齐参数 */
#define SYSTEM_DELAY_MS 100
#define DELAY_BUF_LEN 20
#define DELAY_STEPS (SYSTEM_DELAY_MS / SAMPLE_PERIOD_MS)
/* ★ 陀螺仪量程换算 ★ */
// 正点原子默认 MPU_Set_Gyro_Fsr(3) → ±2000°/s → 灵敏度 16.4 LSB/(°/s)
#define GYRO_SCALE 16.4f
int main(void)
{
u8 t = 0;
float pitch, roll, yaw;
short aacx, aacy, aacz;
short gyrox, gyroy, gyroz;
short temp;
/* 扫频专用变量 */
float t_sweep = 0.0f;
float dt = SAMPLE_PERIOD_MS / 1000.0f;
float phase = 0.0f;
float f_current;
float sweep_ratio;
float sin_val;
u16 pwm_pitch, pwm_roll;
int32_t signed_offset, pwm;
/* 延时对齐缓存 */
int pwm_buf[DELAY_BUF_LEN];
int buf_index = 0;
int aligned_pwm;
int delayed_index;
int i;
/* 输出变量 */
int pwm_offset;
float output_angle;
float output_rate; // ★ 新增：输出角速度
/* LCD背光 GPIO 初始化结构体 */
GPIO_InitTypeDef GPIO_InitStructure;
/* ---------- 初始化 PWM 延时缓存 ---------- */
for(i = 0; i < DELAY_BUF_LEN; i++) {
pwm_buf = 0;
}
/* ---------- 系统初始化 ---------- */
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
uart_init(500000);
delay_init();
usmart_dev.init(72);
LED_Init();
KEY_Init();
LCD_Init();
MPU_Init();
MPU_Set_Gyro_Fsr(3); // 强制陀螺仪量程为 ±2000°/s, 与 GYRO_SCALE 一致
/* PWM初始化（50Hz） */
TIM3_PWM_Init(1999, 719);
TIM_SetCompare1(TIM3, ROLL_CENTER);
TIM_SetCompare2(TIM3, PITCH_CENTER);
TIM_SetCompare4(TIM3, 150);
/* LCD背光修复 */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_SetBits(GPIOB, GPIO_Pin_0);
/* LCD 显示 */
POINT_COLOR = RED;
LCD_ShowString(30,50,200,16,16,"ELITE STM32");
LCD_ShowString(30,70,200,16,16,"MPU6050 SWEEP");
LCD_ShowString(30,90,200,16,16,"ATOM@ALIENTEK");
LCD_ShowString(30,110,200,16,16,"2026/4/28");
/* DMP初始化 */
while(mpu_dmp_init())
{
LCD_ShowString(30,130,200,16,16,"MPU6050 Error");
delay_ms(200);
LCD_Fill(30,130,239,130+16,WHITE);
delay_ms(200);
}
LCD_ShowString(30,130,200,16,16,"MPU6050 OK");
#ifdef SWEEP_CHANNEL_PITCH
LCD_ShowString(30,150,200,16,16,"Pitch Sweep");
#elif defined(SWEEP_CHANNEL_ROLL)
LCD_ShowString(30,150,200,16,16,"Roll Sweep");
#endif
POINT_COLOR = BLUE;
LCD_ShowString(30,170,200,16,16,"Sweeping... ");
LCD_ShowString(30,200,200,16,16," Temp: . C");
LCD_ShowString(30,220,200,16,16,"Pitch: . C");
LCD_ShowString(30,240,200,16,16," Roll: . C");
LCD_ShowString(30,260,200,16,16," Yaw : . C");
delay_ms(1000);
/* ---------- 主循环：相位连续扫频 ---------- */
while(1)
{
/* 1. 固定非激活通道 */
#ifdef SWEEP_CHANNEL_PITCH
TIM_SetCompare1(TIM3, ROLL_CENTER);
TIM_SetCompare4(TIM3, 150);
#elif defined(SWEEP_CHANNEL_ROLL)
TIM_SetCompare2(TIM3, PITCH_CENTER);
TIM_SetCompare4(TIM3, 150);
#endif
/* 2. 计算当前频率 */
sweep_ratio = t_sweep / SWEEP_TOTAL;
if(sweep_ratio > 1.0f) sweep_ratio = 1.0f;
f_current = SWEEP_F_START + (SWEEP_F_END - SWEEP_F_START) * sweep_ratio;
/* 3. 相位连续的chirp信号 */
phase += 2.0f * 3.1415926f * f_current * dt;
sin_val = sinf(phase);
signed_offset = (int32_t)(SWEEP_AMPLITUDE * sin_val);
/* 4. PWM输出 */
#ifdef SWEEP_CHANNEL_PITCH
pwm = PITCH_CENTER + signed_offset;
if(pwm < SERVO_MIN) pwm = SERVO_MIN;
if(pwm > SERVO_MAX) pwm = SERVO_MAX;
TIM_SetCompare2(TIM3, (u16)pwm);
#elif defined(SWEEP_CHANNEL_ROLL)
pwm = ROLL_CENTER + signed_offset;
if(pwm < SERVO_MIN) pwm = SERVO_MIN;
if(pwm > SERVO_MAX) pwm = SERVO_MAX;
TIM_SetCompare1(TIM3, (u16)pwm);
#endif
/* 5. 存入PWM缓存（环形缓冲区） */
pwm_buf[buf_index] = (int)signed_offset;
/* 6. 获取延时对齐后的PWM偏移量 */
delayed_index = buf_index - DELAY_STEPS;
if(delayed_index < 0) delayed_index += DELAY_BUF_LEN;
aligned_pwm = pwm_buf[delayed_index];
buf_index++;
if(buf_index >= DELAY_BUF_LEN) buf_index = 0;
/* 7. 读取IMU并输出对齐后的数据（包含角速度） */
if(mpu_dmp_get_data(&pitch, &roll, &yaw) == 0)
{
temp = MPU_Get_Temperature();
MPU_Get_Accelerometer(&aacx, &aacy, &aacz);
MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz);
// 根据实际轴映射，Pitch 用 gyro_y，Roll 用 gyro_x
#ifdef SWEEP_CHANNEL_PITCH
output_angle = pitch;
output_rate = gyroy / GYRO_SCALE; // 对应 Pitch 轴角速度
#elif defined(SWEEP_CHANNEL_ROLL)
output_angle = roll;
output_rate = gyrox / GYRO_SCALE; // 对应 Roll 轴角速度
#endif
/* ? 核心输出：对齐后的 PWM偏移, 角度, 角速度 */
printf("%d, %.4f, %.2f\r\n", aligned_pwm, output_angle, output_rate);
/* LCD 刷新（每10次循环一次） */
if((t % 10) == 0)
{
if(temp < 0) { LCD_ShowChar(30+48,200,'-',16,0); temp = -temp; }
else LCD_ShowChar(30+48,200,' ',16,0);
LCD_ShowNum(30+48+8,200,temp/100,3,16);
LCD_ShowNum(30+48+40,200,temp%10,1,16);
if(pitch < 0) { LCD_ShowChar(30+48,220,'-',16,0); pitch = -pitch; }
else LCD_ShowChar(30+48,220,' ',16,0);
LCD_ShowNum(30+48+8,220,(int)(pitch*10)/10,3,16);
LCD_ShowNum(30+48+40,220,(int)(pitch*10)%10,1,16);
if(roll < 0) { LCD_ShowChar(30+48,240,'-',16,0); roll = -roll; }
else LCD_ShowChar(30+48,240,' ',16,0);
LCD_ShowNum(30+48+8,240,(int)(roll*10)/10,3,16);
LCD_ShowNum(30+48+40,240,(int)(roll*10)%10,1,16);
if(yaw < 0) { LCD_ShowChar(30+48,260,'-',16,0); yaw = -yaw; }
else LCD_ShowChar(30+48,260,' ',16,0);
LCD_ShowNum(30+48+8,260,(int)(yaw*10)/10,3,16);
LCD_ShowNum(30+48+40,260,(int)(yaw*10)%10,1,16);
t = 0;
LED0 = !LED0;
}
}
t++;
delay_ms(SAMPLE_PERIOD_MS);
t_sweep += dt;
/* 扫频结束，舵机归中 */
if(t_sweep >= SWEEP_TOTAL)
{
#ifdef SWEEP_CHANNEL_PITCH
TIM_SetCompare2(TIM3, PITCH_CENTER);
#elif defined(SWEEP_CHANNEL_ROLL)
TIM_SetCompare1(TIM3, ROLL_CENTER);
#endif
LCD_ShowString(30,170,200,16,16,"Sweep Finished!");
while(1) { LED0 = !LED0; delay_ms(200); }
}
}
} |
