UBEC ESC stuck in programming mode / Flight with drone, dont understand why takeoff is so bad

Projects Programming
1

Hi!

I did one of my first takeoffs with my drone today and it is really struggling on the takeoff and I cannot understand why, my PID calcullation should add upp for a stable takeoff in my brain but it doesnt, I will attach a video with the flight and here is my code:


#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <Servo.h>
#include <nRF24L01.h>
#include <SPI.h>
#include <RF24.h>
#include <printf.h>
#include <TinyGPS++.h>

#define CE_PIN   9
#define CSN_PIN 10

// Create Radio and define pins
RF24 radio(CE_PIN, CSN_PIN);

// Create Radio channels
const uint64_t pAddress = 0xB00B1E5000LL;

// Create a boolean for connection lost
bool lostConnection = false;

bool AutoLevelMode = true;
bool AcroMode = false;

//Radio recived array
float recivedDataArray[5];
bool temp = true;

// Create timer
float elapsedTime, time, timePrev;
long loop_timer;
float previousMessageMillis;

Servo motor_LF;
Servo motor_RF;
Servo motor_LB;
Servo motor_RB;

Adafruit_BNO055 bno = Adafruit_BNO055(-1, 0x28);

bool setZeroBool = true;

float input_throttle = 1000;
float input_yaw = 0;
float input_pitch = 0;
float input_roll = 0;

/* 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 = 0.5;
double roll_ki = 0.03;
double roll_kd = 20;
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 = roll_kp;
double pitch_ki = roll_ki;
double pitch_kd = roll_kd;
float pitch_desired_angle = 0;


/* Yaw PID */
float yaw_PID, yaw_error, yaw_previous_error;
float yaw_pid_p = 0;
float yaw_pid_i = 0;
float yaw_pid_d = 0;
/* Pitch PID Constants */
double yaw_kp = 0.5;
double yaw_ki = 0;
double yaw_kd = 5;
float yaw_desired_angle = 0;

int yaw_pid_max = 200;
int pid_max = 400;

float pitchOffset = 0, rollOffset = 0;

//GPS Variables
float longitude = 0;
float latitude = 0;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  
  while (!Serial);
  printf_begin();

  if(!bno.begin())
  {
    Serial.print("Error, no BNO055 detected ... Check your wiring or I2C ADDR!");
    while(1);
  }

  int8_t temp = bno.getTemp();
  bno.setExtCrystalUse(true);

  delay(1000);

  if (!radio.begin()) {
    Serial.println(F("radio hardware is not responding!!"));
    while (1) {}
  }
  radio.setPALevel(RF24_PA_MAX);
  radio.openReadingPipe(1, pAddress);

  //radio.printDetails();
  radio.startListening();

  delay(5000);

  motor_LF.attach(5);
  motor_RF.attach(6);
  motor_LB.attach(7);
  motor_RB.attach(8);

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

void loop() {
  imu::Vector<3> acro;
  imu::Vector<3> gyro;

  if(AutoLevelMode) {
    imu::Vector<3> gyro = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
  } else if(AcroMode) {
    imu::Vector<3> acro = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
  }
  
  if (!lostConnection) {
    if (radio.available()) {
      radio.read(&recivedDataArray, sizeof(recivedDataArray));
      Serial.println("Recieved array:");
      previousMessageMillis = millis();
      for (byte i = 0; i < 4; i++) {
        Serial.println(recivedDataArray[i]);
      }
      input_throttle = recivedDataArray[0];
      yaw_desired_angle += recivedDataArray[1];
      roll_desired_angle = -recivedDataArray[2] + rollOffset;
      pitch_desired_angle = recivedDataArray[3] + pitchOffset;

      //Set flightmode
      if(recivedDataArray[4] = 0) {
        AutoLevelMode = true;
        AcroMode = false;
      } else if(recivedDataArray[4] = 1) {
        AutoLevelMode = false;
        AcroMode = true;
      }
      //Serial.println();
    }

    if(yaw_desired_angle > 360) {
      yaw_desired_angle = 0;
    } else if(yaw_desired_angle < 0) {
      yaw_desired_angle = 360;
    }

    if (input_throttle > 1600) {
      input_throttle = 1600;
    } else if (input_throttle < 1050) {
      input_throttle = 1000;
    }

    if(AutoLevelMode) {
      SetZero(gyro.x(), gyro.y(), gyro.z());
      PIDControl(gyro.x(), gyro.y(), gyro.z());
    } else if(AcroMode) {
      PIDControl(acro.x(), acro.y(), acro.z());
    }

    timePrev = time;
    time = millis();
    elapsedTime = (time - timePrev) / 1000;

    if (millis() - previousMessageMillis >= 400) {
      lostConnection = false;
      PID_reset();
      motor_LF.writeMicroseconds(1000);
      motor_RF.writeMicroseconds(1000);
      motor_LB.writeMicroseconds(1000);
      motor_RB.writeMicroseconds(1000);
    } else {
      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);
    }

    //print_gps();
    //print_roll_pitch_yaw(gyro.x(), gyro.y(), gyro.z());
    //printDesiredAngles();
    //print_PID();
    //print_throttle();
    //print_detailed_PID();
  } else {
    PID_reset();
    Serial.println("LOST CONNECTION!");
    //delay(2000);
  }
  delay(10);
}

