截止到3月1号的四轴飞行器代码

应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] >> 8) & 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
}

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


对 “截止到3月1号的四轴飞行器代码” 的 67 条 评论

  1. richard 说:

    终于开放源代码了…………

  2. SSS_SXS 说:

    动力兄,能采纳我的建议真的很高兴,但你这一堆代码,我还真一时半会很难看懂,准备慢慢消化了,吃透了准备搞个自平衡小车试试。

    虽然,代码不怎么明白,但有一点很明确,动力兄没有调用ARDUINO自带的PID库,所以涉及到的一些PID的问题没有彻底解决,http://arduino.cc/playground/Code/PIDLibrary,pid库文件应该可以下载到,结合上次给你的官网对PID描述,再试一下,加油啊!(我纸上谈兵了:))
    这里顺便提一下,arduino的库文件真是个好东西,我前段时间做的短信控制ARDUINO也是用到了#include ,问题一下迎刃而解了,
    实现了短信控制电源开关,当然在进一步的话,可以控制带红外遥控器的设备,
    比如空调,电视,再进一步的话,接上汽车发动机就是“安吉星”啦,创意无极限!

    • 好的,多谢提醒,我也试试这个PID库看看,不过有个叫ENIAC的小朋友提到商品电调有可能0.2秒的减速延时,这个很致命啊,我先查查这个事情。

      • 小鸡 说:

        是这样的,很多电调都有自带的滤波,会使电调反应迟缓,最好自己做带I2C总线的电调,比如MK电调

  3. SSS_SXS 说:

    这里顺便提一下,arduino的库文件真是个好东西,我前段时间做的短信控制ARDUINO也是用到了#include 库文件,多串口并用问题一下迎刃而解了,实现了短信控制电源开关,当然再进一步的话,可以控制带红外遥控器的设备,比如空调,电视,再进一步的话,接上汽车发动机就是“安吉星”啦(上海这里这广告特多,不知道首都人民知道吗?呵呵)

  4. ENIAC 说:

    好多程序呀,看着就晕,学习还没学到啊。。

  5. wnq 说:

    哈哈哈,被表扬了,嘿嘿。
    您的代码好多啊,啊啊,可能是我水平太差才觉得,无从入手。希望lz照顾下像我这样笨的,新的四轴制作的话,能不能采用循序渐进的方式,如:
    第一阶段:只用陀螺仪做四轴(最开始,用最简单最少的内容,估计我这种菜鸟有希望入门,啊啊)
    第二阶段:陀螺仪+加速度 做四轴(慢慢加入更多的内容)
    第三阶段:陀螺仪+加速度+超声波 做四轴
    第三阶段:陀螺仪+加速度+超声波+gps 做四轴
    。。。。。。

    啊啊,可能太麻烦了,不过真想要这样的教程,我只好厚脸皮的向lz要啦,哈哈

    • 我本来就是计划这样一步步发攻略的,呵呵,不过后来担心不成功的攻略会误导别人。
      这次也是应邀把代码发出来让大家帮忙找找错。最终实验成功的话,我就会把这些代码分解成小块介绍。

      • SSS_SXS 说:

        确实要想把你这一堆代码弄明白不容易,周末买了陀螺仪和加速度计,准备参照你的代码做一个自平衡小车练练手,顺便验证一下PID库是否实用

  6. 老薛 说:

    动力哥,有一个很厉害的朋友把Arduino的库函数简化了很多,做了一个叫做arduino-lite库,http://www.csksoft.net/blog/catalog.asp?cate=4
    生成代码长度要短很多,可能对你的制作有帮助

    • 多谢老薛,我看看 :)

      • 柯南君 说:

        动力兄,看你好久没发帖了,不知道你还在关注这里吗?我也在做四轴,现在问个问题,四轴在飞行过程中,如果如果突然打剁,四个桨提供的动力在垂直方向的分力会立即减小,四轴岂不是会立即下落,你们是怎么处理这个问题的?谢谢!

    • 呵!老薛,看上去他们不仅是基于现有的Arduino库二次开发,而是从编译系统上改进了。
      还没有开始试用,不过从他们这个例子上看,貌似对性能提高很大!

      代码类型    代码             所需的AVR时钟周期
      Arduino     analogWrite(9, pwm_val);   ~80
      Arduino-Lite  ANALOG_WRITE(9, pwm_val);   2

      美中不足的是库有点少了,比如I2C的库,自己写的话代码有点多

      • 老薛 说:

        是啊,这个库大大的简化了原有的库,但是原有的库比较健壮,考虑的情况比较周全,所以产生代码多。但是我想只要按照Arduino-lite里面的说明和约定来连接硬件,应该就可以了

        那位朋友很厉害,对计算机底层的东西和网络都懂得很深入。我也很想学习这些技术,但不知道从何入手。动力哥可有建议?

        • 隔行如隔山啊,老薛你的编程能力已经相当不错了。但是对咱们这些玩家来说,这些东西应该是用不上的。
          你说的计算机底层应该是指编译原理和操作系统原理,关于网络有一门课叫计算机网络原理,另外计算机专业比较推崇的还有算法导论。
          这几门课很多计算机专业的人学完以后也抱怨没有什么意义,不过高手们一般都学的很好。
          我属于野路子出身的程序员,上面这些东西都是自己瞎看的,理解个皮毛,只有算法研究的多一点,主要是面试总考这些 :)
          这些方面我也不算擅长,所以不敢瞎提建议,老薛如果有兴趣不妨先看看相关教材吧

          • SSS_SXS 说:

            两位,在我眼里都算是高手了,虽然我是计算机网络专业背景,但实在是懂个皮毛,没有深入理解过,ARDUINO算是对自己所学知识运用的一个开始,涉及到编程这块完全是摸石头过河,走一步看一步。

            计算机网络原理我上过,都是些原理性的东西,没有实践真是很难理解

          • 老薛 说:

            是啊!高手们总是学的很好。因为他们总会自己找机会去实践书本上教的那些东西。我学过一门叫做微机原理的课,不知从何实践,所以也早忘了。可能现在重新捡起来看看会有深一些的体会

            动力哥职业上的华丽转身还是很值得佩服的,哈哈!

          • 像你这样的物理学专业人士,简直就是人类的未来啊,计算机只需要当工具来用就好啦 :)

          • darkorigin 说:

            还有强力的基础课 离散数学
            以及为了实现强大数据处理算法的 数据结构等等
            编译原理和操作系统原理目前暂时用不上,毕竟AVR上OS还是很辛苦的(不像ARM都可以上LINUX内核了)

  7. obK仔 说:

    动哥…想问问为什么你不用arduino的servo库要自己写PWM呢?是更精准和好操作一下吗?

    最近终于把架子,电源电路那些都弄好了..
    但发现把马达调到转速很高都没法起飞..只会原地旋转..
    你觉得是怎么一回事呢?是不是桨的问题呢?

    • 对,我是觉得自己的PWM会更精确和容易控制。
      不过这只是我的个人看法,你知道程序员有时候会固执的自己写代码:)

      你的高电平脉宽调到多少了呢?我估计差不多要到1.5ms才能飞起来。
      最好注意一点,带上护目镜,不要在家里飞,或者栓上绳子,因为一飞起来可能就飞老高

      • obK仔 说:

        哈哈..都是执着的程序员啊..
        我也是像你一样..大部分代码都自己重写那样..其实都有现成的轮子可以用了..

        不过我看了看servo库的代码..它是设置AVR的定时器做PWM方波的输出..直接配置寄存器就能很好的生成PWM..这样来得更精准..不会占用CPU的时间..不受中断程序的影响..
        同时也不用自己考虑太多..动哥也可以参考一下这样..

        对了..你之前的四轴支架..轴距大概是多少呢??我的是60cm..不知道是不是太重太沉了..我现在脉宽到1.3ms左右吧..感觉马达已经转得非常快了..转动的声音尤其吓人..都有点怕了..呵呵..
        好的..我下次去外面再开大一点保护好试试看..谢谢你的建议啦~

        • 好的,我回头也看下源代码,印象中系统默认的PWM是50Hz,而且PWM的值只有0~255的范围。不过可能真的实现方式不一样,先去看看再说。
          轴距我也差不多是60cm,称重量接近2kg。脉宽1.3ms太小啦,0.9~2.1是从0到满速,1.3刚开个头呢。
          不过声音是够吓人的,一定要做好保护啊,切记!

        • 刚才看了下servo库,里面主要控制时间的方法是:myservo.writeMicroseconds(1500);
          从这点看来,servo库默认的PWM频率还是50Hz,而且貌似没有修改的接口。
          如果这样的话,只能先用自己的代码来处理了

          • wnq 说:

            记得,以前试过,用自带的servo不能驱动所有的舵机,可能就是这个原因。

  8. obK仔 说:

    动哥..我用你的传感器代码来测试了一下ADXL+D3G的模块
    发现只有一开始的读值正确..只要一移动传感器..
    串口传回来的值就变成全1和全255了..
    很奇怪你有遇过这样的问题吗?

  9. Randy 说:

    问一下动哥。我们可以自己做一个用您的代码?

  10. 小鸡 说:

    我下载了MWC的源码,里面全是宏定义,怎么看啊?

    • darkorigin 说:

      有些文件你不需要动,你可以看作是驱动程序, 只要动定义的那个文件啊
      好像是CONFIG.H这个头文件 比如传感器,不能同时开太多,比如 三轴陀螺你不能开好几个三轴陀螺仪,按照你实际用的传感器来,很多常见配置都已经被罗列 比如:
      //#define INNOVWORKS_10DOF // with ITG3200, BMA180, HMC5883, BMP085
      也就是说INNOVWORKS_10DOF 这个10轴传感器就包含了ITG3200, BMA180, HMC5883, BMP085 4个 ,很多很多常见组合都有了,实在没有就手动的开下面的单个的传感器

      我的10轴在5月份的版本已经测试通过了 当时的标注 翻译过来就是 中国山寨10轴模块 (三轴陀螺+3轴加速度+磁定向+气压)

      • darkorgin同学太强大了 :)

      • 小鸡 说:

        不带这样吧。。这样做四轴还有什么意思呢?可能什么都没有学会啊?我是想全面理解四轴的控制机理。做出来只是为了验证我完全懂了。我说的做四轴实质上是做飞控。。

  11. taorui 说:

    你好,
    我想问下你,关于,电调初始化的程序。
    我用的是 hoobywing skywalker 20A*4 的集成电调

    在开始飞之前,要初始化电调range。(说是不同的发射机,有不同的range)

    但是在程序中,输出是PWM波形,拨到最大值,保持两秒。(那说明保持PWM duty cycle 最大,但是电调怎么知道这是最大的啊?)

    我看电调的manual,里面没有说道电调是怎么工作的,要怎么能查到,电调的
    电路信息,还有工作原理啊?

    谢谢了。
    taorui

    • darkorigin 说:

      个人理解电调其实主要作用在于放大电流和电压(很明显,单片机的管脚肯定无法直接提供驱动电机的足够电流和电压)
      SO
      个人认为,可以忽略它的设计(编程或者电路设计里面叫做 黑盒子 就是说忽略它的内在,只操作的接口和使用的方式)

      电调的说明书主要是针对飞行板而非单片机,自然是不会公开这些技术参数。

      真的需要研究,可以参考MWC 一个开源的飞行控制系统(包含自稳,定向等功能,当然也包含相应周边电路的初始化等工作)

    • 我猜想电调内置了一些参数,其中2ms左右就可以认为是大值

      • taorui 说:

        谢谢你的回答。
        我现在不太明白的是,单片机输出的是PWM信号(490HZ),但是我看网上的资料,信号输出应该是50HZ,在这里有点疑惑。
        我看动力哥的电调属性那里(PPW)20ms的模型-(0.5-2ms)的dutycycle来控制转速。我要做的就是让单片机输出50HZ的PPM信号就OK了?

        周一去学校机房用PWM信号试下能驱动电机不。。

        还有好长距离要做啊。

        • taorui 说:

          在补充一下:

          如果动力哥能把程序的运行步骤(模块)分开写上去就好了。

          这样直接看所有代码。从起飞,检测,离地,PID,油门反应。。混在一起。。

          初学者 还是蛮难看懂的

          taorui

          • 我也想这么写啊,不过我自己还没调好了,先不误人子弟了 :)
            这几个月很忙,很久没更新了,对不起大家

        • 对50Hz就ok了,我用的是arduino单片机,跟淳朴的单片机还不一样,它是已经封装了一些功能,所以输出的PWM已经是50Hz了

  12. taorui 说:

    另外能问个很简单的问题(MWC):
    #if defined(BI) || defined(TRI) || defined(SERVO_TILT) || defined(GIMBAL) || defined(FLYING_WING) || defined(CAMTRIG)
    #define SERVO
    #endif

    #if defined(GIMBAL) || defined(FLYING_WING)
    #define NUMBER_MOTOR 0
    #elif defined(BI)
    #define NUMBER_MOTOR 2
    #elif defined(TRI)
    #define NUMBER_MOTOR 3
    #elif defined(QUADP) || defined(QUADX) || defined(Y4)
    #define NUMBER_MOTOR 4
    #elif defined(Y6) || defined(HEX6) || defined(HEX6X)
    #define NUMBER_MOTOR 6
    #elif defined(OCTOX8) || defined(OCTOFLATP) || defined(OCTOFLATX)
    #define NUMBER_MOTOR 8
    #endif
    这是MWC源码中OUTPUT 控制电机输出代码,但是看不懂之前的define是用来判断什么的啊?
    这些语法是这样么?动力哥能帮忙看下么。

  13. roothoo 说:

    动哥,请教个问题
    accAngleX = atan2(accXYZ[0] – accOffset[0], accXYZ[2])
    X指的是pitch吗? 这个公式有理论依据吗?
    我发现MWC 解算Pitch roll 的公式跟上面的差不多。
    但是根据资料《freescale-tilt sensing using linear accelerometer AN3461.pdf》以及国外的一些做平衡车的资料,
    pitch 角度 = atan2( accXYZ[0], accXYZ[2])。
    迷惑中。。。。

  14. roothoo 说:

    上面第二个公式打错了
    应该是
    pitch 角度 =
    atan2( accXYZ[0], sqrt(accXYZ[2]^2 + accXYZ[1]^2))。

    • 这个我没有看过你说的资料,但是你写的这个公式很奇怪啊。我想算的是倾角,根据直角三角形的正切定义,tan(alpha) = Z / X (正切等于对边比邻边)
      之所以用atan2,是因为atan的范围是180度,而atan2的范围是0到360度。
      而这个公式里的是对边/斜边,看上去像是asin啊

      • roothoo 说:

        感谢动哥的回复
        我还是有疑惑
        当物体只做pitch运动时,tan(alpha) = Z / X 来求pitch角度,这很好理解;但是当物体同时做pitch和roll运动时,能保证tan(pithc) = Z / X,tan(roll) = Z / X吗?

        • 第一个参数是除以Y吧?确实不能保证,这样计算从几何学来说是不行的。
          但是我们本来就是估算吧,而且角度比较小的时候,可以认为近似相等。
          我也有点纠结这个问题,所以一直觉得没有飞起来,不敢乱写攻略,怕误导别人了

  15. liiochen 说:

    最近在自己打算用一种不同的方式来实现X4. 没有APM,没有MWC,Arduino仅用来控制电机和检测电压~

    • 赞,欢迎分享经验 :)

      • liiochen 说:

        目前用了一块i9300的主板.板载了气压计等传感器.去掉了android-jni以上的东西,保留了linux内核.自己驱动3G以及各传感器.
        用usb和arduino通信.
        arduino则用了sero库来驱动,频率上到了600hz以上了.esc用的rctimer 40a
        目前还在做i9300层面的开发,已经完成了3G信号中断重连.VPN断线重连,传感器数据以及控制协议开发.
        关于控制的话用了一台公网服务器,通过VPN转发了主板上的端口.达到可直接tcp/ip通信的目的.
        目前最大的问题有:
        1. 通信延迟, 服务器是电信的,主板用了联通3G.ping延迟基本上到了200ms左右
        2. GPS精度问题. 目前只能达到6m左右. 打算通过天线增益,看能不能有所改善,本来用了陶瓷无源天线,25x25mm的那种.结果发现还没有使用 800mhz-2500mhz全频的3g天线增益大. 自制了陶瓷有源天线,在馈电电路上貌似一直不成功. 不知道老男孩在这方面能否提供一些帮助?
        3. 传感器及pid算法,这个最蛋腾. 唯一的打算是剥离mwc的代码.转换为c代码嵌入

        关于地面站的现实打算使用baidu maps api.地面站支持手机控制
        我想做的,其实并不是实时控制,而是无人机方向.

        关于gps的误差,我打算使用”相对GPS定位”来解决, 就是当x4和控制设备在起飞前各记录一次各自的GPS位置,之后所有路径以及位置信息不依赖GPS坐标。而是通过加速度和磁力计以及陀螺仪来完成路径和位置计算。

        总之一切都是从0开始。主板上的2颗摄像头,后置1080p用于航拍。前面那个打算用来进行障碍计算。。 路太漫长。

        如果对这种方向有兴趣的朋友 也可以与我一起交流,期待。。。

        • buaaer 说:

          感觉不依赖GPS坐标。而是通过加速度和磁力计以及陀螺仪来完成路径和位置计算,难度相当的大,而且传感器的误差积分,。。。。。。。

        • 太赞了,你这可是一个大工程!
          1. 关于通信延时的问题,现在的机房一般都支持联通电信双线吧,看能不能给调一下。如果实在不行,也可以看有没有朋友在双线机房给配个socket转发代理。我的主机是放在美国的,刚才ping了下,也超过200ms了
          2. GPS精度其实还没有这么高,十几米到50米的误差都是正常的。但这个事情不用太纠结,它的误差基本是偏移量,而不是随机误差。市面上的导航设备,也是通过相对位置差分来修正的。但是不建议用加速度和陀螺仪来计算路径,不是准不准的问题,而是根本无法计算。陀螺仪只能测角速度,加速度传感器在不知道准确姿态和当地准确重力加速度的情况下,没法计算路径的。
          GPS虽然有误差,但是在相同的气候条件下,误差基本比较稳定。所以你在起飞的时候记录GPS位置,只要对应这个位置回来,误差应该不会有6米这么大。
          其他的事情过于高端,已经超出我的能力范围啦。我的博客里其实有好多玩四轴的朋友,看来评论这种方式很难交流啊。之前搭过一个论坛,discuz屏蔽垃圾功能做的不行,虽然设置了验证码和验证问题,但是很快就被海量广告淹没了。最近计划自己做一个交流用的小站,到时候欢迎提意见 :)

          • liiochen 说:

            又来汇报进度了..
            目前已经完成了陀螺仪\加速度\磁力计\重力加速度\气压计的校准
            周末在家终于把主板固定到了中心板. 加了根GPS天线.

            我刚好也打算搞个论坛,要不一起?

发表评论

可以使用下列 XHTML 标签:<a href="" title=""> <abbr title=""> <acronym title=""> <b> <blockquote cite=""> <cite> <code> <del datetime=""> <em> <i> <q cite=""> <strike> <strong>