How do I control ESCs at 400Hz with Servo library

Hi!

I am making a drone with 4 ESCs, I am wondering how I can control my ESCs at 400Hz instead of 50Hz (I believe thats default for the servo library). This is my code, I set the wire clock to 400000 (400Hz) but I dont think the Servo library does that to:

#include <Servo.h>
#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif

MPU6050 mpu;
Servo motor_LF;
Servo motor_RF;
Servo motor_LB;
Servo motor_RB;

#define INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards
#define LED_PIN 13
bool blinkState = false;

bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

uint8_t teapotPacket[14] = { '$', 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0x00, 0x00, '\r', '\n' };

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
  mpuInterrupt = true;
}

float input_throttle;

/* Valut */
float elapsedTime, time, timePrev;
long loop_timer;

/* Roll PID */
float roll_PID, pid_throttle_L_F, pid_throttle_L_B, pid_throttle_R_F, pid_throttle_R_B, roll_error, roll_previous_error;
float roll_pid_p = 0;
float roll_pid_i = 0;
float roll_pid_d = 0;
/* Roll PID Constants */
double roll_kp = 7;
double roll_ki = 0.006;
double roll_kd = 1.2;
float roll_desired_angle = 0;

/* Pitch PID */
float pitch_PID, pitch_error, pitch_previous_error;
float pitch_pid_p = 0;
float pitch_pid_i = 0;
float pitch_pid_d = 0;
/* Pitch PID Constants */
double pitch_kp = 72;
double pitch_ki = 0.006;
double pitch_kd = 1.22;
float pitch_desired_angle = 0;

/* Accelerometer X PID */
float accX_PID, accX_error, accX_previous_error;
float accX_pid_p = 0;
float accX_pid_i = 0;
float accX_pid_d = 0;
/* Accelerometer X PID Constants */
double accX_kp = 72;
double accX_ki = 0.006;
double accX_kd = 1.22;

/* Accelerometer Y PID */
float accY_PID, accY_error, accY_previous_error;
float accY_pid_p = 0;
float accY_pid_i = 0;
float accY_pid_d = 0;
/* Accelerometer Y PID Constants */
double accY_kp = 72;
double accY_ki = 0.006;
double accY_kd = 1.22;

void setup() {
  motor_LF.attach(4);
  motor_RF.attach(5);
  motor_LB.attach(6);
  motor_RB.attach(7);

  motor_LF.writeMicroseconds(1000);
  motor_RF.writeMicroseconds(1000);
  motor_LB.writeMicroseconds(1000);
  motor_RB.writeMicroseconds(1000);

  // join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif
  Serial.begin(115200);
  while (!Serial);

  // initialize device
  Serial.println(F("Initializing I2C devices..."));
  mpu.initialize();
  pinMode(INTERRUPT_PIN, INPUT);

  // verify connection
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  // load and configure the DMP
  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXGyroOffset(220);
  mpu.setYGyroOffset(76);
  mpu.setZGyroOffset(-85);
  mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

  if (devStatus == 0) {
    // Calibration Time: generate offsets and calibrate our MPU6050
    mpu.CalibrateAccel(6);
    mpu.CalibrateGyro(6);
    mpu.PrintActiveOffsets();
    // turn on the DMP, now that it's ready
    Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);

    // enable Arduino interrupt detection
    Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
    Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
    Serial.println(F(")..."));
    attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();

    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }

  pinMode(LED_PIN, OUTPUT);
}