void PIDControl(float x, float y, float z) {
  if (input_throttle > 1050) {
    /* PID */
    roll_error = roll_desired_angle - z;
    pitch_error = pitch_desired_angle - y;
    yaw_error = yaw_desired_angle - x;

    /* Prospect */
    roll_pid_p = roll_kp * roll_error;
    pitch_pid_p = pitch_kp * pitch_error;
    yaw_pid_p = yaw_kp * yaw_error;

    // Integral
    roll_pid_i += roll_ki * roll_error;
    pitch_pid_i += pitch_ki * pitch_error;
    yaw_pid_i += yaw_ki * yaw_error;

    // Derivate
    roll_pid_d = roll_kd * (roll_error - roll_previous_error);
    pitch_pid_d = pitch_kd * (pitch_error - pitch_previous_error);
    yaw_pid_d = yaw_kd * (yaw_error - yaw_previous_error);

    // ROLL
    if (roll_pid_i > pid_max) roll_pid_i = pid_max;
    else if (roll_pid_i < pid_max * -1)
      roll_pid_i = pid_max * -1;

    roll_PID = roll_pid_p + roll_pid_i + roll_pid_d;
    if (roll_PID > pid_max) roll_PID = pid_max;
    else if (roll_PID < pid_max * -1)
      roll_PID = pid_max * -1;

    // PITCH
    if (pitch_pid_i > pid_max) pitch_pid_i = pid_max;
    else if (pitch_pid_i < pid_max * -1)
      pitch_pid_i = pid_max * -1;

    pitch_PID = pitch_pid_p + pitch_pid_i + pitch_pid_d;
    if (pitch_PID > pid_max) pitch_PID = pid_max;
    else if (pitch_PID < pid_max * -1)
      pitch_PID = pid_max * -1;

    // YAW
    if (yaw_pid_i > yaw_pid_max) yaw_pid_i = yaw_pid_max;
    else if (yaw_pid_i < yaw_pid_max * -1)
      yaw_pid_i = yaw_pid_max * -1;

    yaw_PID = yaw_pid_p + yaw_pid_i + yaw_pid_d;
    if (yaw_PID > yaw_pid_max) yaw_PID = yaw_pid_max;
    else if (yaw_PID < yaw_pid_max * -1)
      yaw_PID = yaw_pid_max * -1;

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

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

    /* 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;
    }
  } else {
    PID_reset();
  }
}

void print_roll_pitch_yaw(float x, float y, float z) {
  Serial.print(z);
  Serial.print(", ");
  Serial.println(y);
}

void printDesiredAngles() {
  Serial.print(roll_desired_angle);
  Serial.print(", ");
  Serial.println(pitch_desired_angle);
}

void print_throttle() {
  Serial.print(pid_throttle_L_F);
  Serial.print("   ");
  Serial.println(pid_throttle_R_F);
  Serial.print(pid_throttle_L_B);
  Serial.print("   ");
  Serial.println(pid_throttle_R_B);
  Serial.println();
}

void print_PID() {
  Serial.print(roll_PID);
  Serial.print(", ");
  Serial.print(pitch_PID);
  Serial.print(", ");
  Serial.println(yaw_PID);
}

void print_detailed_PID() {
  Serial.print(pitch_pid_p);
  Serial.print(", ");
  Serial.print(pitch_pid_i);
  Serial.print(", ");
  Serial.print(pitch_pid_d);
  Serial.print(", ");
  Serial.println(pitch_PID);
}

void SetZero(float x, float y, float z) {
  if (setZeroBool) {
      pitchOffset = y;
      rollOffset = z;
      yaw_desired_angle = x;

      setZeroBool = false;
    }
}

void PID_reset() {
  pid_throttle_L_F = 1000;
  pid_throttle_L_B = 1000;
  pid_throttle_R_F = 1000;
  pid_throttle_R_B = 1000;

  pitch_PID = 0, roll_PID = 0, yaw_PID = 0;
  roll_error = 0, pitch_error = 0, yaw_error = 0;
  roll_previous_error = 0, pitch_previous_error = 0, yaw_previous_error = 0;
  
  roll_pid_p = 0, pitch_pid_p = 0;
  roll_pid_i = 0, pitch_pid_i = 0;
  roll_pid_d = 0, pitch_pid_d = 0;

  pitch_pid_p = 0, pitch_pid_p = 0;
  pitch_pid_i = 0, pitch_pid_i = 0;
  pitch_pid_d = 0, pitch_pid_d = 0;

  yaw_pid_p = 0, yaw_pid_p = 0;
  yaw_pid_i = 0, yaw_pid_i = 0;
  yaw_pid_d = 0, yaw_pid_d = 0;
}

Link to flight video:

Thanks in advance!
Best regards Max

1 Like
2

The drone is not in balance. I suggest clamping it to the ground, check up all voltages and start debugging the code.

3

Your topic was MOVED to its current forum category as it is more suitable than the original

4

Okey, I checked the voltage and everything seems good, something I have noticed that I dont belive should happend is that the motors "randomly" spins unevenly quick bursts every 5 seconds or so, how can I debug that?

5

Erratic behavior of motorized systems is often caused by poor power supply decoupling and/or inadequate voltage regulation, which can cause the processor to malfunction or reboot.

6

Okay, my power management is as followed:

14.4V battery -> 4 UBEC ESC -> Arduino

Is that a problem? Should I maybe a capacitor in between the ESC and the Arduino to minimize rapid power "changes"?

7

Milli ampere hours...? The capacity of the battery sets the time You can run the stuff.

I suggest decoupling capacitors "here and there". They will absorb eventual noice.

How to debug..... Use serial monitor and Serial.print. Start by printing the data used by the PID.

8

Please post a schematic diagram of the entire power supply and connections.

9

Here is my schimatic/merged with pcb (not very beautiful :slight_smile: ):

image
image2059×1056 293 KB

10

I think I found something! My gyro outputs was 0.0 and 0.0 after I implemented two different flightmodes, I took for sure that it would work so I never "tested th output" Ill try it out now and se how it goes!

11

When tuning up pid loops on a drone you want it tethered so it doesn't keep crashing or you'll run out of replacement propellors.... If you can mount it on some sort of gimbal you can tune the pitch and roll loops independently.

Take off is the worse time for control loops as they will saturate while the craft is on the ground (unless its really really level), and just have to do a step-recovery once off the ground.

Best to have an open-loop launch routine designed to clear the ground, then engage PID feedback (having zeroed any integral windup that's accrued during launch).

12

I tried to set pinMode to OUTPUT but then the ESCs went crazy, now they sound like this:

Help :smiley:

13

For creating any kind of signal regardless of beeing a simple LED on/off I2C, SPI or RC-PWM-signal the IO-pin that shall deliver the signal must be set as output.
Your ESCs would have never started rotating any motor if they would have been not set to output.

Try to estimate how many manufactures of ESCs can deliver ESCs?
The number of user-manuals will be the doubled number.
There is absolutely no standard on how ESC behave and what special signal-sequence they need to be adjusted or what beep-tone means what.

So without a user-manual for exact your type of ESC give the project a new purpose:
writing symphonies for R2D2's

best regards Stefan

14

Okay, heres the manual https://cdn-global-hk.hobbyking.com/media/file/811103388X7478X58.pdf I am using 4 of theese https://hobbyking.com/en_us/hobbyking-20a-2-4s-esc-3a-ubec.html?___store=en_us

15

Nor useful. No power/ground connections to the Arduino, for example.

16

From what I understand my ESCs are stuck in programming mode, and I cannot manage to get them out of ot.......

17

I dont understand, I tried one of my "old" versions of the a different PID setup and that works, this sketch works:

#include <Adafruit_Sensor.h>

#include <Adafruit_BNO055.h>

#include <utility/imumaths.h>

#include <Servo.h>

#include <nRF24L01.h>

#include <SPI.h>

#include <RF24.h>

#include <printf.h>

#define CE_PIN   9

#define CSN_PIN 10

// Create Radio and define pins

RF24 radio(CE_PIN, CSN_PIN);

// Create Radio channels

const uint64_t pAddress = 0xB00B1E5000LL;

//Radio recived array

float recivedDataArray[4];

// Create timer

float elapsedTime, time, timePrev;

long loop_timer;

float previousMessageMillis;

// Create motors

Servo motor_LF;

Servo motor_RF;

Servo motor_LB;

Servo motor_RB;

// Create a IMU

Adafruit_BNO055 bno = Adafruit_BNO055(-1, 0x28);

/* Temporary variable declaration */

float input_throttle = 1000;

/* 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 = 1;

double roll_ki = 0.02;

double roll_kd = 5;

float desired_roll_dps = 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 = roll_kp;

double pitch_ki = roll_ki;

double pitch_kd = roll_kd;

float desired_pitch_dps = 0;

/* Yaw PID */

float yaw_PID, yaw_error, yaw_previous_error;

float yaw_pid_p = 0;

float yaw_pid_i = 0;

float yaw_pid_d = 0;

/* Pitch PID Constants */

double yaw_kp = roll_kp;

double yaw_ki = roll_ki;

double yaw_kd = roll_kd;

float desired_yaw_dps = 0;

/* Limit the PID so that it doesnt go crazy */

int pid_max = 200;

/* Low pass filter variables for the gyroscope and accelerometer */

float IMURollOld = 0, IMURollNew = 0, IMUPitchOld = 0, IMUPitchNew = 0;

float AccXOld = 0, AccYOld = 0, AccXNew = 0, AccYNew = 0;

/* Accelerometer Stabilization variables */

bool accelStabilization = true;

/* Accel PID */

float accX_PID, accY_PID, accY_error, accY_previous_error, accX_error, accX_previous_error;

float accX_pid_p = 0;

float accX_pid_i = 0;

float accX_pid_d = 0;

float accY_pid_p = 0;

float accY_pid_i = 0;

float accY_pid_d = 0;

/* Accel PID Constants */

double acc_kp = 1;

double acc_ki = 0.05;

double acc_kd = 5;

void setup() {

  pinMode(5, OUTPUT);

  pinMode(6, OUTPUT);

  pinMode(7, OUTPUT);

  pinMode(8, OUTPUT);

  Serial.begin(115200);

  Wire.begin();

  

  while (!Serial);

  printf_begin();

  if (!bno.begin())

  {

    Serial.print("Error, no BNO055 detected ... Check your wiring or I2C ADDR!");

    while (1);

  }

  int8_t temp = bno.getTemp();

  bno.setExtCrystalUse(true);

  delay(1000);

  if (!radio.begin()) {

    Serial.println(F("radio hardware is not responding!!"));

    while (1) {}

  }

  radio.setPALevel(RF24_PA_MAX);

  radio.openReadingPipe(1, pAddress);

  //radio.printDetails();

  radio.startListening();

  delay(5000);

  motor_LF.attach(5);

  motor_RF.attach(6);

  motor_LB.attach(7);

  motor_RB.attach(8);

  motor_LF.writeMicroseconds(1000);

  motor_RF.writeMicroseconds(1000);

  motor_LB.writeMicroseconds(1000);

  motor_RB.writeMicroseconds(1000);

}

void loop() {

  imu::Vector<3> gyro = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);

  imu::Vector<3> accel = bno.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);

  if (radio.available()) {

    radio.read( &recivedDataArray, sizeof(recivedDataArray) );

    //Serial.println("Recieved array:");

    previousMessageMillis = millis();

    for (byte i = 0; i < 4; i++) {

      //Serial.println(recivedDataArray[i]);

    }

    input_throttle = recivedDataArray[0];

    //desired_yaw_dps += recivedDataArray[1];

    desired_pitch_dps = recivedDataArray[2] * 2;

    desired_roll_dps = recivedDataArray[3] * 2;

  }

  if (input_throttle > 1500) {

    input_throttle = 1500;

  } else if (input_throttle < 1050) {

    input_throttle = 1000;

  }

  acro(gyro.x(), gyro.y(), gyro.z());

  /* Start Timer */

  timePrev = time;

  time = millis();

  elapsedTime = (time - timePrev) / 1000;

  if (millis() - previousMessageMillis >= 400) {

    //PID_reset();

  } else {

    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);

  }

  delay(20);

  //printPID();

  //printTrottle();

  //printDPS();

  printGyro(gyro.x(), gyro.y());

  //printAccel(accel.x(), accel.y());

}

