Drone PID output

crownous · March 9, 2023, 6:19pm

My CalculatePID() and ControlMotors() functions are now as follows:

double CalculatePID(double Input, double Kp, double Ki,  double Kd, double Setpoint, double &LastError, double &Integral) {

  double error = Setpoint - Input;
  Integral += error * interval;
  double derivative = (error - LastError) / interval;
  double Output = Kp * error + Ki * Integral + Kd * derivative;
  LastError = error;

  return Output;
}

void ControlMotors() {
  sp1 = fabs(pitchOutput + altitudeOutput - yawOutput);
  sp2 = fabs(-rollOutput + altitudeOutput + yawOutput);
  sp3 = fabs(-pitchOutput + altitudeOutput - yawOutput);
  sp4 = fabs(rollOutput + altitudeOutput + yawOutput);

  sp1 = constrain(sp1, 0, 178);
  sp2 = constrain(sp2, 0, 178);
  sp3 = constrain(sp3, 0, 178);
  sp4 = constrain(sp4, 0, 178);
  /*
  Serial.println(sp1);
  Serial.println(sp2);
  Serial.println(sp3);
  Serial.println(sp4);
  /*
    analogWrite(motor1, sp1);
    analogWrite(motor2, sp2);
    analogWrite(motor3, sp3);
    analogWrite(motor4, sp4);
  */
}

crownous · March 9, 2023, 6:22pm

void loop() {
  GetRadio(pitchInput, rollInput, yawInput, altitudeInput, seaLevelPres, camera);
  ReadSensors(pitchInput, rollInput, yawInput, altitudeInput);
  currentMillis = millis();
  if (currentMillis - previousMillis > interval)
  {
    previousMillis = currentMillis;
    pitchOutput = CalculatePID(pitchInput, pitchKp, pitchKi, pitchKd, pitchSetpoint, pitchLastError, pitchIntegral);
    rollOutput = CalculatePID(rollInput, rollKp, rollKi, rollKd, rollSetpoint, rollLastError, rollIntegral);
    yawOutput = CalculatePID(yawInput, yawKp, yawKi, yawKd, yawSetpoint, yawLastError, yawIntegral);
    altitudeOutput = CalculatePID(altitudeInput, altitudeKp, altitudeKi, altitudeKd, altitudeSetpoint, altitudeLastError, altitudeIntegral);
  }
  /*
      Serial.println(pitchOutput);
      Serial.println(rollOutput);
      Serial.println(yawOutput);
      Serial.println(altitudeOutput);
  */

**  Serial.print("Altitude sea level pressure");**
**  Serial.println(seaLevelPres);**
**  Serial.print("altitudeSetpoint");**
**  Serial.println(altitudeSetpoint);**
**  Serial.print("pitchSetpoint");**
**  Serial.println(pitchSetpoint);**
**  Serial.print("rollSetpoint");**
**  Serial.println(rollSetpoint);**
**  Serial.print("yawSetpoint");**
**  Serial.println(yawSetpoint);**

  ControlMotors();
}

Serial.print() shows 0.00 for all values from GetRadio() except Altitude sea level pressure, but in GetRadio() it shows normal potentiometers values

DaveX · March 9, 2023, 6:31pm

crownous:

void ControlMotors() {
  sp1 = fabs(pitchOutput + altitudeOutput - yawOutput);
  sp2 = fabs(-rollOutput + altitudeOutput + yawOutput);
  sp3 = fabs(-pitchOutput + altitudeOutput - yawOutput);
  sp4 = fabs(rollOutput + altitudeOutput + yawOutput);

I dislike these fabs() -- It seems like if there is zero altitudeOutput, a roll (or pitch or yaw) error shouldn't affect both sp2 and sp3 (or others) motors positively, they should bias.

Maybe setpoint isn't getting set?

crownous · March 9, 2023, 6:35pm

Okay, I removed it.

Yes, it isn't. I don't see why.

johnwasser · March 9, 2023, 6:53pm

This is why your Setpoints aren't getting set. You are setting the Inputs twice. Pass the Setpoint variables to GetRadio().

crownous · March 9, 2023, 6:58pm

Oh, I didn't notice. Thanks

crownous · March 10, 2023, 9:30am

I have a question, must the sum of PID constants ever be 1?

DaveX · March 10, 2023, 1:17pm

They do not have to sum to one. They convert the units of the input to units of the output.

If you have degrees of roll error as an input, and microseconds of throttle servo control as output, then there's no reason why kP=50us/° couldn't be right. It really depends on the system and the units of input and output. And with the way you are calculating it, the deltaT between calculations is important too -- If you calculate 1 time/second, then you need a 10X larger Ki and 1/10 Kd than if you sample 10 times/sec -- Improving the Beginner’s PID – Sample Time « Project Blog

If you need your PID to use +/-10° of roll to cause full deflection one way or the other on your 0-178 output, then Kp=17.8 could be a plausible setting.

UKHeliBob · March 12, 2023, 12:36pm

Your other topic on the same subject deleted.

Please do not duplicate your questions as doing so wastes the time and effort of the volunteers trying to help you as they are then answering the same thing in different places.

Please create one topic only for your question and choose the forum category carefully. If you have multiple questions about the same project then please ask your questions in the one topic as the answers to one question provide useful context for the others, and also you won’t have to keep explaining your project repeatedly.

Repeated duplicate posting could result in a temporary or permanent ban from the forum.

Could you take a few moments to Learn How To Use The Forum

It will help you get the best out of the forum in the future.

Thank you.

crownous · March 13, 2023, 3:51pm

So I've tweaked the data processing from the MPU6050 and BMP280 and it looks like it's ready for the first try. I haven't tuned the PID yet because I ran into a problem. When I uncomment analogWrite() in the ControlMotors() function, the program stops responding after a bit of movement on the radio controller and also stops outputting values on the serial monitor. Any ideas?
My current code:

#include<Wire.h>
#include <SPI.h>
#include <RF24.h>
#include <nRF24L01.h>
#include <BME280I2C.h>

#define motor1 3 // left front - dopredu, dozadu kolem Z
#define motor2 5 // right front - doleva, doprava kolem Z
#define motor3 6 // right back - doleva, doprava kolem Z
#define motor4 9 // left back - dopredu, dozadu kolem Z

BME280I2C bme;
const int MPU_addr = 0x68;
RF24 radio(2, 10);
const byte address[10] = "sjSEmD3xK";

double seaLevelPres = 1013.25;
bool camera = false;

// radio input
int data[10];
//sensors
double pitchInput, rollInput, yawInput, altitudeInput;
int counter = 0;
//IMU
double xAcc, yAcc, zAcc, xGyro, yGyro, zGyro;
double currentGyroMillis, previousGyroMillis, deltaGyroTime;
double pitchInputAcc, rollInputAcc, yawInputAcc;
double pitchInputGyro, rollInputGyro, yawInputGyro;
double rollGyroOffset, pitchGyroOffset, yawGyroOffset, rollAccOffset, pitchAccOffset, yawAccOffset;
//kalman's altitude filter
double altitudeInputOffset;
const double QTemp = 0.91; // Procesní šum
const float RTemp = 0.125;// Měřící šum
float xTemp = 0;  // Stavová proměnná
float pTemp = 1;  // Kovariance stavu
float kTemp;// Kalmanovo zesílení
const float QPres = 0.043; // Procesní šum
const float RPres = 0.007;// Měřící šum
float xPres = 0;  // Stavová proměnná
float pPres = 1;  // Kovariance stavu
float kPres;      // Kalmanovo zesílení
//pid
double pitchKp = 2.833, pitchKi = 0 , pitchKd = 0;
double rollKp = 2.833, rollKi = 0, rollKd = 0;
double yawKp = 0.7083, yawKi = 0, yawKd = 0;
double altitudeKp = 2.55, altitudeKi = 0, altitudeKd = 0;
double pitchSetpoint = 0, rollSetpoint = 0, yawSetpoint = 0, altitudeSetpoint = 0;  //radio transmitter values
double pitchLastError, rollLastError, yawLastError, altitudeLastError;
double pitchIntegral, rollIntegral, yawIntegral, altitudeIntegral;
double pitchOutput, rollOutput, yawOutput, altitudeOutput;
unsigned long previousMillis;
unsigned long  currentMillis;
const unsigned long interval = 10;
//motor speeds
double sp1 = 0, sp2 = 0, sp3 = 0, sp4 = 0;
//functions initialization
void InitInput();
void GetRadio(double& yawSetpoint, double& rollSetpoint, double& pitchSetpoint, double& altitudeSetpoint, double& seaLevelPres, bool& camera);
void ReadSensors(double& pitchInput, double& rollInput, double& yawInput, double& altitudeInput);
double CalculatePID(double Input, double Kp, double Ki,  double Kd, double Setpoint, double& LastError, double& Integral);
void ControlMotors();

void setup() {
  InitInput();
}

void loop() {
  GetRadio(yawSetpoint, rollSetpoint, pitchSetpoint, altitudeSetpoint, seaLevelPres, camera);
  ReadSensors(pitchInput, rollInput, yawInput, altitudeInput);
  currentMillis = millis();
  if (currentMillis - previousMillis > interval)
  {
    previousMillis = currentMillis;
    pitchOutput = CalculatePID(pitchInput, pitchKp, pitchKi, pitchKd, pitchSetpoint, pitchLastError, pitchIntegral);
    rollOutput = CalculatePID(rollInput, rollKp, rollKi, rollKd, rollSetpoint, rollLastError, rollIntegral);
    yawOutput = CalculatePID(yawInput, yawKp, yawKi, yawKd, yawSetpoint, yawLastError, yawIntegral);
    altitudeOutput = CalculatePID(altitudeInput, altitudeKp, altitudeKi, altitudeKd, altitudeSetpoint, altitudeLastError, altitudeIntegral);
  }

  /*Serial.println(pitchOutput);
    Serial.println(rollOutput);
    Serial.println(yawOutput);
    Serial.println(altitudeOutput);
    Serial.print("Altitude sea level pressure");
    Serial.println(seaLevelPres);
    Serial.print("altitudeSetpoint");
    Serial.println(altitudeSetpoint);
    Serial.print("pitchSetpoint");
    Serial.println(pitchSetpoint);
    Serial.print("rollSetpoint");
    Serial.println(rollSetpoint);
    Serial.print("yawSetpoint");
    Serial.println(yawSetpoint);
    Serial.print("Pitch");
    Serial.println(pitchInput);
    Serial.print("Roll");
    Serial.println(rollInput);
    Serial.print("Yaw");
    Serial.println(yawInput);
    Serial.print("Altitude");
    Serial.println(altitudeInput);
    Serial.print("Altitude Offset");
    Serial.println(altitudeInputOffset);*/
  ControlMotors();
}
double CalculatePID(double Input, double Kp, double Ki,  double Kd, double Setpoint, double &LastError, double &Integral) {

  double error = Setpoint - Input;
  //Serial.println(error);
  Integral += error * interval;
  double derivative = (error - LastError) / interval;
  double Output = Kp * error + Ki * Integral + Kd * derivative;
  LastError = error;

  return Output;
}
void ControlMotors() {
  sp1 = pitchOutput + altitudeOutput - yawOutput;
  sp2 = -rollOutput + altitudeOutput + yawOutput;
  sp3 = -pitchOutput + altitudeOutput + yawOutput;
  sp4 = rollOutput + altitudeOutput - yawOutput;

  sp1 = constrain(sp1, 0, 255);
  sp2 = constrain(sp2, 0, 255);
  sp3 = constrain(sp3, 0, 255);
  sp4 = constrain(sp4, 0, 255);

  Serial.print("left front");
  Serial.println(sp1);
  Serial.print("ritght front");
  Serial.println(sp2);
  Serial.print("right backt");
  Serial.println(sp3);
  Serial.print("left back");
  Serial.println(sp4); /*
  analogWrite(motor1, sp1);
  analogWrite(motor2, sp2);
  analogWrite(motor3, sp3);
  analogWrite(motor4, sp4);*/
}
void GetRadio(double& yawSetpoint, double& rollSetpoint, double& pitchSetpoint, double& altitudeSetpoint, double& seaLevelPres, bool& camera) {
  if (radio.available())  {
    radio.read(&data, sizeof(data));
    yawSetpoint = data[0];
    altitudeSetpoint = data[1];
    rollSetpoint = data[2];
    pitchSetpoint = data[3];
    seaLevelPres = data[4];
    if ( seaLevelPres == 0) {
      seaLevelPres = 1013.25;
    }
    camera = data[5];
  }
}
void InitInput() {
  Serial.begin(115200);
  radio.begin();
  Wire.begin();
  bme.begin();

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  //gyro config
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x1B);
  Wire.write(0x10);
  Wire.endTransmission(true);
  //accelometer config
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x1C);
  Wire.write(0x10);
  Wire.endTransmission(true);
  radio.openReadingPipe(0, address);
  radio.setPALevel(RF24_PA_MIN);
  radio.startListening();
  pinMode(motor1, OUTPUT);
  pinMode(motor2, OUTPUT);
  pinMode(motor3, OUTPUT);
  pinMode(motor4, OUTPUT);
  currentGyroMillis = millis();
  if (rollAccOffset == 0) {
    for (int i = 0; i < 200; i++) {

      Wire.beginTransmission(MPU_addr);
      Wire.write(0x3B);
      Wire.endTransmission(false);
      Wire.requestFrom(MPU_addr, 6, true);

      xAcc = (Wire.read() << 8 | Wire.read()) / 4096.0 ;
      yAcc = (Wire.read() << 8 | Wire.read()) / 4096.0 ;
      zAcc = (Wire.read() << 8 | Wire.read()) / 4096.0 ;

      pitchAccOffset += (atan((yAcc) / sqrt(pow((xAcc), 2) + pow((zAcc), 2))) * RAD_TO_DEG);
      rollAccOffset += (atan(-1 * (xAcc) / sqrt(pow((yAcc), 2) + pow((zAcc), 2))) * RAD_TO_DEG);

      if (i == 199) {
        rollAccOffset = rollAccOffset /  200;
        pitchAccOffset = pitchAccOffset / 200;
      }
    }
  }
  if (rollGyroOffset == 0) {
    for (int i = 0; i < 200; i++) {

      Wire.beginTransmission(MPU_addr);
      Wire.write(0x43);
      Wire.endTransmission(false);
      Wire.requestFrom(MPU_addr, 6, true);

      xGyro = Wire.read() << 8 | Wire.read();
      yGyro = Wire.read() << 8 | Wire.read();
      zGyro = Wire.read() << 8 | Wire.read();

      rollGyroOffset += yGyro / 32.8 ;
      pitchGyroOffset += xGyro / 32.8;
      yawGyroOffset += zGyro / 32.8;
      if (i == 199) {
        rollGyroOffset = rollGyroOffset / 200;
        pitchGyroOffset = pitchGyroOffset / 200;
        yawGyroOffset = yawGyroOffset / 200;
      }
    }
  }

}
void ReadSensors(double& pitchInput, double& rollInput, double& yawInput, double& altitudeInput) {
  //IMU

  previousGyroMillis = currentGyroMillis;
  currentGyroMillis = millis();
  deltaGyroTime = (currentGyroMillis - previousGyroMillis) / 1000;

  //gyro

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 6, true);
  xGyro = Wire.read() << 8 | Wire.read();
  yGyro = Wire.read() << 8 | Wire.read();
  zGyro = Wire.read() << 8 | Wire.read();


  xGyro = (xGyro / 32.8) - pitchGyroOffset;
  yGyro = (yGyro / 32.8) - rollGyroOffset;
  zGyro = (zGyro / 32.8) - yawGyroOffset;

  pitchInputGyro = xGyro * deltaGyroTime ;
  rollInputGyro = yGyro * deltaGyroTime;
  yawInputGyro = zGyro * deltaGyroTime;

  //accelometer

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 6, true);
  xAcc = (Wire.read() << 8 | Wire.read()) / 4096.0 ;
  yAcc = (Wire.read() << 8 | Wire.read()) / 4096.0 ;
  zAcc = (Wire.read() << 8 | Wire.read()) / 4096.0 ;

  pitchInputAcc = (atan((yAcc) / sqrt(pow((xAcc), 2) + pow((zAcc), 2))) * RAD_TO_DEG) - pitchAccOffset;
  rollInputAcc = (atan(-1 * (xAcc) / sqrt(pow((yAcc), 2) + pow((zAcc), 2))) * RAD_TO_DEG) - rollAccOffset;

  //complementary filter
  rollInput = 0.98 * (rollInput + rollInputGyro) + 0.02 * (rollInputAcc);
  pitchInput = 0.98 * (pitchInput + pitchInputGyro) + 0.02 * (pitchInputAcc);
  yawInputAcc = atan2((sin(rollInput) * cos(pitchInput) * xAcc + sin(pitchInput) * yAcc + cos(rollInput) * cos(pitchInput) * zAcc), sqrt(pow(sin(rollInput) * sin(pitchInput) * xAcc - cos(rollInput) * sin(pitchInput) * zAcc, 2) + pow(cos(pitchInput) * xAcc, 2))) - 1;
  yawInput = 0.98 * (yawInput + yawInputGyro) + 0.02 * (yawInputAcc);

  //altitudemeter

  //bmp280
  float temp(NAN), hum(NAN), pres(NAN);
  BME280::TempUnit tempUnit(BME280::TempUnit_Celsius);
  BME280::PresUnit presUnit(BME280::PresUnit_Pa);
  bme.read(pres, temp, hum, tempUnit, presUnit);
  pres = pres / 100;
  counter += 1;
  //kalmans filter
  float xPredTemp = xTemp;
  float pPredTemp = pTemp + QTemp;
  float xPredPres = xPres;
  float pPredPres = pPres + QPres;
  // Výpočet Kalmanova zesílení
  kTemp = pPredTemp  / (pPredTemp  + RTemp);
  kPres = pPredPres  / (pPredPres  + RPres);
  // Korekce stavu
  xTemp = xPredTemp  + kTemp * (temp - xPredTemp );
  xPres = xPredPres  + kPres * (pres - xPredPres );
  // Aktualizace kovariance stavu
  pTemp = (1 - kTemp) * pPredTemp ;
  pPres = (1 - kPres) * pPredPres ;
  altitudeInput = (((pow((1013.15 /  xPres), 1 / 5.257) - 1) * (xTemp  + 273.15)) / 0.0065);
  if (altitudeInputOffset == 0 && counter == 50) {
    altitudeInputOffset = altitudeInput;
  }
  altitudeInput -= altitudeInputOffset;
}

