//MPU6050相关设置
#include
#include
#include
float fRad2Deg = 57.295779513f; //将弧度转为角度的乘数
const int MPU = 0x68; //MPU-6050的I2C地址
const int nValCnt = 7; //一次读取寄存器的数量
const int nCalibTimes = 1000; //校准时读数的次数
int calibData[nValCnt]; //校准数据
float realVals[7];
unsigned long nCurTime;
unsigned long nLastTime = 0; //上一次读数的时间
float fLastRoll = 0.0f; //上一次滤波得到的Roll角
float fLastPitch = 0.0f; //上一次滤波得到的Pitch角
Kalman kalmanRoll; //Roll角滤波器
Kalman kalmanPitch; //Pitch角滤波器
/**控制接收端变量设置start***/
int i=0;
int iii=0;
int ii[3];
int ss[4];
int s=0;//speed
int a=0;// RX init
int h=0;
int p=0;
int staus=0;// 当前状态,为1时可飞
int pp=0;//限飞电压
int limit=90;//pwm最大输出(0-255)
/**控制接收端变量设置end***/
/**pid变量设置start***/
unsigned long t=0;
float top;
float left;
float topRate ;
float leftRate ;
float measured ;
int sampletime=4;//ms 控制间隔时间
//外环
double kp=0.001;//0.45
double ki=0.0035;
double kd=0.0015;//0.0023
//内环
double knp=0.025;//0.45
double kni=0.0035;
double knd=0.0055;//0.0023
int out1,out2,out3,out4;
float desired=0.5;//获取期望角度
int topp;//是否进行pid调整的中间变量
int leftp;//是否进行pid调整的中间变量
double error; //偏差:期望-测量值
double integ; //偏差积分
double iLimit; //作积分限制
double deriv; //微分应该可用陀螺仪角速度代替
double prerror;前一次偏差
double prlerror;前一次偏差
/**pid变量设置end***/
/**电机引脚变量设置start***/
int pin1=4;
int pin2=5;
int pin3=10;
int pin4=11;
/**电机引脚变量设置end***/
/**PID调整函数***/
void pid()
{
h=s; //遥控传进来的speed值赋值给h变量
int ol=(nCurTime-t)/1000; //获取间隔时间 单位ms
if(ol>sampletime){ //此处判断是否进行调节
t=nCurTime;
measured=top; //获取roll角度
/**PID外环roll方向 start***/
error = desired - measured; //偏差:期望-测量值
float pout=kp*error; //pid外环 P项比例输出
float iout=ki*error*sampletime; //pid外环 i项积分输出
float rout=pout+iout; //pid外环 外环输出
/**PID外环roll方向 end***/
/**PID内环roll方向 start***/
error =rout-topRate;