void printDPS() {

  Serial.print("Roll DPS: ");

  Serial.println(desired_roll_dps);

  Serial.print("Pitch DPS: ");

  Serial.println(desired_pitch_dps);

}

void printTrottle() {

  Serial.print(pid_throttle_L_F);

  Serial.print("  ");

  Serial.println(pid_throttle_R_F);

  Serial.print(pid_throttle_L_B);

  Serial.print("  ");

  Serial.println(pid_throttle_R_B);

  Serial.println();

}

void printGyro(float x, float y) {

  Serial.print(x);

  Serial.print(",");

  Serial.print(y);

  Serial.print(",");

  Serial.print(IMURollNew);

  Serial.print(",");

  Serial.println(IMUPitchNew);

}

void printPID() {

  Serial.print("Roll PID: ");

  Serial.println(roll_PID);

  Serial.print("Pitch PID: ");

  Serial.println(pitch_PID);

}

void printAccel(float x, float y) {

  Serial.print(x);

  Serial.print(",");

  Serial.print(y);

  Serial.print(",");

  Serial.print(AccXNew);

  Serial.print(",");

  Serial.println(AccYNew);

}

void acro(float gyroX, float gyroY, float gyroZ) {

  // Gyroscope low pass filter

  IMURollNew = ((gyroX) * 0.1) + (IMURollOld * 0.9);

  IMUPitchNew = ((gyroY) * 0.1) + (IMUPitchOld * 0.9);

  if (input_throttle > 1100) {

    roll_error = IMURollOld - desired_roll_dps;

    pitch_error = IMUPitchOld - desired_pitch_dps;

    yaw_error = gyroZ - desired_yaw_dps;

    /* Prospect */

    roll_pid_p = roll_kp * roll_error;

    pitch_pid_p = pitch_kp * pitch_error;

    yaw_pid_p = yaw_kp * yaw_error;

    // Integral

    roll_pid_i += roll_ki * roll_error;

    pitch_pid_i += pitch_ki * pitch_error;

    yaw_pid_i += yaw_ki * yaw_error;

    //ROLL

    if (roll_pid_i > pid_max) roll_pid_i = pid_max;

    else if (roll_pid_i < pid_max * -1) roll_pid_i = pid_max * -1;

    roll_PID = roll_kp * roll_error + roll_pid_i + roll_kd * (roll_error - roll_previous_error);

    if (roll_PID > pid_max) roll_PID = pid_max;

    else if (roll_PID < pid_max * -1) roll_PID = pid_max * -1;

    //PITCH

    if (pitch_pid_i > pid_max) pitch_pid_i = pid_max;

    else if (pitch_pid_i < pid_max * -1) pitch_pid_i = pid_max * -1;

    pitch_PID = pitch_kp * pitch_error + pitch_pid_i + pitch_kd * (pitch_error - pitch_previous_error);

    if (pitch_PID > pid_max) pitch_PID = pid_max;

    else if (pitch_PID < pid_max * -1) pitch_PID = pid_max * -1;

    //YAW

    if (yaw_pid_i > pid_max) yaw_pid_i = pid_max;

    else if (yaw_pid_i < pid_max * -1) yaw_pid_i = pid_max * -1;

    yaw_PID = yaw_kp * yaw_error + yaw_pid_i + yaw_kd * (yaw_error - yaw_previous_error);

    if (yaw_PID > pid_max) yaw_PID = pid_max;

    else if (yaw_PID < pid_max * -1) yaw_PID = pid_max * -1;

    /* Set the throttle PID for each motor */

    pid_throttle_L_F = input_throttle - roll_PID + pitch_PID - yaw_PID;

    pid_throttle_R_F = input_throttle + roll_PID + pitch_PID + yaw_PID;

    pid_throttle_L_B = input_throttle - roll_PID - pitch_PID + yaw_PID;

    pid_throttle_R_B = input_throttle + roll_PID - pitch_PID - yaw_PID;

    /* Save Previous Error */

    roll_previous_error = roll_error;

    pitch_previous_error = pitch_error;

    yaw_previous_error = yaw_error;

  } else {

    PID_reset();

  }

  IMURollOld = IMURollNew;

  IMUPitchOld = IMUPitchNew;

}

