四旋翼源码第7版软启动+蓝牙源码（串级PID控制） -

#include "delay.h"
#include "sys.h"
#include "usart.h"
#include "anbt_dmp_fun.h"
#include "anbt_i2c.h"
#include "anbt_dmp_mpu6050.h"
#include "anbt_dmp_driver.h"
#include "debug.h"
#include "PWM.h"
#define BYTE0(dwTemp) (*(char *)(&dwTemp))
#define BYTE1(dwTemp) (*((char *)(&dwTemp) + 1))
#define BYTE2(dwTemp) (*((char *)(&dwTemp) + 2))
#define BYTE3(dwTemp) (*((char *)(&dwTemp) + 3))
#define q30 1073741824.0f
void Data_Send_Status(float Pitch,float Roll,float Yaw,int16_t *gyro,int16_t *accel);
void Send_Data(int16_t *Gyro,int16_t *Accel);
static void DMP_Delay ( __IO uint32_t nCount )
{
for ( ; nCount != 0; nCount -- );
}
int main(void)
{
int a = 0;
float error_pitch=0.0;
static float Pitch_I_out=0.0;
float error_pitch_old=0.0;
static float Pitch_I_shell_out=0.0;
float error_pitch_shell=0.0;
float pitch_shell_out=0.0;
float pitch_out=0.0;
float error_roll=0.0;
static float roll_I_out=0.0;
float error_roll_old=0.0;
static float roll_I_shell_out=0.0;
float error_roll_shell=0.0;
float roll_shell_out=0.0;
float roll_out=0.0;
static float yaw_I_out=0.0;
float error_yaw=0.0;
float error_yaw_old=0.0;
float yaw_out=0.0;
int i=1000000;
float m1=0,m2=0,m3=0,m4=0,m5=0.0;
unsigned long sensor_timestamp;
short gyro[3], accel[3], sensors;//陀螺仪存放数组，加速度存放数组，返回状态量
unsigned char more;
long quat[4];//四元数存放数组
float Yaw=0.00,Roll,Pitch;//欧拉角
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;//计算姿态过程用到的变量
////
pitch_SET = 7;
P_pitch_shell =-25;//-55
P_pitch = -0.72; //-1.2
I_pitch= -0.015;// -0.032
D_pitch=-0.85;//-1.2
I_pitch_shell =-0.00012;//-0.0002
roll_SET = 9;
P_roll_shell= -25;//-55
P_roll = 0.72;//1.2
I_roll= 0.015;//0.032
D_roll= 0.85;//1.2
I_roll_shell = 0.00012;//0.0002
NVIC_Configuration();//设置NVIC中断分组2:2位抢占优先级，2位响应优先级
uart_init(115200); //串口初始化为115200
//引用圆点博士的I2C程序，这里跟我们平常没有什么区别
PWM_Init();
ANBT_I2C_Configuration(); //IIC初始化
// delay_ms(30);
DMP_Delay (300);//
AnBT_DMP_MPU6050_Init(); //6050DMP初始化
//TIM2_Int_Init(4999,7199);//10Khz的计数频率，计数到5000为500ms
DMP_Delay (300);
while(i--)
{
TIM_SetCompare3(TIM3,200);
TIM_SetCompare4(TIM3,200);
TIM_SetCompare3(TIM4,200);
TIM_SetCompare4(TIM4,200);
}
//a = ReceiveOneByte[0];
while(1)
{
error_pitch_old = error_pitch;
error_roll_old = error_roll;
error_yaw_old = error_yaw;
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more);
if ( sensors & INV_WXYZ_QUAT )
{
q0=quat[0] / q30;
q1=quat[1] / q30;
q2=quat[2] / q30;
q3=quat[3] / q30;
Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //感觉没有价值，注掉
// if(Pitch>15||Pitch<-15||Roll>15||Roll<-15)//stop
// {
// TIM_SetCompare3(TIM3,0);
// TIM_SetCompare4(TIM3,0);
// TIM_SetCompare3(TIM4,0);
// TIM_SetCompare4(TIM4,0);
// while(1);
// }
//////////////////Pitch外环(PI)/////////////
error_pitch_shell = Pitch-pitch_SET;
Pitch_I_shell_out+=error_pitch_shell;
if(Pitch_I_shell_out>500)
{
Pitch_I_shell_out = 500;
}
if(Pitch_I_shell_out<-500)
{
Pitch_I_shell_out = -500;
}
pitch_shell_out = P_pitch_shell*error_pitch_shell+I_pitch_shell*Pitch_I_shell_out;
//------------Pitch内环PID gyro[1]---------------//
error_pitch = gyro[1]-pitch_shell_out;
Pitch_I_out+=error_pitch;
if(Pitch_I_out>500)
{
Pitch_I_out = 500;
}
if(Pitch_I_out<-500)
{
Pitch_I_out = -500;
}
pitch_out = P_pitch*error_pitch+I_pitch*Pitch_I_out+D_pitch*(error_pitch-error_pitch_old);
m1=pitch_out;
//////////////////roll外环(PI)gyro[0]/////////////
error_roll_shell = Roll - roll_SET;
roll_I_shell_out+=error_roll_shell;
if(roll_I_shell_out>1000)
{
roll_I_shell_out = 1000;
}
if(roll_I_shell_out<-1000)
{
roll_I_shell_out = -1000;
}
roll_shell_out = P_roll_shell*error_roll_shell+I_roll_shell*roll_I_shell_out;
//////////////////roll内环PID/////////
error_roll = gyro[0]-roll_shell_out;
roll_I_out+=error_roll;
if(roll_I_out>1000)
{
roll_I_out = 1000;
}
if(roll_I_out<-1000)
{
roll_I_out = -1000;
}
roll_out = P_roll*error_roll+I_roll*roll_I_out+D_roll*(error_roll-error_roll_old);
m2 = roll_out;
////////////////gyro[2] PD//////////
error_yaw = gyro[2];
yaw_out = 1.8*error_yaw+2.5*(error_yaw_old-error_yaw);
m3 = yaw_out;
/////////////////////yaw///////////
// error_yaw = Yaw;
// yaw_I_out+=error_yaw;
// if(yaw_I_out>200)
// {
// yaw_I_out = 200;
// }
// if(yaw_I_out<-200)
// {
// yaw_I_out = -200;
// }
// m4 = 0.0000025*error_yaw+0.000000025*yaw_I_out;
TIM_SetCompare3(TIM3,2500+m1+m2+m3+m4+m5);
TIM_SetCompare4(TIM3,2500+m1-m2-m3-m4+m5);
TIM_SetCompare3(TIM4,2500-m1-m2+m3+m4+m5);
TIM_SetCompare4(TIM4,2500-m1+m2-m3-m4+m5);
Push(1,(int)pitch_out);
Push(2,(int)Pitch);
Push(3,(int)gyro[0]);
Push(4,(int)gyro[1]);
Push(5,(int)gyro[2]);
Push(6,(int)Yaw);
SendDataToScope();
}
}
}
/*函数功能：根据匿名最新上位机协议写的显示姿态的程序
*具体原理看匿名的讲解视频
发送基本信息（姿态、锁定状态）
*/
void Data_Send_Status(float Pitch,float Roll,float Yaw,int16_t *gyro,int16_t *accel)
{
unsigned char i=0;
unsigned char _cnt=0,sum = 0;
unsigned int _temp;
u8 data_to_send[50];
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x01;
data_to_send[_cnt++]=0;
_temp = (int)(Roll*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = 0-(int)(Pitch*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(Yaw*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[3] = _cnt-4;
//和校验
for(i=0;i<_cnt;i++)
sum+= data_to_send[i];
data_to_send[_cnt++]=sum;
//串口发送数据
for(i=0;i<_cnt;i++)
AnBT_Uart1_Send_Char(data_to_send[i]);
}
//发送传感器数据
void Send_Data(int16_t *Gyro,int16_t *Accel)
{
unsigned char i=0;
unsigned char _cnt=0,sum = 0;
// unsigned int _temp;
u8 data_to_send[50];
……………………
…………限于本文篇幅余下代码请从51黑下载附件…………

复制代码