首页 › 四轴飞行器 › 自编四轴飞控代码——开源共勉

项目承接 发表于 2018-8-23 12:35:02

自编四轴飞控代码——开源共勉

本帖最后由 项目承接 于 2018-8-23 12:37 编辑

废话不说直接上代码，代码还在持续更新中，四轴DIY交流群：651925582
码云地址：https://gitee.com/s8888/four_axis_diy_code
当前飞机飞行调试状态视频
https://pan.baidu.com/s/186_9qj5sfNYck61N9ZJl_Q


遥控端

/**x轴，y轴，Speed模拟量读取变量定义 start***/
int x=0;
int y=0;
int s=0;

int x1=0;
int y1=0;
int s1=0;
int s2=0;
int ss=0;
/**x轴，y轴，Speed模拟量读取变量定义end***/

int i=0;//飞控端发回状态值变量

int op=0;//状态开关值

void setup() {
    /**串口初始化 start***/
   Serial2.begin(115200);
   Serial.begin(115200);
   /**串口初始化 end***/
   
pinMode(11,OUTPUT);       //蜂鸣器引脚初始化
pinMode(3,INPUT_PULLUP);//遥控启动引脚初始化

/**遥控开机提示 start***/
for(int d=2;d>0;d--)
{
tone(11,2);
delay(d*100);
noTone(11);
delay(100);
}
/**遥控开机提示 end***/

}

void loop() {
/**遥控启动 start***/
if((digitalRead(3)==0)&&(op==0))
{
op=1;
}
/**遥控启动 end***/

/**拨码开关遥控停止 start***/
if((digitalRead(3)==1)&&(op==1))
{for(int d=2;d>0;d--)
{
tone(11,2);
delay(d*200);
noTone(11);
delay(100);
}
op=0;
   Serial2.print("$");//36//停止飞行指令
   Serial2.print("!");//33 用于将每次传入的数值读取到，防止缓冲导致读取的不及时！

}
/**拨码开关 遥控停止 end***/

/**遥控端接收 start***/
if( Serial2.available()>1)
{ i=Serial2.read();
// Serial.println(i);
}
/**遥控端接收 end***/

/**飞控低压停止飞行 start***/
if(i==49)
{
if(op==1)
{
tone(11,5);
delay(100);
noTone(11);
delay(100);
}
op==0;      //遥控停止状态值
i=0;      //遥控接收端数值归零
}
/**飞控低压停止飞行 end***/


   /**遥控模拟量读取 start***/
       x=analogRead(A0);
       y=analogRead(A1);
       s=analogRead(A2)/7;
      /**遥控模拟量读取 end***/

      /**x轴，y轴，Speed 遥控模拟量输出值设置 start***/
if(x>950)
    x=1;
else if(x<50)
   x=2;
   else
   x=0;
if(y>950)
    y=1;
else if(y<50)
    y=2;
else
y=0;
if(s<15)
s=0;
if(s>120)
s=120;
    /**x轴，y轴，Speed 遥控模拟量输出值设置 end***/

   /**遥控发送 start***/
if((s!=s2)&&(op==1))
{
pt();   //调用发送函数
}
/**遥控发送 end***/
}
/**遥控发送函数 start***/
void pt()
{
   /**防止速度间差值过大引起剧烈反应 start***/
ss=s1-s;
if(ss<-25)
s1=s-25;
else if(ss>25)
s1=s1-25;
else
s1=s;
   /**防止速度间差值过大引起剧烈反应 end***/

x1=x;
y1=y;

s2=s;
   if(s!=0)
{
/**遥控端发送格式 start***/
   Serial2.print("&");//38
   Serial2.print(x1);
   Serial2.print("!");//33
   Serial2.print(y1);
   Serial2.print("!");
   Serial2.print(s1);
   Serial2.print("!");
   Serial2.print("%");//37
   Serial2.print("&");//38 用于将每次传入的数值读取到，防止缓冲导致读取的不及时！
      /**遥控端发送格式 end***/
}
/**遥控端停止飞行指令 start***/
   else
   {
if(ss>20)
{
      s1=20;
   for(s1;s1>0;s1=s1-5)
      {
    Serial2.print("&");//38
    Serial2.print(x1);
    Serial2.print("!");//33
    Serial2.print(y1);
    Serial2.print("!");
    Serial2.print(s1);
    Serial2.print("!");
    Serial2.print("%");//37
    Serial2.print("&");//38 用于将每次传入的数值读取到，防止缓冲导致读取的不及时！
   delay(10);
   }
}
   Serial2.print("&");//38
   Serial2.print(x1);
   Serial2.print("!");//33
   Serial2.print(y1);
   Serial2.print("!");
   Serial2.print(0);
   Serial2.print("!");
   Serial2.print("%");//37
   Serial2.print("&");//38 用于将每次传入的数值读取到，防止缓冲导致读取的不及时！
   delay(10);
}
/**遥控端停止飞行指令 end***/

}
/**遥控发送函数 end***/