void PID_reset() {

  //Serial.println("RESET");

  pid_throttle_L_F = 1000;

  pid_throttle_L_B = 1000;

  pid_throttle_R_F = 1000;

  pid_throttle_R_B = 1000;

  roll_pid_p = 0;

  roll_pid_i = 0;

  roll_pid_d = 0;

  pitch_pid_p = 0;

  pitch_pid_i = 0;

  pitch_pid_d = 0;

  pitch_PID = 0;

  roll_PID = 0;

  yaw_PID = 0;

  roll_error = 0;

  pitch_error = 0;

  yaw_error = 0;

  roll_previous_error = 0;

  pitch_previous_error = 0;

  yaw_previous_error = 0;

}

void accelStable(float accX, float accY) {

  // Accelerometer low pass filter

  AccXNew = (accX * 0.2) + (AccXOld * 0.8);

  AccYNew = (accY * 0.2) + (AccYOld * 0.8);

  accX_error = 0 - AccXNew;

  accY_error = 0 - AccYNew;

  accX_pid_p = acc_kp * accX_previous_error;

  accY_pid_p = acc_kp * accY_previous_error;

  

  accX_pid_i = acc_ki * accX_previous_error;

  accY_pid_i = acc_ki * accY_previous_error;

  //ACCX STABILIZATION

  if (accX_pid_i > pid_max) accX_pid_i = pid_max;

  else if (accX_pid_i < pid_max * -1) accX_pid_i = pid_max * -1;

  accX_PID = acc_kp * accX_error + accX_pid_i + acc_kd * (accX_error - accX_previous_error);

  if (accX_PID > pid_max) accX_PID = pid_max;

  else if (accX_PID < pid_max * -1) accX_PID = pid_max * -1;

  //accY STABILIZATION

  if (accY_pid_i > pid_max) accY_pid_i = pid_max;

  else if (accY_pid_i < pid_max * -1) accY_pid_i = pid_max * -1;

  accY_PID = acc_kp * accY_error + accY_pid_i + acc_kd * (accY_error - accY_previous_error);

  if (accY_PID > pid_max) accY_PID = pid_max;

  else if (accY_PID < pid_max * -1) accY_PID = pid_max * -1;

  //desired_pitch_dps = -accX_PID;

  //desired_roll_dps = accY_PID;

  accX_previous_error = 0 - AccXNew;

  accY_previous_error = 0 - AccYNew;

  AccXOld = AccXNew;

  AccYOld = AccYNew;

}

