This gist support youtube video: https://youtu.be/slYDzuUZN9A

simple_bgc_gimbal_demo.ini

/*
# Board selection
platform = atmelavr
board = pro16MHzatmega328
framework = arduino
monitor_speed = 38400

lib_deps =
    ; Simple FOC#minimal
    jrowberg/I2Cdevlib-MPU6050

*/

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
// This uses SimpleFOC but from the 'minimal branch' as this board has very little program space! 
#include "src/BLDCMotor.h"
#include "src/drivers/BLDCDriver3PWM.h"

MPU6050 mpu;

BLDCMotor motor1 = BLDCMotor(7);
BLDCMotor motor2 = BLDCMotor(7);

BLDCDriver3PWM driver1 = BLDCDriver3PWM(3, 5, 6);
BLDCDriver3PWM driver2 = BLDCDriver3PWM(9, 10, 11);

uint8_t fifoBuffer[64];
Quaternion q;        // [w, x, y, z]         quaternion container
VectorFloat gravity; // [x, y, z]            gravity vector
float ypr[3];        // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

void setup()
{
    Wire.begin();
    Wire.setClock(400000);

    Serial.begin(38400);
    delay(1000);

    driver1.init();
    driver2.init();

    motor1.linkDriver(&driver1);
    motor2.linkDriver(&driver2);

    motor1.controller = angle_openloop;
    motor1.voltage_limit = 1.5;
    motor1.velocity_limit = 5;
    motor1.init();

    motor2.controller = angle_openloop;
    motor2.voltage_limit = 1.5;
    motor2.velocity_limit = 5;
    motor2.init();

    mpu.initialize();

    // Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
    uint8_t devStatus = mpu.dmpInitialize();

    if (devStatus == 0)
    {
        mpu.CalibrateAccel(6);
        mpu.CalibrateGyro(6);
        mpu.setDMPEnabled(true);
        Serial.print(F("DMP Initialization Success "));
    }
    else
    {
        Serial.print(F("DMP Initialization failed fail "));
        return;
    }
}

float pitch = 0;
float roll = 0;

void loop()
{
    // Serial.print(".");

    if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer))
    {
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
        pitch = (ypr[1] * 180 / M_PI) / 10;
        roll = (ypr[2] * 180 / M_PI) / 10;

        Serial.print(pitch);
        Serial.print(F("\t"));
        Serial.println(roll);
    }

      motor1.move(pitch);
      motor2.move(roll);
}