飞控端

//MPU6050相关设置
#include <Kalman.h>
#include <Wire.h>
#include <Math.h>
float fRad2Deg = 57.295779513f; //将弧度转为角度的乘数
const int MPU = 0x68; //MPU-6050的I2C地址
const int nValCnt = 7; //一次读取寄存器的数量
const int nCalibTimes = 1000; //校准时读数的次数
int calibData; //校准数据
   float realVals;
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;
int ss;
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;         //角速度偏差：外环积分值-陀螺仪roll角速度测量值直接用的
   pout=knp*error;                //pid内环 P项比例输出
   iout=kni*error*sampletime;   //pid内环 I项积分输出
   float dout=knd*(error-prerror);//pid内环 D项微分输出
   prerror=error;               //保存本次误差
    float rrout=pout+iout+dout;   //pid内环 输出
   /**PID内环roll方向 end***/

      /**PID外环pitch方向start***/
    measured=left;//pitch            //获取pitch角度

    error = desired - measured;      //偏差：期望-测量值
    pout=kp*error;                     //pid外环 P项比例输出
    iout=ki*error*sampletime;          //pid外环 I项积分输出
    rout=pout+iout;                  //pid外环 D项微分输出
      /**PID外环pitch方向 end***/

       /**PID内环pitch方向 start***/
   error =rout-leftRate;             //角速度偏差：外环积分值-陀螺仪pitch角速度测量值直接用的
   pout=knp*error;                   //pid内环 P项比例输出

   iout=kni*error*sampletime;      //pid内环 D项微分输出
   dout=knd*(error-prlerror);      //pid内环 D项微分输出
   prlerror=error;                   //保存本次误差
   floatpiout=pout+iout+dout;      //pid内环 输出
      /**PID内环pitch方向 end***/

      /**PWM输出 start***/
out1=s-piout+rrout;
out3=s+piout-rrout;
out2=s-piout-rrout;
out4=s+piout+rrout;
      /**PWM输出 end***/

   /**PWM输出限幅 start***/
if(out1>limit)
out1=limit;
if(out2>limit)
out2=limit;
if(out3>limit)
out3=limit;
if(out4>limit)
out4=limit;
/**PWM输出限幅 end***/

/**PWM停止输出判断 start***/
if(out3<0)
out3=0;
if(out2<0)
out2=0;
if(out1<0)
out1=0;
if(out4<0)
out4=0;
if(s==0)
{
out1=0;
out2=0;
out3=0;
out4=0;
}
/**PWM停止输出判断 end***/

/**电路电源判断 start***/
//p=analogRead(A6);
//Serial.print("dianyuan:");
//Serial.println(p);
/**电路电源判断 end***/

/**PWM输出串口检测 start***/
//Serial.print("out1:");
//Serial.println(out1);
//Serial.print("out3:");
//Serial.println(out3);
//Serial.print("out2:");
//Serial.println(out2);
//Serial.print("out4:");
//Serial.println(out4);
/**PWM输出串口检测 end***/

/**PWM输出 start***/
analogWrite(pin1,out1);
analogWrite(pin2,out2);
analogWrite(pin3,out3);
analogWrite(pin4,out4);
   /**PWM输出 end***/
}
}



void setup() {
   /**飞控开机提示 start***/
pinMode(25,OUTPUT);//蜂鸣器引脚
for(int d=1;d<4;d++)
{
tone(25, 3);
delay(100);
noTone(25);
delay(100);
}
   /**飞控开机提示end***/

    /**MPU6050初始化 start***/
   Wire.begin(); //初始化Wire库
   WriteMPUReg(0x6B, 0); //启动MPU6050设备
   Calibration(); //执行校准
   nLastTime = micros(); //记录当前时间
    /**MPU6050初始化 end***/

    /**串口初始化 start***/
   Serial2.begin(115200);
   Serial.begin(115200);
   /**串口初始化 end***/

    /**电机输出口初始化 start***/
   pinMode(pin1,OUTPUT);
   pinMode(pin2,OUTPUT);
   pinMode(pin3,OUTPUT);
   pinMode(pin4,OUTPUT);
   /**电机输出口初始化 end***/


}