But this one doesnt (the one Ive used/am using):

#include <Adafruit_Sensor.h>

#include <Adafruit_BNO055.h>

#include <utility/imumaths.h>

#include <Servo.h>

#include <nRF24L01.h>

#include <SPI.h>

#include <RF24.h>

#include <printf.h>

#define CE_PIN   9

#define CSN_PIN 10

// Create Radio and define pins

RF24 radio(CE_PIN, CSN_PIN);

// Create Radio channels

const uint64_t pAddress = 0xB00B1E5000LL;

// Create a boolean for connection lost

bool lostConnection = false;

imu::Vector<3> acro;

imu::Vector<3> gyro;

//Radio recived array

float recivedDataArray[4];

bool temp = true;

// Create timer

float elapsedTime, time, timePrev;

long loop_timer;

float previousMessageMillis;

Servo motor_LF;

Servo motor_RF;

Servo motor_LB;

Servo motor_RB;

Adafruit_BNO055 bno = Adafruit_BNO055(-1, 0x28);

bool setZeroBool = true;

float input_throttle = 1000;

float input_yaw = 0;

float input_pitch = 0;

float input_roll = 0;

/* 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 = 0.5;

double roll_ki = 0.03;

double roll_kd = 20;

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 = roll_kp;

double pitch_ki = roll_ki;

double pitch_kd = roll_kd;

float pitch_desired_angle = 0;

/* Yaw PID */

