做做AI，造造人 - 动力老男孩的博客

应SSS_SXS建议，把目前的代码贴出来供大家分析。其中注释有点乱，有英文有中文，凑合看吧。

【特别感谢】有一位非常热心的朋友wnq送了我几套木质机架，还有一套CACM的飞控+GPS模块。所以最近正在计划重新用成品飞控搭一套四轴找找感觉。原来的四轴暂时不打算拆，正在淘宝各种散件和遥控器。另外电机轴也摔歪了，所以准备按wnq的攻略把几根轴都换一换。

下面这段代码是3月1号最后一次修改，很遗憾最近这段时间一直没啥更新。
大致思路是先计算出飞行姿态角AngleX和AngleY，然后根据角度和角速度，计算出blanceX和blanceY。
这两个参数的含义是，blanceX表示x轴两个电机的参数分配比例，例如balanceX = 1，表示x1和x2两个电机转速相同，
balanceX = 0.5，表示x1的转速是0.5，x2转速是1.5。
Y轴的概念是类似的，之所以这样处理，是期望x轴两个电机的转速和大致和y轴两个电机相同，这样飞行器不至于打转。
另外，还有个balanceXY，用于在电机转速不均匀的时候，微调x轴电机和y轴电机的转速比，这个本来是打算在电子罗盘的应用中加上的，现在暂时保持等于1。

#include 
#include 

/**
 * 记录：目前较为稳定的实验参数为33, 66，挂绳貌似能垂直起降，但是真正试飞时依然侧飞
 * 试飞时，在3秒内飞到10米高，测距周期是100ms，对应的距离是33cm
 * 起飞时：
 * 1. 遥控在0~directCtrlRange时，按n*5计算转速，方便调试
 * 2. 遥控>directCtrlRange时，按n-directCtrlRange+10计算高度，油门按一定速度缓慢上升，直到离地指定的高度
 * 3. 超声波测距量程是2~180cm，不知道为什么超出量程时会读出7cm的数值，必须注意纠错
 */
boolean connectToPc = false;
long stableTime = 1500000;
int directCtrlRange = 40;
double fastStep = 1.8;
double maxThrottle = 650;

/* Gyro sensor */
int GYRO_ADDR = 0x68; // 7-bit address: 1101000, change to 0x69 if SDO is High
byte GYRO_READ_START = 0xA8; //Start register address, total 6 bytes: (XL,XH,YL,YH,ZL,ZH)
int gyroXYZ[3];
int gyroOffset[3];
int gyroCS_Pin = 4;
int gyroSDO_Pin = 5;

/*  Accessory sensor  */
int ACC_ADDR = 0x53; // 7-bit address, change to 0x53 if SDO is High
byte ACC_READ_START = 0x32; //Start register address, total 6 bytes: (XL,XH,YL,YH,ZL,ZH)
int accXYZ[3];
int accOffset[3];

/* Compass sensor */
int COMP_ADDR = 0x3C >> 1;
byte COMP_READ_START = 0x03;
int compXYZ[3];   //Notice that it is XZY

/* Motor settings */
int motorPins[4];
int minPWM = 900; //电机控制的PWM最低幅值为0.9ms，最高幅值为2.1ms
int maxPWM_Delay = 1100; //电机控制的PWM范围，这里记录它们的差并留一点裕度
int ctrlPower = 0; //从遥控器发来的控制指令
double throttle = 0;  //油门范围，从0到1200，对应从minPWM到2100
int motorPowers[4];
int motorCtrl[8]; //电机控制的顺序和时间，值表示从0.9ms开始，(delayMs-PinIdx)*4

/* Ultrasonic sensor */
int trigPin = 3;
int echoPin = 2;  //对应的中断号是0
volatile double ultraT1 = 0;
volatile double ultraT2 = 0;
double distance = 0; //测量的离地距离，单位是厘米
double preDistance = 0;
double stablePower = 0;  //正好可以离地而起的马力（假设保持这个马力，可以匀速上升或下降）
int maxDistance  = 250; //离地可测量的最大高度
int targetDistance = 10; //目标离地高度
int ultraCount = 0;