void loop() {

/**MPU6050测量结果输出 start***/
int readouts;
ReadAccGyr(readouts); //读出测量值
Rectify(readouts, realVals); //根据校准的偏移量进行纠正
//计算加速度向量的模长，均以g为单位
float fNorm = sqrt(realVals * realVals + realVals * realVals + realVals * realVals);
float fRoll = GetRoll(realVals, fNorm); //计算Roll角
if (realVals > 0) {
    fRoll = -fRoll;
}
float fPitch = GetPitch(realVals, fNorm); //计算Pitch角
if (realVals < 0) {
    fPitch = -fPitch;
}
//计算两次测量的时间间隔dt，以秒为单位
   nCurTime = micros();
float dt = (double)(nCurTime - nLastTime) / 1000000.0;
//对Roll角和Pitch角进行卡尔曼滤波
float fNewRoll = kalmanRoll.getAngle(fRoll, realVals, dt);
float fNewPitch = kalmanPitch.getAngle(fPitch, realVals, dt);
//跟据滤波值计算角度速
float fRollRate = (fNewRoll - fLastRoll) / dt;
float fPitchRate = (fNewPitch - fLastPitch) / dt;
   //更新本次测的时间
nLastTime = nCurTime;
//更新Roll角和Pitch角
fLastRoll = fNewRoll;//top
fLastPitch = fNewPitch;//left
/**MPU6050测量结果输出 end***/

   
   /**pid 涉及相关变量初始化 start***/
    topRate=fRollRate;
    leftRate= fPitchRate;
    top=fNewRoll;
    left=fNewPitch;
   /**pid 涉及相关变量初始化 end***/

      /**pid 本次是否进行调整判断 start***/
    if(top<-1)
topp=-top;
    else
      topp=top;
         if(left<-1)
leftp=-left;
    else
      leftp=left;
p=analogRead(A6);//电路电压测量

   if(((h!=s)||(topp>desired)||(leftp>desired))&&(staus==1)&&(p>pp))
pid();
/**pid 本次是否进行调整判断 end***/


       /**遥控数据接收 start***/
if( Serial2.available()>1)
{ i=Serial2.read();
if(i>0)
math();   //遥控数据处理函数调用
}
   /**遥控数据接收 end***/


/**电源判断，低压停止飞行 start***/
//   if((p<pp)&&(s>0)&&(staus==1))
// {
// power();
//
// }
/**电源判断，低压停止飞行 end***/

}



//向MPU6050写入一个字节的数据
//指定寄存器地址与一个字节的值
void WriteMPUReg(int nReg, unsigned char nVal) {
Wire.beginTransmission(MPU);
Wire.write(nReg);
Wire.write(nVal);
Wire.endTransmission(true);
}

//从MPU6050读出一个字节的数据
//指定寄存器地址，返回读出的值
unsigned char ReadMPUReg(int nReg) {
Wire.beginTransmission(MPU);
Wire.write(nReg);
Wire.requestFrom(MPU, 1, true);
Wire.endTransmission(true);
return Wire.read();
}

//从MPU6050读出加速度计三个分量、温度和三个角速度计
//保存在指定的数组中
void ReadAccGyr(int *pVals) {
Wire.beginTransmission(MPU);
Wire.write(0x3B);
Wire.requestFrom(MPU, nValCnt * 2, true);
Wire.endTransmission(true);
for (long i = 0; i < nValCnt; ++i) {
    pVals = Wire.read() << 8 | Wire.read();
}
}

//对大量读数进行统计，校准平均偏移量
void Calibration()
{
float valSums = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0};
//先求和
for (int i = 0; i < nCalibTimes; ++i) {
    int mpuVals;
    ReadAccGyr(mpuVals);
    for (int j = 0; j < nValCnt; ++j) {
      valSums += mpuVals;
    }
}
//再求平均
for (int i = 0; i < nValCnt; ++i) {
    calibData = int(valSums / nCalibTimes);
}
calibData += 16384; //设芯片Z轴竖直向下，设定静态工作点。
}

//算得Roll角。算法见文档。
float GetRoll(float *pRealVals, float fNorm) {
float fNormXZ = sqrt(pRealVals * pRealVals + pRealVals * pRealVals);
float fCos = fNormXZ / fNorm;
return acos(fCos) * fRad2Deg;
}