float yaw_PID, yaw_error, yaw_previous_error;

float yaw_pid_p = 0;

float yaw_pid_i = 0;

float yaw_pid_d = 0;

/* Pitch PID Constants */

double yaw_kp = 0.5;

double yaw_ki = 0;

double yaw_kd = 5;

float yaw_desired_angle = 0;

int yaw_pid_max = 200;

int pid_max = 400;

float pitchOffset = 0, rollOffset = 0;

void setup() {

  pinMode(5, OUTPUT);

  pinMode(6, OUTPUT);

  pinMode(7, OUTPUT);

  pinMode(8, OUTPUT);

  Serial.begin(115200);

  Wire.begin();

  

  while (!Serial);

  printf_begin();

  if(!bno.begin())

  {

    Serial.print("Error, no BNO055 detected ... Check your wiring or I2C ADDR!");

    while(1);

  }

  int8_t temp = bno.getTemp();

  bno.setExtCrystalUse(true);

  delay(1000);

  if (!radio.begin()) {

    Serial.println(F("radio hardware is not responding!!"));

    while (1) {}

  }

  radio.setPALevel(RF24_PA_MAX);

  radio.openReadingPipe(1, pAddress);

  //radio.printDetails();

  radio.startListening();

  delay(5000);

  motor_LF.attach(5);

  motor_RF.attach(6);

  motor_LB.attach(7);

  motor_RB.attach(8);

  motor_LF.writeMicroseconds(1000);

  motor_RF.writeMicroseconds(1000);

  motor_LB.writeMicroseconds(1000);

  motor_RB.writeMicroseconds(1000);

}

