Drone PID output

Projects Programming
1

Hi, I'm working on my DIY arduino drone and recently ran into some problems. The thing is that I keep getting unexpected output from the PID and I don't know what is causing it. My guess is that it has something to do with the acelometer output and the PID. My 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;
//pid
double pitchKp = 1, pitchKi = 1 , pitchKd = 1;
double rollKp = 1, rollKi = 1, rollKd = 1;
double yawKp = 1, yawKi = 1, yawKd = 1;
double altitudeKp = 1, altitudeKi = 1, altitudeKd = 1;
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 interval = 20;
//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, unsigned long& previousMillis, unsigned long& interval);
void ControlMotors();

void setup() {
  InitInput();
}

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

FUNCTIONS:

double CalculatePID(double Input, double Kp, double Ki,  double Kd, double Setpoint, double LastError, double Integral, unsigned long& previousMillis, unsigned long& interval) {
  unsigned long currentMillis = millis();
  double timer = 0;
  timer = double(currentMillis - previousMillis) / 1000;
  if (timer <= interval) {
    double error = Setpoint - Input;
    Integral += error * timer;
    double derivative = (error - LastError) / timer;
    double Output = Kp * error + Ki * Integral + Kd * derivative;
    LastError = error;
    previousMillis = currentMillis;
    return Output;
  }
  return 0;
}

void ControlMotors() {
  sp1 = pitchOutput + altitudeOutput - yawOutput;
  sp2 = -rollOutput + altitudeOutput + yawOutput;
  sp3 = -pitchOutput + altitudeOutput - yawOutput;
  sp4 = 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);
  */
}

void GetRadio(double& yawSetpoint, double& rollSetpoint, double& pitchSetpoint, double& altitudeSetpoint, double& seaLevelPres, bool& camera) {
  if (radio.available())  {
    radio.read(&data, sizeof(data));
    altitudeSetpoint = map(data[0], 0, 1023, -50, 50);
    yawSetpoint = map(data[1], 0, 1023, -180, 180);
    rollSetpoint = map(data[2], 0, 1023, -45, 45);
    pitchSetpoint = map(data[3], 0, 1023, -45, 45);
    seaLevelPres = data[4];
    if ( seaLevelPres == 0) {
      seaLevelPres = 1013.25;
    }  camera = data[5];
    /*
      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);
    */
  }
}

void InitInput() {
  Serial.begin(115200);
  radio.begin();
  Wire.begin();
  bme.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  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);
}

void ReadSensors(double& pitchInput, double& rollInput, double& yawInput, double& altitudeInput) {
  //accelometer
  int x, y, z;
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true);
  x = Wire.read() << 8 | Wire.read();
  y = Wire.read() << 8 | Wire.read();
  z = Wire.read() << 8 | Wire.read();

  double x_Buff = float(x);
  double y_Buff = float(y);
  double z_Buff = float(z);

  rollInput = RAD_TO_DEG * atan2(y_Buff, z_Buff);
  pitchInput = RAD_TO_DEG * atan2(x_Buff, z_Buff);
  yawInput = RAD_TO_DEG * atan2(x_Buff, y_Buff);


  //altitudemeter
  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;
  altitudeInput = ((pow((seaLevelPres / pres), 1 / 5.257) - 1) * (temp + 273.15)) / 0.0065;
  bool setAltitudeZero = true;
  double firstAltitudeInput;
  if (setAltitudeZero == true) {
    firstAltitudeInput = altitudeInput;
    setAltitudeZero = false;
  }
  altitudeInput -=  firstAltitudeInput;
  altitudeInput = constrain(altitudeInput, -50, 50);

  Serial.print("Pitch");
  Serial.println(pitchInput);
  Serial.print("Roll");
  Serial.println(rollInput);
  Serial.print("Yaw");
  Serial.println(yawInput);
  Serial.print("Altitude");
  Serial.println(altitudeInput);

}
2

All of your PID loops are sharing one previousMillis value?

On a PID the time is usually used to set a sampling interval. Your code seems to be using it to detect when it has been more than 20 second since the function was last called. That make no sense to me.

The 'interval' argument seems to be a constant. No need to pass by reference. No need to pass it as an argument at all.

3

Yes

It was intended so that the pidCalculate() function would run every 20 seconds.

Yes, it is, I'll fix it

4

That seems way too infrequent. Did you mean to say every 20 milliseconds?

Even that may be too infrequent.

Say more about the vehicle and the parts you expect to fly and the parts that will control it.

