html飞行器代码,自编四轴飞控代码——开源共勉

//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;