//算得Pitch角。算法见文档。
float GetPitch(float *pRealVals, float fNorm) {
float fNormYZ = sqrt(pRealVals * pRealVals + pRealVals * pRealVals);
float fCos = fNormYZ / fNorm;
return acos(fCos) * fRad2Deg;
}

//对读数进行纠正，消除偏移，并转换为物理量。公式见文档。
void Rectify(int *pReadout, float *pRealVals) {
for (int i = 0; i < 3; ++i) {
    pRealVals = (float)(pReadout - calibData) / 16384.0f;
}
pRealVals = pReadout / 340.0f + 36.53;
for (int i = 4; i < 7; ++i) {
    pRealVals = (float)(pReadout - calibData) / 131.0f;
}
}


//遥控数据处理函数
void math()
{
   /**停止飞行声音提醒 start***/
if((i==36)&&(p>pp))
{staus=0;    //是否可以飞行状态值

   stop_f();   //停止飞行函数
   for(int d=2;d>0;d--)
{
tone(25,3);
delay(d*200);
noTone(25);
delay(100);
}
}
/**停止飞行声音提醒 end***/

   if(i==38)
{
    a=1;
   
}
if(i==33)
{
if(iii==1)
ss=ii;
if(iii==2)
ss=ii*10+ii;
if(iii==3)
ss=ii*100+ii*10+ii;
iii=0;
a=a+1;

}
if(i==37)
{
//x1=ss;//x轴控制值
//y1=ss;//x轴控制值

s=ss;   //Speed控制值


//是否可以飞行状态值 设置
if((p>pp)&&(s>0))
staus=1;

   a=0;       //数据接收控制符
}
if(i>47)
{
   iii=iii+1;
   if(iii==1)
   ii=i-48;
    if(iii==2)
   ii=i-48;
if(iii==3)
   ii=i-48;
      i=0;
}
}
/**遥控停止飞行函数 start***/
void stop_f()
{
Serial2.print(p);
if(s>10){
for(int sss=s;sss>0;sss--)
{
    analogWrite(pin1,sss);
    analogWrite(pin2,sss);
    analogWrite(pin3,sss);
    analogWrite(pin4,sss);

delay(10);

}
   analogWrite(pin1,0);
   analogWrite(pin2,0);
   analogWrite(pin3,0);
   analogWrite(pin4,0);
}
s=0;
staus=0;
}
/**遥控停止飞行函数 end***/

/**低压停止飞行函数 start***/
void power()
{
Serial2.print(1);//低压报警值，传至遥控端
if(s>0){
for(int sss=s;sss>0;sss--)
{
   
    analogWrite(pin1,sss);
    analogWrite(pin2,sss);
    analogWrite(pin3,sss);
    analogWrite(pin4,sss);

delay(10);

}
    analogWrite(pin1,0);
    analogWrite(pin2,0);
    analogWrite(pin3,0);
    analogWrite(pin4,0);                                                                                                                                                                                                                                                                                                                                                                                                      

}
   s=0;
staus=0;
}
/**低压停止飞行函数 end***/



电路图正在整理ing~


当前飞机飞行调试状态视频
https://pan.baidu.com/s/186_9qj5sfNYck61N9ZJl_Q

HX711学习者 发表于 2018-8-23 15:44:50

赞:loveliness:

wing 发表于 2018-8-26 10:15:09

非常不错的项目，
但是为什么用水平拉线来固定机体呢？是不是出现了比较麻烦的自旋现象？

wing 发表于 2018-8-26 10:22:42

我很想star你的项目，不过没有码云账号，然而用github帐号关联马云原来很麻烦，话说我也要告别github，所以算了。
☆☆只能在这里给你了

项目承接 发表于 2018-8-26 19:08:14

wing 发表于 2018-8-26 10:15
非常不错的项目，
但是为什么用水平拉线来固定机体呢？是不是出现了比较麻烦的自旋现象？

恩，目前有点自旋问题，同时平衡也有问题:lol

项目承接 发表于 2018-8-26 19:09:14

wing 发表于 2018-8-26 10:22
我很想star你的项目，不过没有码云账号，然而用github帐号关联马云原来很麻烦，话说我也要告别github，所以 ...

哈哈，github后期会补上，最近家里网络不太好:P

项目承接 发表于 2018-8-26 19:09:51

HX711学习者 发表于 2018-8-23 15:44
赞

昂，:loveliness:

PINKWALKMAN 发表于 2018-8-27 14:08:12

建议参照APM的改，完全自己写走的弯路太多了。

查看完整版本: 自编四轴飞控代码——开源共勉