第八,九,十天:陀螺仪姿态解算

本文围绕使用陀螺仪控制小车展开,一是尝试让小车在跷跷板上平衡,通过读取爬坡角度控制小车行进,但未成功;二是实现小车旋转偏移回正,先实现不动摇杆时自动回正,后探讨摇杆控制行进中受外力自动回正,给出不同思路及代码实现,还提及官方例程思路。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

目录

一:使用陀螺仪读取爬坡角度,试图在跷跷板上面平衡

二:使用陀螺仪进行旋转偏移的回正

0.目标:

1.先实现不动摇杆时的自动回正

2.之后实现摇杆控制小车行进过程中踢一脚能够进行自动回正并继续受摇杆控制


一:使用陀螺仪读取爬坡角度,试图在跷跷板上面平衡

1.往只有陀螺仪的工程里面移植进入遥控器,CAN通信的对应文件,并添加好对应路径,同时配置好CubeMX。

2.读取INS_angle[1]的值,使得小车可以自行爬坡。

如果是在mian.c里面开的线程,记得先在上面声明一下INS_task里面的INS_angle:

extern fp32 INS_angle[3];      //euler angle, unit rad.欧拉角,单位: rad

然后可以通过判断INS_angle[1]的值是否为0来决定小车是否向前或后退,同时倾斜角度越大,动力应越大:

 if(INS_angle[1]!=0)
 {
        set_angle[0]=INS_angle[1]*500;
        set_angle[1]=INS_angle[1]*500;
        set_angle[2]=INS_angle[1]*500;
        set_angle[3]=INS_angle[1]*500;                
        current_angle[0]=PID_calc(&PID_angle[0],motor_data[0].speed_rpm,-set_angle[0]); //1
                              current_angle[1]=PID_calc(&PID_angle[1],motor_data[1].speed_rpm,set_angle[1]); //2
                       current_angle[2]=PID_calc(&PID_angle[2],motor_data[2].speed_rpm,set_angle[2]); //3
        current_angle[3]=PID_calc(&PID_angle[3],motor_data[3].speed_rpm,-set_angle[3]);//4           
                            can_cmd_motor(&hcan1,0x200,current_angle[0],current_angle[1],current_angle[2],current_angle[3]);        
  } 

 值得注意的是,INS_angle[1]的值为负数时对应的转速和电流也会相应为负,所以轮子会自动实现不同方向的转动。

但最后未能实现小车在跷跷板上平衡,小车总是无法及时修正。

PS;当然也可以使用串行PID的方法去控制小车,外环PID是输入角度输出转速,内环PID是输入转速得到电流,最后发送给电机。

二:使用陀螺仪进行旋转偏移的回正

0.目标:

通过陀螺仪实现小车偏航自稳,通过读取陀螺仪数据使小车能当在偏航角(yaw)非0时通过自旋进行yaw轴归中,实现偏航角度闭环控制

1.先实现不动摇杆时的自动回正

最简单的做法是通过判断INS_angle[0]的值是否为0来进行自动回正,和判断INS_angle[1]来试图在跷跷板上面平衡的方法类似:

//When INS_angle[0] is bigger than 0, spin right 
//But in the computer, the variable of INS_angle[0] can't be 0.00 ,because the float can't be 0.00 
if(INS_angle[0]>2)
{
	set_angle[0]=INS_angle[0]*50;
	set_angle[1]=INS_angle[0]*50;
	set_angle[2]=INS_angle[0]*50;
	set_angle[3]=INS_angle[0]*50;				
				        
    current_angle[0]=PID_calc(&PID_angle[0],motor_data[0].speed_rpm,set_angle[0]); 
				 
    current_angle[1]=PID_calc(&PID_angle[1],motor_data[1].speed_rpm,set_angle[1]); 
				 
    current_angle[2]=PID_calc(&PID_angle[2],motor_data[2].speed_rpm,set_angle[2]); 
				 
    current_angle[3]=PID_calc(&PID_angle[3],motor_data[3].speed_rpm,set_angle[3]); 			
				 
can_cmd_motor(&hcan1,0x200,current_angle[0],current_angle[1],current_angle[2],current_angle[3]);				
}
else if(INS_angle[0]<-2)//When INS_angle[0] is bigger than 0, spin left 
{
	set_angle[0]=INS_angle[0]*50;
	set_angle[1]=INS_angle[0]*50;
	set_angle[2]=INS_angle[0]*50;
	set_angle[3]=INS_angle[0]*50;				
	current_angle[0]=PID_calc(&PID_angle[0],motor_data[0].speed_rpm,-set_angle[0]); 
    current_angle[1]=PID_calc(&PID_angle[1],motor_data[1].speed_rpm,-set_angle[1]); 
	current_angle[2]=PID_calc(&PID_angle[2],motor_data[2].speed_rpm,-set_angle[2]); 
	current_angle[3]=PID_calc(&PID_angle[3],motor_data[3].speed_rpm,-set_angle[3]); 			
				can_cmd_motor(&hcan1,0x200,current_angle[0],current_angle[1],current_angle[2],current_angle[3]);					
}
else
{			
	current_angle[0]=PID_calc(&PID_angle[0],motor_data[0].speed_rpm,0); 
	current_angle[1]=PID_calc(&PID_angle[1],motor_data[1].speed_rpm,0); 
	current_angle[2]=PID_calc(&PID_angle[2],motor_data[2].speed_rpm,0); 
	current_angle[3]=PID_calc(&PID_angle[3],motor_data[3].speed_rpm,0); 			
				can_cmd_motor(&hcan1,0x200,current_angle[0],current_angle[1],current_angle[2],current_angle[3]);					
}

由于浮点不可能为0,加上陀螺仪读取到的数值会有一定程度的波动,所以可以设一个死区。

在死区内认为小车已回正,停止转动。否则小车会”摇摆“。

 以上是陀螺仪没有校准时候的参量设置(INS_angle[0]可以破百),所以倍率偏小。

以下则是使用官方例程的含有校准功能的工程进行的参数设置,并且进行了弧度转角度的计算:

//弧度转角度
for(int i=0;i<4;i++)
{
	angle_360[i]=INS_angle[i]/pi*180;
}
//When INS_angle[0] is bigger than 0, spin right 
//But in the computer, the variable of INS_angle[0] can't be 0.00 ,because the float can't be 0.00 
if(angle_360[0]>2)
{
	set_angle[0]=1000;
	set_angle[1]=1000;
	set_angle[2]=1000;
	set_angle[3]=1000;				
				    
    current_angle[0]=PID_calc(&PID_angle[0],motor_data[0].speed_rpm,set_angle[0]); 
				 
    current_angle[1]=PID_calc(&PID_angle[1],motor_data[1].speed_rpm,set_angle[1]); 
				 
    current_angle[2]=PID_calc(&PID_angle[2],motor_data[2].speed_rpm,set_angle[2]); 
				 
    current_angle[3]=PID_calc(&PID_angle[3],motor_data[3].speed_rpm,set_angle[3]); 			
				CAN_cmd_chassis(current_angle[0],current_angle[1],current_angle[2],current_angle[3]);				}
else if(angle_360[0]<-2)//When INS_angle[0] is bigger than 0, spin left 
{
	set_angle[0]=-1000;
	set_angle[1]=-1000;
	set_angle[2]=-1000;
	set_angle[3]=-1000;				
				 
     current_angle[0]=PID_calc(&PID_angle[0],motor_data[0].speed_rpm,set_angle[0]); 
				 
     current_angle[1]=PID_calc(&PID_angle[1],motor_data[1].speed_rpm,set_angle[1]); 
				 
     current_angle[2]=PID_calc(&PID_angle[2],motor_data[2].speed_rpm,set_angle[2]); 
				 
     current_angle[3]=PID_calc(&PID_angle[3],motor_data[3].speed_rpm,set_angle[3]); 			
				CAN_cmd_chassis(current_angle[0],current_angle[1],current_angle[2],current_angle[3]);					
}
else 
{			
	current_angle[0]=PID_calc(&PID_angle[0],motor_data[0].speed_rpm,0); 
    current_angle[1]=PID_calc(&PID_angle[1],motor_data[1].speed_rpm,0); 
	current_angle[2]=PID_calc(&PID_angle[2],motor_data[2].speed_rpm,0); 
	current_angle[3]=PID_calc(&PID_angle[3],motor_data[3].speed_rpm,0); 			
				CAN_cmd_chassis(current_angle[0],current_angle[1],current_angle[2],current_angle[3]);					
}

进阶一点的做法是使用串级PID,也是和上面说的一样,角度控制转速再让转速控制电流。

先定义好需要用到的PID结构和变量:

pid_type_def Pid[4];
pid_type_def PID_rpm;//outside PID
pid_type_def PID_current[4];//inside PID

const fp32 kp_i_d[3]={14,0,0};
const fp32 k_angle[3]={40,10,0};
const fp32 k_angle_rpm[3]={20,0,0};

extern RC_ctrl_t rc_ctrl;  
extern fp32 INS_quat[4] ;
extern fp32 INS_angle[3];    
extern motor_measure_t  motor_data[4]; 

float set_rpm[4];      
float give_current[4];    
float set_angle; 
float give_rpm;  
float current_angle_rpm[4];

并进行相应初始化:

  delay_init();
  
  can_filter_init();                   //can过滤器初始化
  for(int i=0;i<4;i++)
  {
        PID_init(&Pid[i],PID_POSITION,kp_i_d,1200,200);        //pid初始化  
        PID_init(&PID_current[i],PID_POSITION,k_angle_rpm,1500,200);    
  }
  PID_init(&PID_rpm,PID_POSITION,k_angle_rpm,1200,200);    
  remote_control_init();            //遥控器初始化

这里PID设置的不是很好,导致回正的时候转动速度偏慢。(并且设置的ki不为0,以达到在逼近目标值的时候能够通过积分输出更好的去达到预期值)

然后是对应计算过程:

		
set_angle=INS_angle[0]/pi*180;
if(set_angle<-1||set_angle>1)//skip right
{
	give_rpm=PID_calc(&PID_rpm,set_angle,0);
	for(int i=0;i<4;i++)
	{
		current_angle_rpm[i]=PID_calc(&PID_current[i],motor_data[i].speed_rpm,-give_rpm);
	}	
	 
CAN_cmd_chassis(current_angle_rpm[0],current_angle_rpm[1],current_angle_rpm[2],current_angle_rpm[3]);	
}
else//stop
{
	for(int i=0;i<4;i++)
	{
					current_angle_rpm[i]=PID_calc(&PID_current[i],motor_data[i].speed_rpm,0);
	}				
				CAN_cmd_chassis(current_angle_rpm[0],current_angle_rpm[1],current_angle_rpm[2],current_angle_rpm[3]);									
}

这里同样设置了死区,但是死区不要设置太小,因为数值会有一定程度的抖动,即便是0.01范围的抖动,弧度制放大变成角度制抖动角度也不小了。所以死区设置太小可能还是会在逼近平衡位置的时候出现”摇摆“的现象。

2.之后实现摇杆控制小车行进过程中踢一脚能够进行自动回正并继续受摇杆控制

思路是使用串行PID,

外环:输入角度输出角速度,并将角速度给到麦轮分解的公式里面;

内环:输入转速输出电流,并将电流给到电机。


最开始想到的是外环PID输入的当前值是INS_angle[0],预期是控制自旋的摇杆通道rc_ctrl.rc.ch[0]的值,于是有:

vw=PID_calc(&PID_yaw,INS_angle[0],rc_ctrl.rc.ch[0]);

但是由于当前值是一个弧度制,范围在-pi~pi,而rc_ctrl.rc.ch[0]的值范围大概在-660~660,所以k要设在几百甚至上千。

但如果直接把vw当作当作麦轮分解的第三个参数vw(角速度分量),那就会发现:

不仅不动摇杆的时候能够偏航回正,就连摇杆控制自旋一定的角度后小车也会自动回到最开始的角度,因此上述过程将通道0的值当作了期望,于是会出现小车无法偏离原来角度的结果。

于是想到给摇杆加一个偏移量,时刻记录摇杆通道0所偏移的里程:

定义一个 angle_set且让: angle_set+=rc_ctrl.rc.ch[0];

再vw=PID_calc(&PID_yaw,INS_angle[0],angle_set);

但是这样会导致angle_set的值在推几次摇杆后变的特别大,于是为了从一个不超过pi的值涨到几千甚至几万的值,vw会一直输出一个非常大的值(如果k调得够大的话)并且不能够短时间接近目标值,于是就会发现小车开始疯转许久停不下来。

并且由于vw的值远远超出了另外两个通道(控制平移)的值,所以会导致小车的平移不受控制,无论怎么推摇杆,都会以一个极大的值原地疯转。


