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