/* For testing */
int reportType = 0;
int reportArr[3];
int reportT[3];

/* Read from control */
int buffer[6];   //0,1: 0xFF, 2: Power, 3: Log Type, 4: P_Value, 5: I_Value
int bufferIndex = 0;

/* PID console */
double P_Control, I_Control;
double balanceGyro = 0.99; //用于融合陀螺仪和加速度计的角度
double balanceAcc = 0.9; // 加速度传感器读数平均，用于消除抖动
double balanceSpeed = 0.05; // 用于陀螺仪读数平均（陀螺仪比较精确）
double balanceXY = 1, balanceX = 1, balanceY = 1;
double angleX, angleY, angleZ, speedX, speedY, accAngleX, accAngleY;
double testAngleX;
double compAngle;
boolean hasReadCompAngle = false;

/* Main control start */
void setup()
{
  motorPins[0] = 38;   //x1
  motorPins[1] = 43;   //x2
  motorPins[2] = 42;   //y1
  motorPins[3] = 39;   //y2
  for(int i = 0; i < 4; i++) pinMode(motorPins[i], OUTPUT);
  Serial.begin(9600);
  Wire.begin();
  pinMode(gyroCS_Pin, OUTPUT);
  pinMode(gyroSDO_Pin, OUTPUT);
  digitalWrite(gyroCS_Pin, 1); //enable I2C for GYRO
  digitalWrite(gyroSDO_Pin, 0); //set I2C address last bit
  setupGyroSensor(250);
  setupAccSensor();
  setupCompSensor();
  pinMode(trigPin, OUTPUT);
  attachInterrupt(0, blink, CHANGE);
  digitalWrite(trigPin, 0);
  // 从EEPROM中读取零点的校准值
  for(int i = 0; i < 3; i++) {
    gyroOffset[i] = EEPROM.read(2 * i + 1) << 8;
    gyroOffset[i] |= EEPROM.read(2 * i);

    accOffset[i] = EEPROM.read(7 + 2 * i) << 8;
    accOffset[i] |= EEPROM.read(6 + 2 * i);
  }
}

// 用于处理超声波传感器产生的中断，尽量减少在中断函数中处理逻辑
void blink()
{
  if(ultraT1 == 0) ultraT1 = micros();
  else ultraT2 = micros();
}