void loop() {
  // if setup fail dont run the program
  if (!dmpReady) return;

  /* Start Timer */
  timePrev = time;
  time = millis();
  elapsedTime = (time - timePrev) / 1000;

  /* Get throttle value from reciver */
  int read_input_throttle = analogRead(A0);
  input_throttle = map(read_input_throttle, 0, 1023, 1000, 1500);

  /* PID */
  roll_error = roll_desired_angle - (ypr[2] * 180 / M_PI);
  pitch_error = pitch_desired_angle - (ypr[1] * 180 / M_PI);

  /* Acceleromteter PID */
  accX_error = 0 - aaReal.x;
  accY_error = 0 - aaReal.y;

  /* Prospect */
  roll_pid_p = roll_kp * roll_error;
  pitch_pid_p = pitch_kp * pitch_error;

  /* Acceleromteter Prospect */
  accX_pid_p = accX_kp * accX_error;
  accY_pid_p = accY_kp * accY_error;

  /* Integral */
  if (-3 < roll_error < 3)
  {
    roll_pid_i = roll_pid_i + (roll_ki * roll_error);
  }
  if (-3 < pitch_error < 3)
  {
    pitch_pid_i = pitch_pid_i + (pitch_ki * pitch_error);
  }

  /* Acceleromteter Integral */
  if (-3 < accX_error < 3)
  {
    accX_pid_i = accX_pid_i + (accX_ki * accX_error);
  }
  if (-3 < accY_error < 3)
  {
    accY_pid_i = accY_pid_i + (accY_ki * accY_error);
  }


  /* Derivate */
  roll_pid_d = roll_kd * ((roll_error - roll_previous_error) / elapsedTime);
  pitch_pid_d = pitch_kd * ((pitch_error - pitch_previous_error) / elapsedTime);


  /* Acceleromteter Derivate */
  accX_pid_d = accX_kd * ((accX_error - accX_previous_error) / elapsedTime);
  accY_pid_d = accY_kd * ((accY_error - accY_previous_error) / elapsedTime);

  /* PID summery */
  roll_PID = roll_pid_p + roll_pid_i + roll_pid_d;
  pitch_PID = pitch_pid_p + pitch_pid_i + pitch_pid_d;


  /* Acceleromteter PID summery */
  accX_PID = accX_pid_p + accX_pid_i + accX_pid_d;
  accY_PID = accY_pid_p + accY_pid_i + accY_pid_d;

  /* Regulate PID output for ESCs */
  if (roll_PID < -400) {
    roll_PID = -400;
  }
  if (roll_PID > 400) {
    roll_PID = 400;
  }
  if (pitch_PID < -400) {
    pitch_PID = -400;
  }
  if (pitch_PID > 400) {
    pitch_PID = 400;
  }


  /* Regulate Acceleromteter PID */
  if (accX_PID > 20) {
    accX_PID = 20;
  }
  if (accY_PID > 20) {
    accY_PID = 20;
  }
  if (accY_PID < -20) {
    accY_PID = -20;
  }

  /* Set desired angle for Accelerometer stabilization */
  if (roll_desired_angle == 0) {
    roll_desired_angle = accX_PID;
  }
  if (pitch_desired_angle == 0) {
    pitch_desired_angle = accY_PID;
  }

  /* Set the throttle PID for each motor */
  pid_throttle_R_F = input_throttle + roll_PID - pitch_PID;
  pid_throttle_L_F = input_throttle - roll_PID - pitch_PID;
  pid_throttle_R_B = input_throttle + roll_PID + pitch_PID;
  pid_throttle_L_B = input_throttle - roll_PID + pitch_PID;

  /* Regulate throttle for ESCs */
  //Right front
  if (pid_throttle_R_F < 1100)
  {
    pid_throttle_R_F = 1100;
  }
  if (pid_throttle_R_F > 2000)
  {
    pid_throttle_R_F = 2000;
  }

  //Left front
  if (pid_throttle_L_F < 1100)
  {
    pid_throttle_L_F = 1100;
  }
  if (pid_throttle_L_F > 2000)
  {
    pid_throttle_L_F = 2000;
  }

  //Right back
  if (pid_throttle_R_B < 1100)
  {
    pid_throttle_R_B = 1100;
  }
  if (pid_throttle_R_B > 2000)
  {
    pid_throttle_R_B = 2000;
  }

  //Left back
  if (pid_throttle_L_B < 1100)
  {
    pid_throttle_L_B = 1100;
  }
  if (pid_throttle_L_B > 2000)
  {
    pid_throttle_L_B = 2000;
  }

  /* Save Previous Error */
  roll_previous_error = roll_error;
  pitch_previous_error = pitch_error;

  /* Save Acceleromteter Previous Error */
  accX_previous_error = accX_error;
  accY_previous_error = accY_error;


  Serial.println(input_throttle);

  if (input_throttle < 1100) {
    pid_throttle_L_F = input_throttle;
    pid_throttle_L_B = input_throttle;
    pid_throttle_R_F = input_throttle;
    pid_throttle_R_B = input_throttle;

    roll_error = 0;
    pitch_error = 0;
    roll_previous_error = 0;
    pitch_previous_error = 0;
  }

  motor_LF.writeMicroseconds(pid_throttle_L_F);
  motor_RF.writeMicroseconds(pid_throttle_R_F);
  motor_LB.writeMicroseconds(pid_throttle_L_B);
  motor_RB.writeMicroseconds(pid_throttle_R_B);

  mpu.dmpGetQuaternion(&q, fifoBuffer);
  mpu.dmpGetGravity(&gravity, &q);
  mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

  Serial.print("ypr\t");
  Serial.print(ypr[0] * 180 / M_PI);
  Serial.print("\t");
  Serial.print(ypr[1] * 180 / M_PI);
  Serial.print("\t");
  Serial.println(ypr[2] * 180 / M_PI);
}

Thanks beforehand!
Best regards Max

Are the ESCs capable of operating with 400Hz signals?

In the manual it says that the PWM operating frequency is 8kHz - 16kHz.

I did a search for "400Hz esc servo library arduino" and got some hits.
https://github.com/superjax/ESC
and more.