void loop() {

  imu::Vector<3> gyro = bno.getVector(Adafruit_BNO055::VECTOR_EULER);

  

  if (!lostConnection) {

    if (radio.available()) {

      radio.read(&recivedDataArray, sizeof(recivedDataArray));

      //Serial.println("Recieved array:");

      previousMessageMillis = millis();

      for (byte i = 0; i < 4; i++) {

        //Serial.println(recivedDataArray[i]);

      }

      input_throttle = recivedDataArray[0];

      yaw_desired_angle += recivedDataArray[1];

      roll_desired_angle = -recivedDataArray[2] + rollOffset;

      pitch_desired_angle = recivedDataArray[3] + pitchOffset;

      //Serial.println();

    }

    if(yaw_desired_angle > 360) {

      yaw_desired_angle = 0;

    } else if(yaw_desired_angle < 0) {

      yaw_desired_angle = 360;

    }

    if (input_throttle > 1600) {

      input_throttle = 1600;

    } else if (input_throttle < 1050) {

      input_throttle = 1000;

    }

    PIDControl(gyro.x(), gyro.y(), gyro.z());

    timePrev = time;

    time = millis();

    elapsedTime = (time - timePrev) / 1000;

    if (millis() - previousMessageMillis >= 400) {

      lostConnection = false;

      PID_reset();

      motor_LF.writeMicroseconds(1000);

      motor_RF.writeMicroseconds(1000);

      motor_LB.writeMicroseconds(1000);

      motor_RB.writeMicroseconds(1000);

    } else {

      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);

    }

    //print_gps();

    //print_roll_pitch_yaw(gyro.x(), gyro.y(), gyro.z());

    //printDesiredAngles();

    //print_PID();

    //print_throttle();

    //print_detailed_PID();

  } else {

    PID_reset();

    Serial.println("LOST CONNECTION!");

    //delay(2000);

  }

  delay(10);

}