以下提供另外的思路——

                   一个有缺陷的版本:

先进行必要的变量或结构定义并初始化:

//声明其他文件的变量
extern RC_ctrl_t rc_ctrl;   
extern fp32 INS_angle[3]; 
extern motor_measure_t  motor_data[4]; 

//PID结构
pid_type_def Pid[4];

pid_type_def PID_yaw;

//包含kp,ki,kd的数组
const fp32 kp_i_d[3]={14,0,0};

const fp32 k_yaw[3]={500,0,0};

float set_rpm[4];            //设定的转速 /每分钟
float give_current[4];     //发送给电机的电流
float angle_set;  //控制回正的期望
float vw;//角速度

//初始化

  delay_init();
  //can过滤器初始化
  can_filter_init(); 
   //遥控器初始化
  remote_control_init();    
  //pid初始化    
  for(int i=0;i<4;i++)
  {
        PID_init(&Pid[i],PID_POSITION,kp_i_d,1200,200);        
        PID_init(&PID_current[i],PID_POSITION,k_angle_rpm,1500,200);    
  }
  PID_init(&PID_rpm,PID_POSITION,k_angle_rpm,1200,200);
  PID_init(&PID_yaw,PID_POSITION,k_yaw,1200,100);

 然后是实现部分:

            //通过PID,输入角度,输出角速度
            if(rc_ctrl.rc.ch[0]<-2 || rc_ctrl.rc.ch[0]>2)
            {                
                angle_set=INS_angle[0];
            }
            vw=PID_calc(&PID_yaw,INS_angle[0],angle_set);        
            //将包含角速度的值进行麦轮分解
            set_rpm[0]=(-rc_ctrl.rc.ch[3]+rc_ctrl.rc.ch[2]+rc_ctrl.rc.ch[0])*10;  //1
            set_rpm[1]=(rc_ctrl.rc.ch[3]+rc_ctrl.rc.ch[2]+rc_ctrl.rc.ch[0])*10;  //2
            set_rpm[2]=(rc_ctrl.rc.ch[3]-rc_ctrl.rc.ch[2]+rc_ctrl.rc.ch[0])*10;  //3
            set_rpm[3]=(-rc_ctrl.rc.ch[3]-rc_ctrl.rc.ch[2]+rc_ctrl.rc.ch[0])*10;  //4
            for(int i=0;i<4;i++)
            {
            give_current[i]=PID_calc(&Pid[i],motor_data[i].speed_rpm,set_rpm[i]-50*vw);   //发送数据计算电流    
            }
            CAN_cmd_chassis(give_current[0],give_current[1],give_current[2],give_current[3]);

实现思路是将自动回正的PID和麦轮分解的计算分开,保证要摇杆控制的稳定性。

而这部分代码的缺陷在控制摇杆自旋的时候踢一脚无法实现自动回正后再继续自旋。

当摇杆不控制的时候,angle_set是当前车头的朝向,这个时候能够通过外环PID自动回正,

因为此时set_rpm的值为0,而如果受到外力,vw的值不为0,就会给电流到电机实现对应的PID控制回正,

当摇杆控制的时候,会进入if的判断语句,使得预期角度等于当前角度,此时vw始终为0,这是内环PID的最后一个参数也就是期望完全取决于摇杆的set_rpm,实现摇杆有效控制;

而又由于平移的vx和vy不受yaw的影响,所以可以一边控制左摇杆平移一边实现偏航自稳。

唯独无法实现摇杆控制小车自旋的时候能够偏航自稳。


接下来是官方例程的部分实现思路:

void PID_task(void const * argument)
{    
    //底盘初始化
    //底盘数据更新
    //先更新电机速度
    for(int i=0;i<4;i++)
    {
        speed[i]=CHASSIS_MOTOR_RPM_TO_VECTOR_SEN * motor_data[i].speed_rpm;
    }
    //再更新vw
    vw=(-speed[0]-speed[1]-speed[2]-speed[3]) * MOTOR_SPEED_TO_CHASSIS_SPEED_WZ / MOTOR_DISTANCE_TO_CENTER;
    //然后计算底盘姿态角度,这里只处理yaw
    yaw=INS_angle[0];        
    while(1)
    {
        if(rc_ctrl.rc.s[0]==0x0001)
        {
            for(int i=0;i<4;i++)
            {
                give_current[i]=PID_calc(&Pid[i],motor_data[i].speed_rpm,0); //断流,写保护                
            }    
            CAN_cmd_chassis(give_current[0],give_current[1],give_current[2],give_current[3]);    
            osDelay(5);                
        }
        else if(rc_ctrl.rc.s[0]==0x0002)//右开关向下拨动开车
        {
            //底盘数据更新
            //先更新电机速度
            for(int i=0;i<4;i++)
            {
                speed[i]=CHASSIS_MOTOR_RPM_TO_VECTOR_SEN * motor_data[i].speed_rpm;
            }
            //再更新vw
            vw=(-speed[0]-speed[1]-speed[2]-speed[3]) * MOTOR_SPEED_TO_CHASSIS_SPEED_WZ / MOTOR_DISTANCE_TO_CENTER;
            //然后计算底盘姿态角度,这里只处理yaw
            yaw=INS_angle[0];    
            //底盘控制量设置,
            fp32 delat_angle = 0.0f;            
            yaw_set=loop_fp32_constrain(yaw-rc_ctrl.rc.ch[0],-pi,pi);
            delat_angle=loop_fp32_constrain(yaw_set-INS_angle[0],-pi,pi);
            vw=PID_calc(&yaw_angle,0.0f,delat_angle);
            //底盘控制PID计算
            set_rpm[0]=(-rc_ctrl.rc.ch[3]+rc_ctrl.rc.ch[2]+vw)*10;  //1
            set_rpm[1]=(rc_ctrl.rc.ch[3]+rc_ctrl.rc.ch[2]+vw)*10;  //2
            set_rpm[2]=(rc_ctrl.rc.ch[3]-rc_ctrl.rc.ch[2]+vw)*10;  //3
            set_rpm[3]=(-rc_ctrl.rc.ch[3]-rc_ctrl.rc.ch[2]+vw)*10;  //4
            for(int i=0;i<4;i++)
            {
                give_current[i]=PID_calc(&Pid[i],motor_data[0].speed_rpm,set_rpm[i]);
            }
            CAN_cmd_chassis(give_current[0],give_current[1],give_current[2],give_current[3]);
            osDelay(5); //注意一定要给延时,不然会疯
       }
        else if(rc_ctrl.rc.s[0]==0x0003)//the right middle is balance 
        {
            osDelay(5); //注意一定要给延时,不然会疯
        }    
    }
}

