#include "stm32f10x.h"
#include "I2C_MPU6050.h"
#include "USART.h"
#include "SysTick.h"
#include "math.h"
#include "app.h"
#include "control.h"
#include "usmart.h"
#include "timer.h"
#include "NRF24L01.h"
#include "spi.h"
#include <stdio.h>
//二阶互补滤波
#define pi 3.141592653
float K2 =0.2; // 对加速度计取值的权重
float dt=0.016;//dt为kalman滤波器采样时间;
float x1,x2,y1;
float m1,m2,n1;
float anglex2,angley2,anglez2;
float Roll=0,Yaw=0,Pitch=0;
float Roll_tar=0,Yaw_tar=0,Pitch_tar=0;
float Roll_o=0,Yaw_o=0,Pitch_o=0;
float Accel_x; //X轴加速度值暂存
float Accel_y; //Y轴加速度值暂存
float Accel_z; //Z轴加速度值暂存
float Gyro_x; //X轴陀螺仪数据暂存
float Gyro_y; //Y轴陀螺仪数据暂存
float Gyro_z; //Z轴陀螺仪数据暂存
float Angle_x_temp; //由加速度计算的x倾斜角度
float Angle_y_temp; //由加速度计算的y倾斜角度
float Angle_z_temp; //由加速度计算的y倾斜角度
float Angle_X_Final; //X最终倾斜角度
float Angle_Y_Final; //Y最终倾斜角度
float Angle_Z_Final; //z最终倾斜角度
long temp;
float Q_jiao;
unsigned char systick;
int16_t gx,gy,gz;//存储原始数据
float ggx,ggy,ggz;//存储量化后的数据
float Ax,Ay,Az;//单位 g(9.8m/s^2)
u16 THROTTLE=1300;
unsigned char tmp_buf[33];
unsigned int motor1,motor2,motor3,motor4;
unsigned char buff[7];
//unsigned int x,y,z;
//double angle;
void Angle_Calcu(void);
void MPU6050(void);
void Erjielvbox(float angle_m,float gyro_m);
void Erjielvboy(float angle_m,float gyro_m);
void Erjielvboz(float angle_m,float gyro_m);
float hmc_measure();
void Init_HMC5883L();
void yaw(void);
void MPU6050()
{
gx=GetData(GYRO_XOUT_H);
gy=GetData(GYRO_YOUT_H);
gz=GetData(GYRO_ZOUT_H);
ggx=gy/65.6; //陀螺仪量程+-500度
ggy=-gx/65.6; //陀螺仪量程处理
ggz=-gz/65.6;
// printf("%f,",ggx);
//// printf("%f,",ggy);
// printf("%f,\r\n",ggz);
}
//NRF24L01接收
void jieshou(void)
{
RX_Mode();
if(NRF24L01_RxPacket(tmp_buf)==0)//一旦接收到信息,则显示出来.
{
tmp_buf[32]=0;//加入字符串结束符
switch(tmp_buf[0])
{
case '1': if(THROTTLE<1400) {THROTTLE=-500;} break; //停止键
case '2': THROTTLE=THROTTLE+5; break; //上升
case '3': THROTTLE=THROTTLE-5; break; //下降
case '4': Q_jiao++;if(Q_jiao>10) {Q_jiao=5;} break; //倾角增大
case '5': Q_jiao--;if(Q_jiao<1) {Q_jiao=0; } break; //倾角减小
case '6': Q_jiao=5; break; //倾角15
case '7': THROTTLE=1350; break;
case '8': Q_jiao=0; break;
case '9': break;
case 'A': Roll_tar=Q_jiao; Pitch_tar=0; break;
case 'B': Roll_tar=0; Pitch_tar=Q_jiao; break;
case 'C': Roll_tar=0; Pitch_tar=-Q_jiao; break;
case 'D': Roll_tar=-Q_jiao; Pitch_tar=0; break;
}
}
}
/*
* 函数名：main
* 描述 ：主函数
* 输入 ：无
* 输出 ：无
* 返回 ：无
*/
int main(void)
{
SystemInit(); //系统时钟初始化
Usart_Configuration();//串口初始化配置
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //串口中断初始化
usmart_dev.init(SystemCoreClock/1000000); //usmart初始化
I2C_MPU6050_Init();
SysTick_Init();
/*时钟启动*/
TimeStart();
/*陀螺仪传感器初始化*/
InitMPU6050();
// Init_HMC5883L();
Pid_init(60,0,34,340,7,0); //24
All_GPIO_Config();//所有GPIO配置
TIM3_Config();//TIM2初始化配置
NRF24L01_Init(); //初始化NRF24L01
RX_Mode();
while(NRF24L01_Check())//检测不到24L01
{
printf("nRF24L01检测出错!请确认nRF24L01的连接! \n\r");
delay_ms(1000);
}
printf("\n\r--------------------------------------");
printf("\n\r 神舟III号2.4G无线模块实验程序");
printf("\n\r\n\r 请设置NRF24L01无线模块的工作模式");
printf("\n\r --USER1 按键:设置NRF24L01为接收模式");
printf("\n\r --USER2 按键:设置NRF24L01为发送模式");
printf("\n\r --TAMEPR按键:退出NRF24L01发送接收");
printf("\n\r--------------------------------------");
//motor需从1000开始向上加，否则无法启动
motor1=motor2=motor3=motor4=1000; time(); delay_ms(1000);
motor1=motor2=motor3=motor4=1100; time(); delay_ms(1000);
motor1=motor2=motor3=motor4=1200; time(); delay_ms(1000);
motor1=motor2=motor3=motor4=1300; time(); delay_ms(1000);
while(1)
{
// 关闭滴答定时器
SysTick->CTRL &= ~ SysTick_CTRL_ENABLE_Msk;
printf("%d,",systick);
systick=0;
//使能
SysTick->CTRL |= SysTick_CTRL_ENABLE_Msk;
// Angle_z_temp=hmc_measure();
// MPU6050();
// Angle_Calcu();
// jieshou();
// Control(Roll,Pitch,Yaw,Roll_tar,Pitch_tar,Yaw_tar);//4通道 油门+方向
// time();
}
}
//角度计算
void Angle_Calcu(void)
{
float x,y,z;
Accel_x = GetData(ACCEL_XOUT_H); //x轴加速度值暂存
Accel_y = GetData(ACCEL_YOUT_H); //y轴加速度值暂存
Accel_z = GetData(ACCEL_ZOUT_H); //z轴加速度值暂存
Gyro_x = GetData(GYRO_XOUT_H); //x轴陀螺仪值暂存
Gyro_y = GetData(GYRO_YOUT_H); //y轴陀螺仪值暂存
Gyro_z = GetData(GYRO_ZOUT_H); //z轴陀螺仪值暂存
//处理x轴加速度
if(Accel_x<32764) x=Accel_x/16384;
else x=1-(Accel_x-49152)/16384;
//处理y轴加速度
if(Accel_y<32764) y=Accel_y/16384;
else y=1-(Accel_y-49152)/16384;
//处理z轴加速度
if(Accel_z<32764) z=Accel_z/16384;
else z=(Accel_z-49152)/16384;
//用加速度计算三个轴和水平面坐标系之间的夹角
Angle_x_temp=(atan(y/z))*180/pi;
Angle_y_temp=(atan(x/z))*180/pi;
//角度的正负号
if(Accel_x<32764) Angle_y_temp = +Angle_y_temp;
if(Accel_x>32764) Angle_y_temp = -Angle_y_temp;
if(Accel_y<32764) Angle_x_temp = +Angle_x_temp;
if(Accel_y>32764) Angle_x_temp = -Angle_x_temp;
//向前运动
if(Gyro_x<32768) Gyro_x=+(Gyro_x/16.4);//范围为1000deg/s时，换算关系：16.4 LSB/(deg/s)
//向后运动
if(Gyro_x>32768) Gyro_x=-(65535-Gyro_x)/16.4;
//向前运动
if(Gyro_y<32768) Gyro_y=+(Gyro_y/16.4);//范围为1000deg/s时，换算关系：16.4 LSB/(deg/s)
//向后运动
if(Gyro_y>32768) Gyro_y=-(65535-Gyro_y)/16.4;
Erjielvbox(Angle_x_temp,Gyro_x);
Erjielvboy(Angle_y_temp,-Gyro_y);
Erjielvboz(Angle_z_temp,ggz);
Pitch=-anglex2-1;
Roll=-angley2+2.2;
Yaw= anglez2-180;
// printf("%f,",Yaw);
// printf("%f,",Roll);
// printf("%f,\r\n",Pitch);
//// printf("%f,",Yaw);
}
void Erjielvbox(float angle_m,float gyro_m)
{
x1=(angle_m-anglex2)*(1-K2)*(1-K2);
y1=y1+x1*dt;
x2=y1+2*(1-K2)*(angle_m-anglex2)+gyro_m;
anglex2=anglex2+ x2*dt;
}
void Erjielvboy(float angle_m,float gyro_m)
{
m1=(angle_m-angley2)*(1-K2)*(1-K2);
n1=n1+m1*dt;
m2=n1+2*(1-K2)*(angle_m-angley2)+gyro_m;
angley2=angley2+ m2*dt;
}
void Erjielvboz(float angle_m,float gyro_m)
{
m1=(angle_m-anglez2)*(1-K2)*(1-K2);
n1=n1+m1*dt;
m2=n1+2*(1-K2)*(angle_m-anglez2)+gyro_m;
anglez2=anglez2+ m2*dt;
}
|
QQ好友和群
QQ空间
腾讯微博
腾讯朋友
收藏
淘帖
顶
踩