void PIDControl(float x, float y, float z) {

  if (input_throttle > 1050) {

    /* PID */

    roll_error = roll_desired_angle - z;

    pitch_error = pitch_desired_angle - y;

    yaw_error = yaw_desired_angle - x;

    /* Prospect */

    roll_pid_p = roll_kp * roll_error;

    pitch_pid_p = pitch_kp * pitch_error;

    yaw_pid_p = yaw_kp * yaw_error;

    // Integral

    roll_pid_i += roll_ki * roll_error;

    pitch_pid_i += pitch_ki * pitch_error;

    yaw_pid_i += yaw_ki * yaw_error;

    // Derivate

    roll_pid_d = roll_kd * (roll_error - roll_previous_error);

    pitch_pid_d = pitch_kd * (pitch_error - pitch_previous_error);

    yaw_pid_d = yaw_kd * (yaw_error - yaw_previous_error);

    // ROLL

    if (roll_pid_i > pid_max) roll_pid_i = pid_max;

    else if (roll_pid_i < pid_max * -1)

      roll_pid_i = pid_max * -1;

    roll_PID = roll_pid_p + roll_pid_i + roll_pid_d;

    if (roll_PID > pid_max) roll_PID = pid_max;

    else if (roll_PID < pid_max * -1)

      roll_PID = pid_max * -1;

    // PITCH

    if (pitch_pid_i > pid_max) pitch_pid_i = pid_max;

    else if (pitch_pid_i < pid_max * -1)

      pitch_pid_i = pid_max * -1;

    pitch_PID = pitch_pid_p + pitch_pid_i + pitch_pid_d;

    if (pitch_PID > pid_max) pitch_PID = pid_max;

    else if (pitch_PID < pid_max * -1)

      pitch_PID = pid_max * -1;

    // YAW

    if (yaw_pid_i > yaw_pid_max) yaw_pid_i = yaw_pid_max;

    else if (yaw_pid_i < yaw_pid_max * -1)

      yaw_pid_i = yaw_pid_max * -1;

    yaw_PID = yaw_pid_p + yaw_pid_i + yaw_pid_d;

    if (yaw_PID > yaw_pid_max) yaw_PID = yaw_pid_max;

    else if (yaw_PID < yaw_pid_max * -1)

      yaw_PID = yaw_pid_max * -1;

    /* Set the throttle PID for each motor */

    pid_throttle_L_F = input_throttle - roll_PID + pitch_PID - yaw_PID;

    pid_throttle_R_F = input_throttle + roll_PID + pitch_PID + yaw_PID;

    pid_throttle_L_B = input_throttle - roll_PID - pitch_PID + yaw_PID;

    pid_throttle_R_B = input_throttle + roll_PID - pitch_PID - yaw_PID;

    /* Save Previous Error */

    roll_previous_error = roll_error;

    pitch_previous_error = pitch_error;

    yaw_previous_error = yaw_error;

    /* 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;

    }

  } else {

    PID_reset();

  }

}

void print_roll_pitch_yaw(float x, float y, float z) {

  Serial.print(z);

  Serial.print(", ");

  Serial.println(y);

}

void printDesiredAngles() {

  Serial.print(roll_desired_angle);

  Serial.print(",");

  Serial.println(pitch_desired_angle);

}

void print_throttle() {

  Serial.print(pid_throttle_L_F);

  Serial.print("   ");

  Serial.println(pid_throttle_R_F);

  Serial.print(pid_throttle_L_B);

  Serial.print("   ");

  Serial.println(pid_throttle_R_B);

  Serial.println();

}

void print_PID() {

  Serial.print(roll_PID);

  Serial.print(", ");

  Serial.print(pitch_PID);

  Serial.print(", ");

  Serial.println(yaw_PID);

}

void print_detailed_PID() {

  Serial.print(pitch_pid_p);

  Serial.print(", ");

  Serial.print(pitch_pid_i);

  Serial.print(", ");

  Serial.print(pitch_pid_d);

  Serial.print(", ");

  Serial.println(pitch_PID);

}

void SetZero(float x, float y, float z) {

  if (setZeroBool) {

      pitchOffset = y;

      rollOffset = z;

      yaw_desired_angle = x;

      setZeroBool = false;

    }

}

void PID_reset() {

  pid_throttle_L_F = 1000;

  pid_throttle_L_B = 1000;

  pid_throttle_R_F = 1000;

  pid_throttle_R_B = 1000;

  pitch_PID = 0, roll_PID = 0, yaw_PID = 0;

  roll_error = 0, pitch_error = 0, yaw_error = 0;

  roll_previous_error = 0, pitch_previous_error = 0, yaw_previous_error = 0;

  

  roll_pid_p = 0, pitch_pid_p = 0;

  roll_pid_i = 0, pitch_pid_i = 0;

  roll_pid_d = 0, pitch_pid_d = 0;

  pitch_pid_p = 0, pitch_pid_p = 0;

  pitch_pid_i = 0, pitch_pid_i = 0;

  pitch_pid_d = 0, pitch_pid_d = 0;

  yaw_pid_p = 0, yaw_pid_p = 0;

  yaw_pid_i = 0, yaw_pid_i = 0;

  yaw_pid_d = 0, yaw_pid_d = 0;

}
18

well bascially there are two ways to finish a project successfully:

way 1: doing 10.000 quick guessings without really understanding your code.
Each guessing will take 6 minutes. Project finishing-time 60000 minutes = 1000 hours.

way 2:

  • learning programming 100 hours
  • reading manuals carefully to really understand them, doing all the steps that are shown in the manual 20 hours
  • making pre-tests with small parts of the program 200 hours
  • adding one small part to your main-code and start testing 2 hours per part (all in all 40 parts = 80 hours)

in summary 400 hours.

I have no idea how much time you worked on your project. The numbers are just example-numbers to show a basic principle.

You are free to choose which way makes more fun to you
best regards Stefan

edit:

I did a content compare of your two sketches.
The codes are about 500 lines long. You changed estimated about 20% of the code.
If you changed so much without a single test inbetween this not-testing is a almost guarantee to make your code mal-functionable

grafik
grafik1422×1046 197 KB

19

Hey! I have been working with this project for about a year. I have had many different versions of the drone, with both different IMUs and different flight models, both ACRO and AUTO-LEVEL. This is my latest version and I did some changes to "clean it up" and make it more logical and estetical looking, I also added in the beginning two flight modes via 2 booleans but that is removed, then I flight tested the same time as I did my first recording shown in this thread and I crashed it "not so bad" but my power splitter got plugged out, once I fixed that I noticed some errrors in my code witch I removed to make it back to the same version I had when it flew successfully previously, but the ESCs started beeping and I came to the conclusion that they went into programming mode. The ESCs enter programming mode when I execute a 2000 milisecond pulse on startup to the ESCs. Witch I dont do, thats why I dont know how that even happens, you program the ESCs with varying the throttle between 1000ms to 2000ms pulses. I know my entire program with my eyes closed, I have written it myself every line and every letter. But clearly I have missed something. I had an earlier version of the other code that I submitted and just thought "why not test if this still works" and the ESCs booted perfecly. Thats when I came to the conclusion that there is something in my code missig/is wrong. Thanks for the support! Sorry for the long message :smiley:

20

This message not too long. A complex problem needs a detailed description. You provided a lot of very important information with this last poting. And it could still be more.

You should apologise for not posting this message as the first message.

What Kind of measuring equipement do you have?
Do you have a storage oscilloscope?
Do you have a digital mutlimeter which can also measure frequency?
Do you have a 8 Channel logic analyser?
Do you happen to have a spare microcontroller that you could use to examine the signal-output that goes out to the ESCs?

Do you know what a voltage-follower is and how to use it to "sniff" a signal without interfering with the signal through the additional connection?

2000 milliseconds would be 2 full seconds.
2 seconds high than low? Well this could happen with all kinds of coding.
It might be that even beeing not yet configured as output but as input,
that, on startup a HIGH-signal is seen by the ESC.

Is this correct? or do you mean 2000 micro-seconds?

What exact type of microcontroller are you using? Do you have pull-down-resistors on the out-put pins.

Did you really check if they really entered programming-mode. Did you read the manual and did you confirm beep for beep that the beeps are the exact same sequence as described in the manual if the ESC enters programming mode?

The other method would be to go back to a version that you archived and that you know for working good. And start adding modifying small changes and re-test again until the bug occurs.

best regards Stefan