johnwasser · March 13, 2023, 4:01pm

Sounds like your motors are drawing enough power or causing enough electrical noise to cause your Arduino to crash.

crownous · March 13, 2023, 4:08pm

Is there any way to prevent this? Maybe add a capacitor on motors?

crownous · March 13, 2023, 4:35pm

I added 100nF cap on every motor, but it didn't help.

DaveX · March 13, 2023, 4:47pm

Schematic?

Caps help with noise. Power draw against an inadequate supply is a different problem. It could be either. Can you measure voltage or noise at the Arduino?

Maybe try uncommenting only one motor?

crownous · March 13, 2023, 4:49pm

I took a video of what's happening:
Google drive video

crownous · March 13, 2023, 4:51pm

I messured the voltage, and it's fine. How to messure the noise? I suppose with the use of an oscilloscope?

crownous · March 13, 2023, 4:56pm

Schematic:

crownous · March 13, 2023, 5:22pm

I forgot to mention caps on stabilizer.
circuit2

crownous · March 13, 2023, 5:41pm

I tested circuit just with stabilizer, MOSFET and motor on arduino UNO and it works.

crownous · March 13, 2023, 5:54pm

I GOT IT arduino hadn't enough power, so I put into it 7.2V from battery and stabilized only motors. Thanks for your time

Topic		Replies	Views
Multiwii (PID). Quad X FC (Arduino + MPU6050) Software	17	479	December 30, 2025
Two wheel balncing robot with adxl345 General Guidance	68	1096	March 22, 2026
PID autotuning code, fuzzy logic, Datalogger system heater chauffant 4-20mA Réalisations et Projets Finis	13	645	September 3, 2026
Quadcopter PID tuning self balancing problems HELP Programming	16	698	October 5, 2025
PID control of DC motor Motors, Mechanics, Power and CNC	26	2218	January 10, 2025

Drone PID output

Related topics