详见官方的chassis_task.c文件。 

发现yaw的值在一定时间后会被锁死(oled屏幕上数字不发生改变),查找原因,查找原因修正 empty.c #include <math.h> // 添加数学库用于fabs函数 #include "board.h" #include "my_key.h" #include "my_time.h" #include "ti_msp_dl_config.h" #include "oled.h" void BUZZY_OFF(void) { DL_GPIO_setPins(BUZZY_PORT, BUZZY_PIN_PIN); } void BUZZY_ON(void) { DL_GPIO_clearPins(BUZZY_PORT, BUZZY_PIN_PIN); } void refresh_oled(void); void key(void); void go_straight(int dis); void go_arc_ccd(hsu_time_t); void go_brc_ccd(hsu_time_t); void turn_in_place(float angle); void sound_light_alert(void); void show_task_now(void); u8 Car_Mode = Diff_Car; int Motor_Left, Motor_Right; // 电机PWM变量 应是Motor的 u8 PID_Send; // 延时和调参相关变量 float RC_Velocity = 200, RC_Turn_Velocity, Move_X, Move_Y, Move_Z, PS2_ON_Flag; // 遥控控制的速度 float Velocity_Left, Velocity_Right; // 车轮速度(mm/s) u16 test_num, show_cnt; float Voltage = 0; extern float Yaw; // 声明外部YAW角度变量 int64_t left_encoder = 0, right_encoder = 0; void SysTick_Handler(void) { hsu_time_systick_handler(); } typedef enum { BEGIN, T1, T2, T3, T4 } TaskState; typedef enum { STOP, GO_STRAIGHT, GO_CCD, TURN_IN_PLACE, WAIT_ALERT } DoingWhat; typedef struct __TASK_NAMESPACE { uint8_t is; uint8_t is_running; uint8_t finish; uint8_t sub_finish; uint8_t running_state; // 0: 停止, 1: 运行中 TaskState state; DoingWhat doing_what; float target; float vx; float vz; // 用于复杂任务 uint8_t sub_task_stage; // 子任务阶段 uint8_t lap_count; // 圈数计数 int64_t start_encoder; // 起始编码器值 uint32_t alert_start_time; // 声光提示开始时间 float start_yaw; // 起始YAW角度 float target_yaw_diff; // 目标YAW角度差 hsu_time_t ccd_end_time; } TaskNamespace; void reset_task_namespace(TaskNamespace *t) { t->is_running = 0; t->finish = 0; t->sub_finish = 0; t->state = BEGIN; t->doing_what = STOP; t->vx = 0; t->vz = 0; t->sub_task_stage = 0; t->lap_count = 0; t->start_encoder = left_encoder; t->alert_start_time = 0; t->start_yaw = 0; t->target_yaw_diff = 0; t->running_state = 0; // 明确重置运行状态为0 t->ccd_end_time = 0; t->is = 0; } void next_state(TaskNamespace *t) { TaskState last_state = t->state; reset_task_namespace(t); if (last_state < T4) { t->state = last_state + 1; } } TaskNamespace task_namespace; void show_task_now(void) { //OLED_ShowString(0, 0, "Task Now:"); switch (task_namespace.state) { case BEGIN: OLED_ShowString(1, 10,"0"); break; case T1: OLED_ShowString(1, 10,"1"); break; case T2: OLED_ShowString(1, 10,"2"); break; case T3: OLED_ShowString(1, 10,"3"); break; case T4: OLED_ShowString(1, 10,"4"); break; default: break; } } void main_task(void); int main(void) { // 系统初始化 SYSCFG_DL_init(); // 初始化系统配置 hsu_time_init(); // 时间 // 清除所有外设的中断挂起状态 NVIC_ClearPendingIRQ(ENCODERA_INT_IRQN); // 编码器A中断 NVIC_ClearPendingIRQ(ENCODERB_INT_IRQN); // 编码器B中断 NVIC_ClearPendingIRQ(UART_0_INST_INT_IRQN); // UART0串口中断 // 使能各外设的中断 NVIC_EnableIRQ(ENCODERA_INT_IRQN); // 开启编码器A中断 NVIC_EnableIRQ(ENCODERB_INT_IRQN); // 开启编码器B中断 NVIC_EnableIRQ(UART_0_INST_INT_IRQN); // 开启UART0中断 reset_task_namespace(&task_namespace); task_namespace.state = BEGIN; // 明确设置初始状态 // 定时器和ADC相关中断配置 NVIC_ClearPendingIRQ(TIMER_0_INST_INT_IRQN); // 清除定时器0中断挂起 NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN); // 开启定时器0中断 NVIC_EnableIRQ(ADC12_VOLTAGE_INST_INT_IRQN); NVIC_EnableIRQ(ADC12_CCD_INST_INT_IRQN); OLED_Init(); // 初始化OLED显示屏 OLED_ShowString(1, 1, "Task Now:"); OLED_ShowString(2, 1, "state:"); OLED_ShowString(3, 1, "yaw:"); MPU6050_initialize(); DMP_Init(); BUZZY_ON(); // 主循环 // printf("Test delay 500us\n"); // hsu_time_delay_us(500); // printf("Test delay 500us end\n"); uint8_t main_task_timer = hsu_time_timer_create(10, true, main_task); hsu_time_timer_start(main_task_timer); uint8_t refresh_oled_timer = hsu_time_timer_create(5, true, refresh_oled); hsu_time_timer_start(refresh_oled_timer); uint8_t key_timer = hsu_time_timer_create(2, true, key); hsu_time_timer_start(key_timer); while (1) { hsu_time_timer_process(); RD_TSL(); // 读取CCD数据 Find_CCD_Median(); // 计CCD数据中值 Read_DMP(); show_task_now(); char yaw_str[10]; // 存储格式化后的字符串 snprintf(yaw_str, sizeof(yaw_str), "%.1f", Yaw); // 格式化为带一位小数的字符串 OLED_ShowString(3, 6, yaw_str); // 在指定位置显示YAW值 //DL_GPIO_togglePins(LED_PORT, LED_led_PIN); // printf("L=%lld R=%lld YAW=%.1f\n", left_encoder, right_encoder, Yaw); } } void task_no(void); void task_1(void); void task_2(void); void task_3(void); void task_4(void); void main_task(void) { if (!(task_namespace.is)) return; printf("main task\n"); switch (task_namespace.state) { case BEGIN: task_no(); break; case T1: task_1(); break; case T2: task_2(); break; case T3: task_3(); break; case T4: task_4(); break; default: break; } switch (task_namespace.doing_what) { case STOP: Get_Target_Encoder(0, 0); break; case GO_STRAIGHT: if ((left_encoder * 1.f) < task_namespace.target) { Get_Target_Encoder(0.3, 0); // 提高速度到300mm/s } else { Get_Target_Encoder(0, 0); task_namespace.doing_what = STOP; task_namespace.finish = 1; } break; case GO_CCD: if (task_namespace.ccd_end_time < hsu_time_get_ms()) { Get_Target_Encoder(0, 0); task_namespace.doing_what = STOP; task_namespace.finish = 1; } else { CCD_Mode(); } break; case TURN_IN_PLACE: // 原地转向控制 if (task_namespace.target_yaw_diff != 0) { float current_yaw_diff = Yaw - task_namespace.start_yaw; // 处理角度跨越±180度的情况 if (current_yaw_diff > 180) { current_yaw_diff -= 360; } else if (current_yaw_diff < -180) { current_yaw_diff += 360; } printf("Turn: Start=%.1f Current=%.1f Diff=%.1f Target=%.1f\n", task_namespace.start_yaw, Yaw, current_yaw_diff, task_namespace.target_yaw_diff); // 检查是否达到目标角度 if ((task_namespace.target_yaw_diff > 0 && current_yaw_diff >= task_namespace.target_yaw_diff) || (task_namespace.target_yaw_diff < 0 && current_yaw_diff <= task_namespace.target_yaw_diff)) { Get_Target_Encoder(0, 0); // 停止转向 task_namespace.doing_what = STOP; task_namespace.finish = 1; } else { // 继续转向 float turn_speed = (task_namespace.target_yaw_diff > 0) ? 0.1 : -0.1; Get_Target_Encoder(0, turn_speed); } } break; case WAIT_ALERT: Get_Target_Encoder(0, 0); // 停车 if (hsu_time_get_ms() - task_namespace.alert_start_time > 1000) { // 声光提示1秒 task_namespace.doing_what = STOP; task_namespace.finish = 1; } break; default: break; } } void task_no(void) { return; } // 任务1:A点到B点直线行驶 void task_1(void) { if (!task_namespace.is_running) { task_namespace.is_running = 1; task_namespace.sub_task_stage = 0; task_namespace.finish = 1; return; } if (task_namespace.finish) { switch (task_namespace.sub_task_stage) { case 0: // 开始第一阶段:A到B go_straight(300); break; case 1: // A到B完成,开始B到C DL_GPIO_togglePins(LED_PORT, LED_led_PIN); break; case 2: //任务结束 reset_task_namespace(&task_namespace); task_namespace.running_state = 0; // 重置为停止状态 break; } task_namespace.finish = 0; task_namespace.sub_task_stage++; } return; } // 任务2:A->B->C->D->A循环 void task_2(void) { if (!task_namespace.is_running) { task_namespace.is_running = 1; task_namespace.sub_task_stage = 0; task_namespace.finish = 1; return; } if (task_namespace.finish) { switch (task_namespace.sub_task_stage) { case 0: // 开始第一阶段:A到B go_straight(3300); break; case 1: //DL_GPIO_togglePins(LED_PORT, LED_led_PIN); go_straight(2000); break; case 2: // B到C弧线完成,开始C到D直线 sound_light_alert(); //turn_in_place(-17.0f); go_straight(2980); break; case 3: // C到D完成,开始D到A弧线 sound_light_alert(); go_arc_ccd(3530); break; case 4: // D到A弧线完成,任务结束 sound_light_alert(); reset_task_namespace(&task_namespace); break; } task_namespace.finish = 0; task_namespace.sub_task_stage++; } return; } // 任务3:A->C->B->D->A循环 void task_3(void) { if (!task_namespace.is_running) { task_namespace.is_running = 1; task_namespace.sub_task_stage = 0; task_namespace.finish = 1; return; } if (task_namespace.finish) { switch (task_namespace.sub_task_stage) { case 0: turn_in_place(-31.0f); break; case 1: go_straight(4060); break; case 2: turn_in_place(30.0f); break; case 3: // 开始C到B弧线 sound_light_alert(); //go_straight(40); go_brc_ccd(3550); break; case 4: turn_in_place(36.0f); break; case 5: sound_light_alert(); go_straight(3985); break; case 6: turn_in_place(-40.0f); break; case 7: // 开始C到B弧线 sound_light_alert(); go_arc_ccd(3600); break; case 8: // D到A弧线完成,任务结束 sound_light_alert(); reset_task_namespace(&task_namespace); break; } task_namespace.finish = 0; task_namespace.sub_task_stage++; } return; } // 任务4:重复任务3路径4圈 void task_4(void) { if (!task_namespace.is_running) { task_namespace.is_running = 1; task_namespace.sub_task_stage = 0; task_namespace.finish = 1; return; } if (task_namespace.finish) { switch (task_namespace.sub_task_stage) { case 0: turn_in_place(-30.0f); break; case 1: go_straight(4045); break; case 2: turn_in_place(29.0f); break; case 3: // 开始C到B弧线 sound_light_alert(); //go_straight(40); go_brc_ccd(5000); break; case 4: turn_in_place(34.0f); break; case 5: sound_light_alert(); go_straight(4035); break; case 6: turn_in_place(-33.0f); break; case 7: // 开始C到B弧线 sound_light_alert(); go_arc_ccd(5000); break; case 8: // D到A弧线完成,任务结束 sound_light_alert(); reset_task_namespace(&task_namespace); break; } task_namespace.finish = 0; task_namespace.sub_task_stage++; } } void TIMER_0_INST_IRQHandler(void) { if (DL_TimerA_getPendingInterrupt(TIMER_0_INST)) { if (DL_TIMER_IIDX_ZERO) { Get_Velocity_From_Encoder(Get_Encoder_countA, Get_Encoder_countB); Get_Encoder_countA = Get_Encoder_countB = 0; MotorA.Motor_Pwm = Incremental_PI_Left(MotorA.Current_Encoder, MotorA.Target_Encoder); MotorB.Motor_Pwm = Incremental_PI_Right(MotorB.Current_Encoder, MotorB.Target_Encoder); if (!Flag_Stop) { Set_PWM(-MotorA.Motor_Pwm, -MotorB.Motor_Pwm); } else { Set_PWM(0, 0); } } } } uint32_t gpio_interrup1, gpio_interrup2; int64_t B1, B2, B3, B4; int64_t A1, A2, A3, A4; void GROUP1_IRQHandler(void) { // 获取中断信号 gpio_interrup1 = DL_GPIO_getEnabledInterruptStatus(ENCODERA_PORT, ENCODERA_E1A_PIN | ENCODERA_E1B_PIN); gpio_interrup2 = DL_GPIO_getEnabledInterruptStatus(ENCODERB_PORT, ENCODERB_E2A_PIN | ENCODERB_E2B_PIN); // encoderB if ((gpio_interrup1 & ENCODERA_E1A_PIN) == ENCODERA_E1A_PIN) { if (!DL_GPIO_readPins(ENCODERA_PORT, ENCODERA_E1B_PIN)) { right_encoder--; Get_Encoder_countB--; } else { right_encoder++; Get_Encoder_countB++; } } else if ((gpio_interrup1 & ENCODERA_E1B_PIN) == ENCODERA_E1B_PIN) { if (!DL_GPIO_readPins(ENCODERA_PORT, ENCODERA_E1A_PIN)) { right_encoder++; Get_Encoder_countB++; } else { right_encoder--; Get_Encoder_countB--; } } // encoderA if ((gpio_interrup2 & ENCODERB_E2A_PIN) == ENCODERB_E2A_PIN) { if (!DL_GPIO_readPins(ENCODERB_PORT, ENCODERB_E2B_PIN)) { left_encoder++; Get_Encoder_countA--; } else { left_encoder--; Get_Encoder_countA++; } } else if ((gpio_interrup2 & ENCODERB_E2B_PIN) == ENCODERB_E2B_PIN) { if (!DL_GPIO_readPins(ENCODERB_PORT, ENCODERB_E2A_PIN)) { left_encoder--; Get_Encoder_countA++; } else { left_encoder++; Get_Encoder_countA--; } } DL_GPIO_clearInterruptStatus(ENCODERA_PORT, ENCODERA_E1A_PIN | ENCODERA_E1B_PIN); DL_GPIO_clearInterruptStatus(ENCODERB_PORT, ENCODERB_E2A_PIN | ENCODERB_E2B_PIN); } // 直线行驶函数 void go_straight(int dis) { task_namespace.doing_what = GO_STRAIGHT; task_namespace.target = left_encoder + dis; task_namespace.finish = 0; } // 原地转向函数 void turn_in_place(float angle) { task_namespace.doing_what = TURN_IN_PLACE; task_namespace.start_yaw = Yaw; task_namespace.target_yaw_diff = angle; // 正值右转,负值左转 task_namespace.finish = 0; } // CCD巡线函数(需要外部条件结束) void go_ccd_line(void) { task_namespace.doing_what = GO_CCD; task_namespace.start_encoder = left_encoder; task_namespace.finish = 0; // 设置一个安全的最大距离,防止无限巡线 // 可以根据实际场地调整这个值 static uint32_t ccd_end_time = 0; if (ccd_end_time == 0) { ccd_end_time = hsu_time_get_ms(); } // 如果巡线时间超过10秒或距离超过2000mm,强制结束 if (hsu_time_get_ms() - ccd_end_time > 10000 || (left_encoder * 1.f - task_namespace.start_encoder) > 2000) { task_namespace.finish = 1; ccd_end_time = 0; } } // 弧线CCD巡线函数 void go_arc_ccd(hsu_time_t time) { task_namespace.doing_what = GO_CCD; task_namespace.start_encoder = left_encoder; task_namespace.ccd_end_time = hsu_time_get_ms() + time; } void go_brc_ccd(hsu_time_t time) { task_namespace.doing_what = GO_CCD; task_namespace.start_encoder = right_encoder; task_namespace.ccd_end_time = hsu_time_get_ms() + time; } // 声光提示函数 void sound_light_alert(void) { DL_GPIO_togglePins(LED_PORT, LED_led_PIN); DL_GPIO_setPins(BUZZY_PORT, BUZZY_PIN_PIN); uint32_t start_time = hsu_time_get_ms(); while (hsu_time_get_ms() - start_time < 1000) { // 空循环等待1秒 } //hsu_time_delay_ms(200); DL_GPIO_togglePins(LED_PORT, LED_led_PIN); DL_GPIO_clearPins(BUZZY_PORT, BUZZY_PIN_PIN); } // callback void refresh_oled(void) { show_task_now(); OLED_ShowString(2, 1, "state:"); if (task_namespace.running_state) { OLED_ShowString(2, 7, "1"); // 运行中 } else { OLED_ShowString(2, 7, "0"); // 停止 } } uint32_t key_get_tick_ms(void) { return hsu_time_get_ms(); } void key(void) { key_event_t event = key_scan(); //uint8_t key_value = key_read_pin(); // 获取按键状态 //S1 switch (event) { case KEY_EVENT_SINGLE_CLICK: next_state(&task_namespace); break; case KEY_EVENT_DOUBLE_CLICK: task_namespace.is = 1; task_namespace.running_state = 1; task_namespace.is_running = 0; // 重置任务运行标志 break; } } MPU6050.c #include "MPU6050.h" #include <stdio.h> #include "inv_mpu.h" // #include "IOI2C.h" // #include "usart.h" #define PRINT_ACCEL (0x01) #define PRINT_GYRO (0x02) #define PRINT_QUAT (0x04) #define ACCEL_ON (0x01) #define GYRO_ON (0x02) #define MOTION (0) #define NO_MOTION (1) #define DEFAULT_MPU_HZ (200) #define FLASH_SIZE (512) #define FLASH_MEM_START ((void *)0x1800) #define q30 1073741824.0f short gyro[3], accel[3], sensors; float Roll, Pitch, Yaw; float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; static signed char gyro_orientation[9] = {-1, 0, 0, 0, -1, 0, 0, 0, 1}; Imu_t mpu6050 = {0}; Imu_t RegOri_mpu6050 = {0}; // iic转接 #include "bsp_siic.h" static pIICInterface_t siic = &User_sIICDev; uint8_t IICwriteBits(uint8_t addr, uint8_t reg, uint8_t bitStart, uint8_t length, uint8_t data) { uint8_t b; if (siic->read_reg(addr << 1, reg, &b, 1, 200) == IIC_OK) { uint8_t mask = (0xFF << (bitStart + 1)) | (0xFF >> ((8 - bitStart) + length - 1)); data <<= (8 - length); data >>= (7 - bitStart); b &= mask; b |= data; return siic->write_reg(addr << 1, reg, &b, 1, 200); } return 1; } uint8_t IICwriteBit(uint8_t dev, uint8_t reg, uint8_t bitNum, uint8_t data) { uint8_t b; siic->read_reg(dev << 1, reg, &b, 1, 200); b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum)); return siic->write_reg(dev << 1, reg, &b, 1, 200); } uint8_t IICreadBytes(uint8_t dev, uint8_t reg, uint8_t length, uint8_t *data) { return siic->read_reg(dev << 1, reg, data, length, 200); } int i2cRead(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf) { return siic->read_reg(addr << 1, reg, buf, len, 200); } unsigned char I2C_ReadOneByte(unsigned char I2C_Addr, unsigned char addr) { uint8_t b = 0; siic->read_reg(I2C_Addr << 1, addr, &b, 1, 200); return b; } static unsigned short inv_row_2_scale(const signed char *row) { unsigned short b; if (row[0] > 0) b = 0; else if (row[0] < 0) b = 4; else if (row[1] > 0) b = 1; else if (row[1] < 0) b = 5; else if (row[2] > 0) b = 2; else if (row[2] < 0) b = 6; else b = 7; // error return b; } static unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx) { unsigned short scalar; scalar = inv_row_2_scale(mtx); scalar |= inv_row_2_scale(mtx + 3) << 3; scalar |= inv_row_2_scale(mtx + 6) << 6; return scalar; } static void run_self_test(void) { int result; long gyro[3], accel[3]; result = mpu_run_self_test(gyro, accel); if (result == 0x7) { /* Test passed. We can trust the gyro data here, so let's push it down * to the DMP. */ float sens; unsigned short accel_sens; mpu_get_gyro_sens(&sens); gyro[0] = (long)(gyro[0] * sens); gyro[1] = (long)(gyro[1] * sens); gyro[2] = (long)(gyro[2] * sens); dmp_set_gyro_bias(gyro); mpu_get_accel_sens(&accel_sens); accel[0] *= accel_sens; accel[1] *= accel_sens; accel[2] *= accel_sens; dmp_set_accel_bias(accel); // printf("setting bias succesfully ......\r\n"); } } uint8_t buffer[14]; int16_t MPU6050_FIFO[6][11]; int16_t Gx_offset = 0, Gy_offset = 0, Gz_offset = 0; /************************************************************************** Function: The new ADC data is updated to FIFO array for filtering Input : ax,ay,az:x,y, z-axis acceleration data;gx,gy,gz:x. Y, z-axis angular acceleration data Output : none 函数功能:将新的ADC数据更新到 FIFO数组,进行滤波处理 入口参数:ax,ay,az:x,y,z轴加速度数据;gx,gy,gz:x,y,z轴角加速度数据 返回 值:无 **************************************************************************/ void MPU6050_newValues(int16_t ax, int16_t ay, int16_t az, int16_t gx, int16_t gy, int16_t gz) { unsigned char i; int32_t sum = 0; for (i = 1; i < 10; i++) { // FIFO 操作 MPU6050_FIFO[0][i - 1] = MPU6050_FIFO[0][i]; MPU6050_FIFO[1][i - 1] = MPU6050_FIFO[1][i]; MPU6050_FIFO[2][i - 1] = MPU6050_FIFO[2][i]; MPU6050_FIFO[3][i - 1] = MPU6050_FIFO[3][i]; MPU6050_FIFO[4][i - 1] = MPU6050_FIFO[4][i]; MPU6050_FIFO[5][i - 1] = MPU6050_FIFO[5][i]; } MPU6050_FIFO[0][9] = ax; // 将新的数据放置到 数据的最后面 MPU6050_FIFO[1][9] = ay; MPU6050_FIFO[2][9] = az; MPU6050_FIFO[3][9] = gx; MPU6050_FIFO[4][9] = gy; MPU6050_FIFO[5][9] = gz; sum = 0; for (i = 0; i < 10; i++) { // 求当前数组的合,再取平均值 sum += MPU6050_FIFO[0][i]; } MPU6050_FIFO[0][10] = sum / 10; sum = 0; for (i = 0; i < 10; i++) { sum += MPU6050_FIFO[1][i]; } MPU6050_FIFO[1][10] = sum / 10; sum = 0; for (i = 0; i < 10; i++) { sum += MPU6050_FIFO[2][i]; } MPU6050_FIFO[2][10] = sum / 10; sum = 0; for (i = 0; i < 10; i++) { sum += MPU6050_FIFO[3][i]; } MPU6050_FIFO[3][10] = sum / 10; sum = 0; for (i = 0; i < 10; i++) { sum += MPU6050_FIFO[4][i]; } MPU6050_FIFO[4][10] = sum / 10; sum = 0; for (i = 0; i < 10; i++) { sum += MPU6050_FIFO[5][i]; } MPU6050_FIFO[5][10] = sum / 10; } /************************************************************************** Function: Setting the clock source of mpu6050 Input : source:Clock source number Output : none 函数功能:设置 MPU6050 的时钟源 入口参数:source:时钟源编号 返回 值:无 * CLK_SEL | Clock Source * --------+-------------------------------------- * 0 | Internal oscillator * 1 | PLL with X Gyro reference * 2 | PLL with Y Gyro reference * 3 | PLL with Z Gyro reference * 4 | PLL with external 32.768kHz reference * 5 | PLL with external 19.2MHz reference * 6 | Reserved * 7 | Stops the clock and keeps the timing generator in reset **************************************************************************/ void MPU6050_setClockSource(uint8_t source) { IICwriteBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source); } /** Set full-scale gyroscope range. * @param range New full-scale gyroscope range value * @see getFullScaleRange() * @see MPU6050_GYRO_FS_250 * @see MPU6050_RA_GYRO_CONFIG * @see MPU6050_GCONFIG_FS_SEL_BIT * @see MPU6050_GCONFIG_FS_SEL_LENGTH */ void MPU6050_setFullScaleGyroRange(uint8_t range) { IICwriteBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range); } /************************************************************************** Function: Setting the maximum range of mpu6050 accelerometer Input : range:Acceleration maximum range number Output : none 函数功能:设置 MPU6050 加速度计的最大量程 入口参数:range:加速度最大量程编号 返回 值:无 **************************************************************************/ // #define MPU6050_ACCEL_FS_2 0x00 //===最大量程+-2G // #define MPU6050_ACCEL_FS_4 0x01 //===最大量程+-4G // #define MPU6050_ACCEL_FS_8 0x02 //===最大量程+-8G // #define MPU6050_ACCEL_FS_16 0x03 //===最大量程+-16G void MPU6050_setFullScaleAccelRange(uint8_t range) { IICwriteBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range); } /************************************************************************** Function: Set mpu6050 to sleep mode or not Input : enable:1,sleep;0,work; Output : none 函数功能:设置 MPU6050 是否进入睡眠模式 入口参数:enable:1,睡觉;0,工作; 返回 值:无 **************************************************************************/ void MPU6050_setSleepEnabled(uint8_t enabled) { IICwriteBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled); } /************************************************************************** Function: Read identity Input : none Output : 0x68 函数功能:读取 MPU6050 WHO_AM_I 标识 入口参数:无 返回 值:0x68 **************************************************************************/ uint8_t MPU6050_getDeviceID(void) { IICreadBytes(devAddr, MPU6050_RA_WHO_AM_I, 1, buffer); return buffer[0]; } /************************************************************************** Function: Check whether mpu6050 is connected Input : none Output : 1:Connected;0:Not connected 函数功能:检测MPU6050 是否已经连接 入口参数:无 返回 值:1:已连接;0:未连接 **************************************************************************/ uint8_t MPU6050_testConnection(void) { if (MPU6050_getDeviceID() == 0x68) // 0b01101000; return 1; else return 0; } /************************************************************************** Function: Setting whether mpu6050 is the host of aux I2C cable Input : enable:1,yes;0;not Output : none 函数功能:设置 MPU6050 是否为AUX I2C线的主机 入口参数:enable:1,是;0:否 返回 值:无 **************************************************************************/ void MPU6050_setI2CMasterModeEnabled(uint8_t enabled) { IICwriteBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled); } /************************************************************************** Function: Setting whether mpu6050 is the host of aux I2C cable Input : enable:1,yes;0;not Output : none 函数功能:设置 MPU6050 是否为AUX I2C线的主机 入口参数:enable:1,是;0:否 返回 值:无 **************************************************************************/ void MPU6050_setI2CBypassEnabled(uint8_t enabled) { IICwriteBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled); } /************************************************************************** Function: initialization Mpu6050 to enter the available state Input : none Output : none 函数功能:初始化 MPU6050 以进入可用状态 入口参数:无 返回 值:无 **************************************************************************/ void MPU6050_initialize(void) { // 未识别陀螺仪,复位 if (MPU6050_getDeviceID() != 0x68) DL_SYSCTL_resetDevice(DL_SYSCTL_RESET_POR); MPU6050_setClockSource(MPU6050_CLOCK_PLL_YGYRO); // 设置时钟 MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_2000); // 陀螺仪量程设置 MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2); // 加速度度最大量程 +-2G MPU6050_setSleepEnabled(0); // 进入工作状态 MPU6050_setI2CMasterModeEnabled(0); // 不让MPU6050 控制AUXI2C MPU6050_setI2CBypassEnabled(0); // 主控制器的I2C与 MPU6050的AUXI2C 直通关闭 } /************************************************************************** Function: Initialization of DMP in mpu6050 Input : none Output : none 函数功能:MPU6050内置DMP的初始化 入口参数:无 返回 值:无 **************************************************************************/ void DMP_Init(void) { uint8_t resetflag = 0; uint8_t temp[1] = {0}; i2cRead(0x68, 0x75, 1, temp); printf("mpu_set_sensor complete ......\r\n"); if (temp[0] != 0x68) DL_SYSCTL_resetDevice(DL_SYSCTL_RESET_POR); if (!mpu_init()) { if (!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) printf("mpu_set_sensor complete ......\r\n"); else resetflag = 1; if (!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) printf("mpu_configure_fifo complete ......\r\n"); else resetflag = 1; if (!mpu_set_sample_rate(DEFAULT_MPU_HZ)) printf("mpu_set_sample_rate complete ......\r\n"); else resetflag = 1; if (!dmp_load_motion_driver_firmware()) printf("dmp_load_motion_driver_firmware complete ......\r\n"); else resetflag = 1; if (!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) printf("dmp_set_orientation complete ......\r\n"); else resetflag = 1; if (!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) printf("dmp_enable_feature complete ......\r\n"); else resetflag = 1; if (!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) printf("dmp_set_fifo_rate complete ......\r\n"); else resetflag = 1; run_self_test(); if (!mpu_set_dmp_state(1)) printf("mpu_set_dmp_state complete ......\r\n"); } else { DL_SYSCTL_resetDevice(DL_SYSCTL_RESET_POR); } if (resetflag) { mpu6050_i2c_sda_unlock(); DL_SYSCTL_resetDevice(DL_SYSCTL_RESET_POR); } } /************************************************************************** Function: Read the attitude information of DMP in mpu6050 Input : none Output : none 函数功能:读取MPU6050内置DMP的姿态信息 入口参数:无 返回 值:无 **************************************************************************/ void Read_DMP(void) { unsigned long sensor_timestamp; unsigned char more; long quat[4]; dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more); // 读取DMP数据 if (sensors & INV_WXYZ_QUAT) { q0 = quat[0] / q30; q1 = quat[1] / q30; q2 = quat[2] / q30; q3 = quat[3] / q30; // 四元数 Roll = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3; // 计出横滚角 Pitch = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3; // 计出俯仰角 Yaw = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3; // 计出偏航角 } } /************************************************************************** Function: Read mpu6050 built-in temperature sensor data Input : none Output : Centigrade temperature 函数功能:读取MPU6050内置温度传感器数据 入口参数:无 返回 值:摄氏温度 **************************************************************************/ int Read_Temperature(void) { float Temp; Temp = (I2C_ReadOneByte(devAddr, MPU6050_RA_TEMP_OUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_TEMP_OUT_L); if (Temp > 32768) Temp -= 65536; // 数据类型转换 Temp = (36.53 + Temp / 340) * 10; // 温度放大十倍存放 return (int)Temp; } //------------------End of File----------------------------
最新发布
07-22
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值