a7

5

image
image1920×2560 799 KB

The drone receives data from the radio controller, stores it and then uses it as PID setpoints. The angles are measured with the MPU6050 accelerometer(later I will use the gyroscope with accelerometer as IMU). The altitude is calculated from a barometric formula with the current sea level pressure, which is set from the radio controller and displayed on the controller's oled display, and the BMP280 temperature and pressure values. Once the PIDcalculate loops are complete, the next step is to use their output and set the values on the pwm pins.

6

You can't use one 'previousMillis' value for four independent timers. You could have them independent by each having their own 'previousMillis' variable or you could have one timer and call all four PID functions when the interval expires:

void CalculatePIDs()
{
  unsigned long 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);
  }
}
7

So if I use the piece of code you posted in my loop function, how should the calculatePID function change?

8

Take out the timing since it will be called every 10 milliseconds:

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;
}
9

Thanks, code started calculating PIDs and the output from them in serial monitor looks like this:

28843.00
-9681.00
293122.00
-6799220.50
28873.00
-9691.00
293419.81
-6806247.00
28903.00
-9701.00
293742.18
-6813274.00
28933.00
-9711.00
294017.59
-6820300.50
28963.00
-9721.00
294309.09
-6827327.50
28993.00
-9731.00
294621.18
-6834354.50
29023.00
-9741.00
294919.90
-6841381.50
29053.00
-9751.00
295231.09
-6848408.50
29083.00
-9761.00
295552.09
-6855435.50
29113.00
-9771.00
295849.81
-6862462.50
29143.00

But there is still a problem, from some reason sp1-4 values are just 0.00

10

Those Output values look a little wild for values you want to be in the 0 to 179 range.

What are the values BEFORE the 'constrain()' function? Your constrain calls will force any negative value to 0.

11

The output before constrain is -316731.00 -389287.90 -319391.96 -396174.03 -321873.96 -395511.00 -324495.00 -402517.00 -327017.00 -401755.09 -329598.03 -408838.96 -332160.03 -407977.00

So could fabs() function help?

12

Values after using fabs() function are surprisingly 178.00

13

Doesn't surprise me. If you make all the huge negative numbers into huge positive numbers, they will hit the other end of your constrain() range.

I would worry about how your PID calculations are coming up with such large values. You have obviously not tuned the PID constants so maybe they are way off.

Are you getting the expected Setpoints from your radio?
Are you getting reasonable Inputs from your sensors?
Does each step of your PID get the expected results?

14

Those values are far outside of the 0-178 limits on the devices you are trying to control.

Messing with the interval between calculations will screw up the how the Ki and Kd tuning works, and Ki with this simplistic calculation can blow up the integral terribly.

How did you come up with your (Kp=1,Ki=1,Kd=1) tuning parameters? With 10ms between calculations, just a 1° pitch error with Ki = 1.0 will grow the integral contribution by 100 output units per second, which could easily push you out of the 0-178 control range.

I'd try (Kp=1,Ki=0,Kd=0) and it should calm things quite a bit.

Since you don't scale the tuning parameters with time in your calculation code, you'd need to scale the raw parameters. For example, if your Ki=1 is supposed to mean 1°out/(°error*second) and you calculate every 10ms, then you need to set your Ki =0.01 to make it work like 1°out/(1°error*second).

15

Me neither..

Yes, I do.

The outputs are Pitch-3.00 Roll1.00 Yaw-32.00 Altitude0.00 Altitude Offset100.00

I'll try to find out.

16

After changing Kp=1,Ki=0,Kd=0 I got the values of my sensors from the PID outputs and they are not responding to the controller data.

17

If you move the Ki multiplication from the sum into the integral like this:

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

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

    return Output;
}

It is mathematically equivalent, but makes it easier to interpret the Integral as how much it contributes to the output in output units.

Then you could better see how the integral might be blowing up the output to numbers like "28873.00"

18

I displayed radio data in loop and their values are 0.00 except the sea level pressure

sea level pressure997.00
altitudeSetpoint0.00
pitchSetpoint0.00
rollSetpoint0.00
yawSetpoint0.00
1 Like
19

The Output should be Setpoint - Input.

What values of Input are you getting?
What values of Output do you get?

20

Are you doing fabs()? or consrtaining to (0-179)? What's your current code?

Since with (1,0,0), Output = 1*(Setpoint-Input) +0 +0. the only way to get Output=Input would be if Setpoint = 2*Input (or maybe fabs())