Arduino quadcopter servo not responding to pid control

[DanThe400Man]

Hi all, first time posting so just let me know if I could format my question better. I'm using an arduino Nano.

I've made a few other arduino projects before but this is the most complex. I'm making an arduino based on a quadcopter, writing my own PID controller from scratch to satisfy the engineer in me. I've got the hardware of the quadcopter finished and it looks to be working fine. When throttling up and down all 4 motors spin up and down at the same rate; as expected, the angle I'm calculating using the MPU6050 also looks to be correct when I tilt the quadcopter.

However when I then apply the PID output to the throttle (only 1 axis and only P control at this point, attached to a gimbal I made) the motors all stay spinning at the same speed. If I write different values to each motors I can tell that they are spinning at different speeds.

If I tilt the quadcopter to a 25degree angle I can see that the difference of the throttle being applied to the front and rear and rotors should be over 100microseconds. However, it just doesn't seem to being applied to the motors they still spin up and down with the throttle but it's as if the PID adjustment just hasn't been applied (I know it definitely has as i've printed out the variable that I apply to writeMicroseconds). Totally stumped and spent weeks going through every part of my code trying to find the problem.

Other quirks that may or may not be related:

1. If I don't make the pid adjustment (pid_p in the code below) an int before adding it to the throttle the motors don't spin at all (this may be obvious but had me stumped for a while).

Any ideas would be an absolute lifesaver.

const int Ch1 = 2;

#include "Wire.h"
#include "PinChangeInterrupt.h"
#include "Servo.h"

const int setpoint = 0;
float error, Kp;
int FrontSignal, RearSignal;
float pid_p;

const int MPU_addr = 0x68;
float AcX, AcY, AcZ, GyX, GyY, GyZ;
float AccRawX, AccRawY, AccRawZ;
float GyRawX, GyRawY, GyRawZ;
float rad2deg = 180 / 3.14;
float ElapsedTime, CurrentTime, PrevTime;
float FiltAngleX, FiltAngleY;

Servo ESC6;
Servo ESC9;
Servo ESC10;
Servo ESC11;

int ESCPin6 = 6;
int ESCPin9 = 9;
int ESCPin10 = 10;
int ESCPin11 = 11;

volatile float Ch1StartTime = 0;
volatile boolean Ch1NewSignal = false;
volatile float Ch1PulseDuration;
int Ch1PulseMapped;
int Ch1SigMain;
int pidF;

void setup() {
  attachInterrupt(digitalPinToInterrupt(Ch1), pin_ISR1, CHANGE);
  Serial.begin(9600);

  Kp = 4;

  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);

  CurrentTime = millis();

  ESC6.attach(ESCPin6);
  ESC9.attach(ESCPin9);
  ESC10.attach(ESCPin10);
  ESC11.attach(ESCPin11);

  ESC6.writeMicroseconds(1000);
  ESC9.writeMicroseconds(1000);
  ESC10.writeMicroseconds(1000);
  ESC11.writeMicroseconds(1000);

}

void loop() {
  //Reading the MPU6050 and calcuating the angle based on the accelerometer
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 6, true);
  AcX = Wire.read() << 8 | Wire.read();
  AcY = Wire.read() << 8 | Wire.read();
  AcZ = Wire.read() << 8 | Wire.read();
  AcX = AcX / 16384;
  AcY = AcY / 16384;
  AcZ = AcZ / 16384;

  AccRawX = ((atan((AcY) / sqrt(pow((AcX), 2) + pow((AcZ), 2))) * rad2deg)); //
  AccRawY = ((atan(-1 * (AcX) / sqrt(pow((AcY), 2) + pow((AcZ), 2))) * rad2deg));

  PrevTime = CurrentTime;
  CurrentTime = millis();
  ElapsedTime = (CurrentTime - PrevTime) / 1000;
  // Reading the MPU6050 and calculating the angle based on the Gyroscope
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 4, true);

  GyX = Wire.read() << 8 | Wire.read();
  GyY = Wire.read() << 8 | Wire.read();

  GyX = GyX / 131;
  GyY = GyY / 131;

  GyRawX = GyX * ElapsedTime;
  GyRawY = GyY * ElapsedTime;
  // Using a complimentary filter to calculate the current angle of the quadcopter
  FiltAngleX = (0.98 * (FiltAngleX + GyRawX)) + (0.02 * AccRawX);
  FiltAngleY = (0.98 * (FiltAngleY + GyRawY)) + (0.02 * AccRawY);

  //  Serial.print("Angle X = "); Serial.print(FiltAngleX);
  //  Serial.print(" Angle Y = "); Serial.print(FiltAngleY);

  error = setpoint - FiltAngleY;
  int pid_p = error * Kp;

  //  Serial.print(" Throttle = "); Serial.println(AdjThrottle);

  Ch1SigMain = Ch1PulseDuration;
  if (Ch1NewSignal) {
    Ch1NewSignal = false;
    Ch1PulseMapped = map(Ch1SigMain, 1260,  1708, 1000, 2000);
  }



  FrontSignal = Ch1PulseMapped - pid_p;
  RearSignal = Ch1PulseMapped + pid_p;

  if (FrontSignal < 1000) {
    FrontSignal = 1000;
  }

  if (FrontSignal > 2000) {
    FrontSignal = 2000;
  }

  if (RearSignal < 1000) {
    RearSignal = 1000;
  }

  if (RearSignal > 2000) {
    RearSignal = 2000;
  }

  ESC6.writeMicroseconds(FrontSignal);
  ESC9.writeMicroseconds(FrontSignal);
  ESC10.writeMicroseconds(RearSignal);
  ESC11.writeMicroseconds(RearSignal);
}



void pin_ISR1() {

  if (digitalRead(Ch1) == HIGH) {
    Ch1StartTime = micros();
  }

  else {
    Ch1PulseDuration = int (micros() - Ch1StartTime);
    Ch1StartTime = 0;
    Ch1NewSignal = true;
  }
}

[MarkT]

Have you got props on your motors? Are the ESCs suitable for drone use?

Have you tried characterizing the prop/motor/ESC combination - rpm v. servo pulse width? It's even better to have a rig for measuring thrust too so you can characterize thrust v. pulse width.

[DanThe400Man]

Thanks Mark, yes I have the same problem with props on and off the motors. ESC's are suitable for drone use as per the data sheet.

I haven't tried that but I will give it a go, I have a thrust stand I made to characterise some model rockets last year so I will modify that.

Thanks for the ideas!