int loopIndex = 0;
void loop()
{
  if(reportType == 103) {
    /* 校准模式，在静止的平地上测量N次读数，记录零点误差
       校准隔一段时间做一次即可，例如季节变化，地点变化等 */
    gyroOffset[0] = gyroOffset[1] = gyroOffset[2] = 0;
    accOffset[0] = accOffset[1] = accOffset[2] = 0;
    int repeatTime = 100;
    for(int i = 0; i < repeatTime; i++) {
      // 单次读数大约6ms，校准读数总耗时在0.3秒左右
      readGyroSensor();
      readAccSensor();
      for(int j = 0; j < 3; j++) {
        gyroOffset[j] += gyroXYZ[j];
        accOffset[j] += accXYZ[j];
      }
      delay(10);
    }
    // 读数完毕写入EEPROM，每个字节需要3.3ms写入
    for(int i = 0; i < 3; i++) {
      gyroOffset[i] /= repeatTime;
      EEPROM.write(2 * i, (byte) (gyroOffset[i] & 0xFF));
      EEPROM.write(2 * i + 1, (byte) ((gyroOffset[i] >>  & 0xFF));
      accOffset[i] /= repeatTime;
      EEPROM.write(6 + 2 * i, (byte) (accOffset[i] & 0xFF));
      EEPROM.write(7 + 2 * i, (byte) (accOffset[i] >> 8 & 0xFF));
    }
    // 校准完成，恢复成普通模式
    reportType = 0;
  }
  /*
   * 采样率为350Hz，所以每个周期2.857ms。每个周期中都需要读一次陀螺仪和加速度计，
   * 用时大约0.7ms；然后剩余的时间分别用来做不同的事情，例如控制电机，计算角度等。
   * 然后进行角度姿态的融合，大约需要0.7ms。这部分放在控制电机的后面，因为0.9ms开始
   * 就需要控制电机，时间不够用，放在后面则刚好够用。
   * 电机的控制周期是20ms，也就是每7次循环修改一次电机功率
   */
  long t0 = micros(); //记录进入loop的时间
  if(loopIndex == 1) {
    for(int i = 0; i < 4; i++) digitalWrite(motorPins[i], HIGH);
  } else {
    // digitalWrite需要20微秒，为了读数时间均匀，其他情况统一做相应延时
    delayMicroseconds(20);
  }
  // 读取陀螺仪和加速度计的读数，前1.5秒等待传感器稳定
  if(t0 > stableTime) {
    readGyroSensor();
    readAccSensor();
  }
  if(connectToPc && loopIndex == ctrlPower % 7) reportT[0] = (int) (micros() - t0);
  double newSpeedX, newSpeedY, newAngleX, newAngleY, power, powerX, powerY, adjust;
  int loopCountLimit;
  switch(loopIndex) {
    case 0:
      // 这时候角度数值已就绪：angleX, angleY, angleZ, speedX, speedY
      // 但是需要修正45度安装的换算
      newSpeedX = (speedX + speedY) * 0.707;
      newSpeedY = (speedY - speedX) * 0.707;
      // 假设四轴翻转不会超过90度
      newAngleX = asin((sin(angleX) + sin(angleY)) * 0.707);
      newAngleY = asin((sin(angleY) - sin(angleX)) * 0.707);
      // 计算平衡参数
      adjust = 1;
      if(throttle > 350) adjust = 500.0 / throttle;
      balanceX = 1 - (newSpeedX * P_Control + newAngleX * I_Control) * adjust;
      balanceX = min(max(balanceX, 0), 2);
      balanceY = 1 - (newSpeedY * P_Control + newAngleY * I_Control) * adjust;
      balanceY = min(max(balanceY, 0), 2);
      // 计算油门值
      if(ctrlPower <= directCtrlRange) {
        if(throttle > ctrlPower * 5) {
          // 缓冲下降20ms下降0.8，1秒钟下降40
          throttle -= fastStep;
        } else {
          // 上升之前可以不考虑缓冲问题
          throttle = ctrlPower * 5;
        }
      } else {
        // 目标离地高度为ctrlPower - directCtrlRange + 10;
        targetDistance = ctrlPower - directCtrlRange + 10;
        if(stablePower == 0 && distance > preDistance + 2) {
          // 100ms内爬升了2cm以上，设置2主要是为了避免读数跳动误差
          stablePower = throttle;
        }
        if(stablePower == 0) {
          // 处于起飞状态
          throttle += fastStep;
        } else {
          // 可以认为已经离地
          double newThrottle = stablePower + (targetDistance - distance) * 0.11- (distance - preDistance) * 0.13;
          if(newThrottle > throttle + fastStep) throttle += fastStep;
          else if(newThrottle < throttle - fastStep) throttle -= fastStep;
          else throttle = newThrottle;
        }
        // 油门保护，不超过某个最大值（避免飞太快超出控制范围）
        if(throttle > maxThrottle) throttle = maxThrottle;
        // directCtrlRange优先控制权
        if(throttle < directCtrlRange * 5) throttle = directCtrlRange * 5;
      }
      // 计算电机功率
      power = throttle;
      powerX = power * balanceXY;
      powerY = power * 2 - powerX;
      motorPowers[0] = (int) (powerX * balanceX);
      motorPowers[1] = (int) (powerX * 2) - motorPowers[0];
      motorPowers[2] = (int) (powerY * balanceY);
      motorPowers[3] = (int) (powerY * 2) - motorPowers[2];
      motorCtrl[0] = 9999;
      for(int i = 0; i < 4; i++) {
        motorPowers[i] += 5 * i;
        if(motorPowers[i] > maxPWM_Delay) motorPowers[i] = maxPWM_Delay;
        if(motorPowers[i] < motorCtrl[0]) {
          motorCtrl[0] = motorPowers[i];
          motorCtrl[1] = i;
        }
      }
      // 排序, 设置motorCtrl
      for(int i = 1; i < 4; i++) {
        int pre = motorCtrl[i * 2 - 2];
        int preIdx = motorCtrl[i * 2 - 1];
        motorCtrl[i * 2] = 9999;
        for(int j = 0; j < 4; j++) {
          if(motorPowers[j] > pre || (motorPowers[j] == pre && j > preIdx)) {
            if(motorPowers[j] < motorCtrl[i * 2]) {
              motorCtrl[i * 2] = motorPowers[j];
              motorCtrl[i * 2 + 1] = j;
            }
          }
        }
      }
      // 计算delayMs
      for(int i = 0; i < 3; i++) {
        motorCtrl[6 - i * 2] -= (motorCtrl[4 - i * 2] + 5);
      }
      break;
    case 1:
      // 控制电机转速，首先运行到0.9ms
      delayMicroseconds(minPWM + t0 - micros());
      for(int i = 0; i < 4; i++) {
        // 根据间隔时间设置低电平
        if(motorCtrl[i * 2] > 0) delayMicroseconds(motorCtrl[i * 2]);
        digitalWrite(motorPins[motorCtrl[i * 2 + 1]], LOW);
      }
      break;
    case 2:
      // 电子指南针读数，这个不需要太精确，所以20ms读一次
      if(t0 > stableTime) {
        readCompSensor();
      }
      // 从串口获取遥控信息
      loopCountLimit = 18;
      while(Serial.available() && loopCountLimit--) {
        pushData(Serial.read());
      }
      break;
    case 3:
      // 发送各种报告
      if(connectToPc) {
        if(reportType == 1) writeBuffer(gyroOffset, 3);
        if(reportType == 2) writeBuffer(accOffset, 3);
        if(reportType == 3) writeBuffer(compXYZ, 3);
        //if(reportType == 1) writeNums(motorCtrl[0],motorCtrl[1],motorCtrl[2]);
        //if(reportType == 2) writeNums(motorCtrl[3],motorCtrl[4],motorCtrl[5]);
        //if(reportType == 3) writeNums(motorCtrl[6],motorPowers[2],motorPowers[3]);
        if(reportType == 4) writeBuffer(reportArr, 3);
        if(reportType == 5) writeNums((int)distance, (int)(speedX * 1000), motorPowers[0]);
        //if(reportType == 5) writeNums(reportArr[1], motorPowers[2], motorPowers[3]);
        if(reportType == 6) writeBuffer(reportT, 3);
      }
      break;
    case 4:
      // 测量离地距离，音速340m/s = 34cm/ms = 0.034cm/μs，测量的是来回距离，所以需要除以2
      if(ultraT1 > 0 && ultraT2 > ultraT1) {
        double newDistance = (ultraT2 - ultraT1) * 0.017;
        if(newDistance < 250) {
          preDistance = distance;
          if(distance > 100 && newDistance < 10) {
            // 高度突然变小的情况（从100以上变到小于10，可以认为是超出量程）
            distance = maxDistance;
          } else {
            distance = newDistance;
          }
        }
      }
      // 考虑到回音等问题，把测量的时间间隔增加到100ms，相当于17米的反射
      ultraCount++;
      if(ultraCount >= 5) {
        ultraT1 = 0;
        ultraT2 = 0;
        // 因为每次执行到这个循环是20ms，所以计数器增长到5时进行一次测量
        ultraCount = 0;
        // 用10μs的高电平发出触发信号
        digitalWrite(trigPin, 1);
        delayMicroseconds(10);
        digitalWrite(trigPin, 0);
      }
      break;
    default:
      // 其他扩展功能
      break;
  }
  if(connectToPc && loopIndex == ctrlPower % 7) reportT[1] = (int) (micros() - t0);
  // 计算角度angleX, angleY, speedX, speedY, accAngleX, accAngleY
  if(t0 > stableTime) {
    // 量程为250时，测量值单位是8.75 mdps/digi;
    // 正确：180/3.1415926*1000/8.75 = 6548.09
    // 错误：3754.94 = (65536/1000)*(180/3.1415926)
    double readSpeedX = (gyroXYZ[0] - gyroOffset[0]) / 6548.09; //单位：弧度/秒
    speedX = balanceSpeed * speedX + (1 - balanceSpeed) * readSpeedX;
    accAngleX = accAngleX * balanceAcc + atan2(accXYZ[0] - accOffset[0], accXYZ[2]) * (1 - balanceAcc);
    angleX = (angleX + readSpeedX * 0.002857) * balanceGyro + accAngleX * (1 - balanceGyro);
    testAngleX = testAngleX + readSpeedX * 0.002857;
    reportArr[0] = (int) (angleX * 1000);
    reportArr[1] = gyroXYZ[0] ;
    reportArr[2] = (int) (testAngleX * 1000);
    double readSpeedY = (gyroXYZ[1] - gyroOffset[1]) / 6548.09; //单位：弧度/秒
    speedY =  balanceSpeed * speedY + (1 - balanceSpeed) * readSpeedY;
    accAngleY = accAngleY * balanceAcc + atan2(accXYZ[1] - accOffset[1], accXYZ[2]) * (1 - balanceAcc);
    angleY = (angleY + readSpeedY * 0.002857) * balanceGyro + accAngleY * (1 - balanceGyro);
  }
  if(loopIndex == ctrlPower % 7) reportT[2] = (int) (micros() - t0);
  // 更新loopIndex
  loopIndex++;
  if(loopIndex >= 7) loopIndex = 0;
  // 截止到2.857ms
  t0 = micros() % 2857;
  delayMicroseconds(2817 - t0);
}

/* get data from bluetooth devices */
void pushData(byte data)
{
  //验证是否数据的开始
  if (bufferIndex < 1 && data != (byte)0xFF) {
    bufferIndex = 0;
  } else {
    buffer[bufferIndex] = data;
    if (bufferIndex == 5) {
      setParams();
    }
    bufferIndex = (bufferIndex + 1) % 6;
  }
}

void setParams()
{
  // 因为超声波中断可能导致数据丢失，所以需要额外的一个字节用于校验
  if(buffer[1] ^ buffer[2] ^ buffer[3] ^ buffer[4] == buffer[5]) {
    ctrlPower = buffer[1];
    reportType = buffer[2];
    //P_Control = (20.0 + buffer[3] / 4.0) / 200.0;
    //I_Control = (20.0 + buffer[4] / 4.0) / 200.0;
    P_Control = buffer[3] / 100.0;
    I_Control = buffer[4] / 100.0;
    if(reportType == 100 && !connectToPc) {
      Serial.end();
      connectToPc = true;
      Serial.begin(115200);
    }
    if(reportType == 101) {
      // 设置为上拉绳调试模式
      directCtrlRange = maxThrottle / 5;
    }
    if(reportType == 102) {
      // 设置为下拉绳调试模式
      directCtrlRange = 40;
    }
    //if(reportType == 103) {} //校准模式
  }
}

/*****************************************************************************/

void writeBuffer(int arr[], int len) {
  byte buff[len * 2 + 2];
  buff[0] = 0xFF;
  buff[1] = 0xFF;
  for(int i = 0; i < len; i++) {
    buff[i * 2 + 2] = (byte) (arr[i] >> 8 & 0xFF);
    buff[i * 2 + 3] = (byte) (arr[i] & 0xFF);
  }
  Serial.write(buff, len * 2 + 2);
}

void writeNums(int n1, int n2, int n3) {
   byte buff[8];
   buff[0] = 0xFF;
   buff[1] = 0xFF;
   buff[2] = (byte) (n1 >> 8 & 0xFF);
   buff[3] = (byte) (n1 & 0xFF);
   buff[4] = (byte) (n2 >> 8 & 0xFF);
   buff[5] = (byte) (n2 & 0xFF);
   buff[6] = (byte) (n3 >> 8 & 0xFF);
   buff[7] = (byte) (n3 & 0xFF);
   Serial.write(buff, 8);
}

/*****************************************************************************/

void writeRegister(byte deviceAddress, byte address, byte val) {
    Wire.beginTransmission(deviceAddress); // start transmission to device
    Wire.send(address);       // send register address
    Wire.send(val);         // send value to write
    Wire.endTransmission();     // end transmission
}

byte readRegister(int deviceAddress, byte address){
    Wire.beginTransmission(deviceAddress);
    Wire.send(address);
    Wire.endTransmission();
    Wire.requestFrom(deviceAddress, 1);
    while(!Wire.available()) {}
    return Wire.receive();
}

/*****************************************************************************/

int setupGyroSensor(int scale){
  byte GYRO_CTRL_REG1 = 0x20;
  byte GYRO_CTRL_REG2 = 0x21;
  byte GYRO_CTRL_REG3 = 0x22;
  byte GYRO_CTRL_REG4 = 0x23;
  byte GYRO_CTRL_REG5 = 0x24;
  writeRegister(GYRO_ADDR, GYRO_CTRL_REG1, 0x0f);
  writeRegister(GYRO_ADDR, GYRO_CTRL_REG2, 0x00);
  writeRegister(GYRO_ADDR, GYRO_CTRL_REG3, 0x08);
  if(scale == 250){
    writeRegister(GYRO_ADDR, GYRO_CTRL_REG4, 0x00);
  }else if(scale == 500){
    writeRegister(GYRO_ADDR, GYRO_CTRL_REG4, 0x10);
  }else{
    writeRegister(GYRO_ADDR, GYRO_CTRL_REG4, 0x30);
  }
  writeRegister(GYRO_ADDR, GYRO_CTRL_REG5, 0x00);
}

void readGyroSensor(){
    Wire.beginTransmission(GYRO_ADDR);
    Wire.send(GYRO_READ_START);
    Wire.endTransmission();
    Wire.requestFrom(GYRO_ADDR, 6);
    for(int i = 0; i < 3; i++) {
      while(!Wire.available()) {}
      gyroXYZ[i] = Wire.receive();  //Low byte
      while(!Wire.available()) {}
      gyroXYZ[i] |= (Wire.receive() << 8); //High byte
    }
}

/*****************************************************************************/
void readAccSensor(){
    Wire.beginTransmission(ACC_ADDR);
    Wire.send(ACC_READ_START);
    Wire.endTransmission();
    Wire.requestFrom(ACC_ADDR, 6);
    for(int i = 0; i < 3; i++) {
      while(!Wire.available()) {}
      accXYZ[i] = Wire.receive();  //Low byte
      while(!Wire.available()) {}
      accXYZ[i] |= (Wire.receive() << 8); //High byte
    }
}

int setupAccSensor(){
  writeRegister(ACC_ADDR, 0x2D, 0x28);  //Power control
}

/*****************************************************************************/

void readCompSensor(){
    Wire.beginTransmission(COMP_ADDR);
    Wire.send(COMP_READ_START);
    Wire.endTransmission();
    Wire.requestFrom(COMP_ADDR, 6);
    for(int i = 0; i < 3; i++) {
      while(!Wire.available()) {}
      compXYZ[i] = Wire.receive();  //High byte
      while(!Wire.available()) {}
      compXYZ[i] = Wire.receive() | (compXYZ[i] << 8); //Low byte
    }
    // Switch xzy to xyz
    int temp = compXYZ[1];
    compXYZ[1] = compXYZ[2];
    compXYZ[2] = temp;
}

int setupCompSensor() {
  writeRegister(COMP_ADDR, 0x00, 0x70); //Mod setting
  writeRegister(COMP_ADDR, 0x02, 0x00); //Mod setting
}

/*****************************************************************************/