四轴飞行器稳定性pid代码

Question

我正在使用加速度计和陀螺仪构建四轮直升机。表格看起来像这样：

我的音高由前后电机控制。 我的滚动由左右马达控制。

我使用 PID 和伺服库。

这是我的音高稳定性代码：

double Kp=3.15, Ki=4.3, Kd=0.6;
int throttle = 1000;

PID PitchPID(&PitchInput, &PitchOutput, &PitchSetpoint, Kp, Ki, Kd, DIRECT);

 PitchPID.SetMode(AUTOMATIC);

  backESC.attach(3);
  frontESC.attach(5);

 // arme the motors
  backESC.writeMicroseconds(2000);
  frontESC.writeMicroseconds(2000);
  delay(5000);

  backESC.writeMicroseconds(700);
  frontESC.writeMicroseconds(700);
  delay(2000);

void loop{
 // i have the angle and pitch
  int xAngle = GetX();
  int Pitch = GetPitch();



PitchInput = (Pitch) ;
PitchSetpoint = 0;
PitchPID.SetOutputLimits(-150,150);
PitchPID.Compute();


backPower =  PitchOutput;

frontPower = (PitchOutput*-1);


frontESC.writeMicroseconds(backPower + throttle);
  backESC.writeMicroseconds(frontPower + throttle);
}

正如您所看到的，我还没有真正的算法来稳定四边形。 我会感激一些帮助。我已经尝试了一些样品。它并没有顺利进行。如果不使用我的 PID 库，即使是帮助也不胜感激。