本帖最后由 项目承接 于 2018-8-23 12:37 编辑
废话不说直接上代码,代码还在持续更新中,四轴DIY交流群:651925582
码云地址:https://gitee.com/s8888/four_axis_diy_code
当前飞机飞行调试状态视频
https://pan.baidu.com/s/186_9qj5sfNYck61N9ZJl_Q
遥控端
飞控端
- //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[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; //角速度偏差:外环积分值-陀螺仪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; //保存本次误差
- float piout=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[nValCnt];
- ReadAccGyr(readouts); //读出测量值
- Rectify(readouts, realVals); //根据校准的偏移量进行纠正
- //计算加速度向量的模长,均以g为单位
- float fNorm = sqrt(realVals[0] * realVals[0] + realVals[1] * realVals[1] + realVals[2] * realVals[2]);
- float fRoll = GetRoll(realVals, fNorm); //计算Roll角
- if (realVals[1] > 0) {
- fRoll = -fRoll;
- }
- float fPitch = GetPitch(realVals, fNorm); //计算Pitch角
- if (realVals[0] < 0) {
- fPitch = -fPitch;
- }
- //计算两次测量的时间间隔dt,以秒为单位
- nCurTime = micros();
- float dt = (double)(nCurTime - nLastTime) / 1000000.0;
- //对Roll角和Pitch角进行卡尔曼滤波
- float fNewRoll = kalmanRoll.getAngle(fRoll, realVals[4], dt);
- float fNewPitch = kalmanPitch.getAngle(fPitch, realVals[5], 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[i] = Wire.read() << 8 | Wire.read();
- }
- }
- //对大量读数进行统计,校准平均偏移量
- void Calibration()
- {
- float valSums[7] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0};
- //先求和
- for (int i = 0; i < nCalibTimes; ++i) {
- int mpuVals[nValCnt];
- ReadAccGyr(mpuVals);
- for (int j = 0; j < nValCnt; ++j) {
- valSums[j] += mpuVals[j];
- }
- }
- //再求平均
- for (int i = 0; i < nValCnt; ++i) {
- calibData[i] = int(valSums[i] / nCalibTimes);
- }
- calibData[2] += 16384; //设芯片Z轴竖直向下,设定静态工作点。
- }
- //算得Roll角。算法见文档。
- float GetRoll(float *pRealVals, float fNorm) {
- float fNormXZ = sqrt(pRealVals[0] * pRealVals[0] + pRealVals[2] * pRealVals[2]);
- float fCos = fNormXZ / fNorm;
- return acos(fCos) * fRad2Deg;
- }
- //算得Pitch角。算法见文档。
- float GetPitch(float *pRealVals, float fNorm) {
- float fNormYZ = sqrt(pRealVals[1] * pRealVals[1] + pRealVals[2] * pRealVals[2]);
- float fCos = fNormYZ / fNorm;
- return acos(fCos) * fRad2Deg;
- }
- //对读数进行纠正,消除偏移,并转换为物理量。公式见文档。
- void Rectify(int *pReadout, float *pRealVals) {
- for (int i = 0; i < 3; ++i) {
- pRealVals[i] = (float)(pReadout[i] - calibData[i]) / 16384.0f;
- }
- pRealVals[3] = pReadout[3] / 340.0f + 36.53;
- for (int i = 4; i < 7; ++i) {
- pRealVals[i] = (float)(pReadout[i] - calibData[i]) / 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[a]=ii[0];
- if(iii==2)
- ss[a]=ii[0]*10+ii[1];
- if(iii==3)
- ss[a]=ii[0]*100+ii[1]*10+ii[2];
- iii=0;
- a=a+1;
- }
- if(i==37)
- {
- // x1=ss[1]; //x轴控制值
- // y1=ss[2]; //x轴控制值
- s=ss[3]; //Speed控制值
-
- //是否可以飞行状态值 设置
- if((p>pp)&&(s>0))
- staus=1;
-
- a=0; //数据接收控制符
- }
- if(i>47)
- {
- iii=iii+1;
- if(iii==1)
- ii[0]=i-48;
- if(iii==2)
- ii[1]=i-48;
- if(iii==3)
- ii[2]=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
|