Faulty PID Loop

Projects Programming
1

Hi Everyone,

Picked up. my hobby project once again after hitting a bit of a brick wall.

The project is an EDF powered rocket. It will be controlled by gimbaling the motor as opposed to thrust vectoring veins that are often used in similar projects. The brains of the project use an MPU9250 and a BMP180 for sensory readings.

Because I am able to actuate in both axises (axies?), at the same time to excerpt a diagonal thrust or thrust to stabilise in any direction, I'm hoping that Eulers angles won't need to be considered. For the time being stabilisation is the only goal, not navigation.

The problem When testing the gimbaling by applying a Proportional value to the PID loop the actuation only works along one axis. The other axis spits out the odd variation here or there but is in no way responsive to the movement. The project uses two PID loops which both pull data from the MPU9250 to generate a suitable output. See below code for the control system


#include "MPU9250.h"
#include <Arduino.h>
#include <ESP32Servo.h>
#include <SFE_BMP180.h>
#include <esp_now.h>
#include <WiFi.h>
#include <PID_v1.h>
#include "Filter.h"
#include "math.h"

//GlobalVariables  --------------------------------------
//Set Min/Max values for PID Output
int MAXANGLE = 180;
int MINANGLE = 0;

//Set Servo Pins
int XPin = 16;
int YPin = 17;
int ESCPin = 4;

//PID Tuning Parameters
double Kp=1;
double Ki=0;
double Kd=0;

float RollOutput = 0;
float PitchOutput = 0;

float RawPitch = 0;
float RawRoll = 0;
float RawAltitude = 0;

float FilteredPitch = 0;
float FilteredRoll = 0;
float FilteredAltitude = 0;
//Create PID Variables
double SetpointPitch, InputPitch, OutputPitch;
double SetpointRoll, InputRoll, OutputRoll;

double PitchAVG = 0;
double RollAVG= 0;

float pitch = 0;
float roll = 0;

//Initialisers -----------------------------------------
//Create Servos
Servo PitchAxis;
Servo RollAxis;
Servo ESC;

//Create pressure sesor variables
SFE_BMP180 pressure;

//Create IMU Variable
MPU9250 mpu;

//Filter Variables
ExponentialFilter<long> FilterPitch(10, 0);
ExponentialFilter<long> FilterRoll(10, 0);
ExponentialFilter<long> FilterAltitude(10, 0);

double baseline;

PID PitchPID(&InputPitch, &OutputPitch, &SetpointPitch, Kp, Ki, Kd, DIRECT);
PID RollPID(&InputRoll, &OutputRoll, &SetpointRoll, Kp, Ki, Kd, DIRECT);
//--------------------------------------------------------
                   
//RX Data struct
typedef struct struct_message {
  int val;
} struct_message;
struct_message myData;

//Recieve ESP32 Data from controller
void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
  memcpy(&myData, incomingData, sizeof(myData));
}

//Pressure get funcion
double getPressure()
{
  char status;
  double T,P,p0,a;

  status = pressure.startTemperature();
  if (status != 0)
  {


    delay(status);

    status = pressure.getTemperature(T);
    if (status != 0)
    {

      status = pressure.startPressure(3);
      if (status != 0)
      {

        delay(status);

        status = pressure.getPressure(P,T);
        if (status != 0)
        {
          return(P);
        }
        else Serial.println("error retrieving pressure measurement\n");
      }
      else Serial.println("error starting pressure measurement\n");
    }
    else Serial.println("error retrieving temperature measurement\n");
  }
  else Serial.println("error starting temperature measurement\n");
}

void print_calibration() {
  Serial.println("< calibration parameters >");
  Serial.println("accel bias [g]: ");
  Serial.print(mpu.getAccBiasX() * 1000.f / (float)MPU9250::CALIB_ACCEL_SENSITIVITY);
  Serial.print(", ");
  Serial.print(mpu.getAccBiasY() * 1000.f / (float)MPU9250::CALIB_ACCEL_SENSITIVITY);
  Serial.print(", ");
  Serial.print(mpu.getAccBiasZ() * 1000.f / (float)MPU9250::CALIB_ACCEL_SENSITIVITY);
  Serial.println();
  Serial.println("gyro bias [deg/s]: ");
  Serial.print(mpu.getGyroBiasX() / (float)MPU9250::CALIB_GYRO_SENSITIVITY);
  Serial.print(", ");
  Serial.print(mpu.getGyroBiasY() / (float)MPU9250::CALIB_GYRO_SENSITIVITY);
  Serial.print(", ");
  Serial.print(mpu.getGyroBiasZ() / (float)MPU9250::CALIB_GYRO_SENSITIVITY);
  Serial.println();


}

void setup() {

  Serial.begin(115200);
  Wire.begin();
  delay(2000);

  WiFi.mode(WIFI_STA);

  // Init ESP-NOW
  if (esp_now_init() != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }
  
  // Once ESPNow is successfully Init, we will register for recv CB to
  // get recv packer info
  esp_now_register_recv_cb(OnDataRecv);

  if (!mpu.setup(0x68)) {  // change to your own address
  while (1) {
    Serial.println("MPU connection failed. Please check your connection with `connection_check` example.");
    delay(5000);
    }
    }
  if (pressure.begin()){
    Serial.println("BMP180 init success");
  }else{
    Serial.println("BMP180 init fail (disconnected?)\n\n");
    while(1); // Pause forever.
    }


  // calibrate anytime you want to
  Serial.println("Accel Gyro calibration will start in 5sec.");
  Serial.println("Please leave the device still on the flat plane.");
  mpu.verbose(true);
  delay(5000);
  mpu.calibrateAccelGyro();

  print_calibration();
  mpu.verbose(false);

  ESP32PWM::allocateTimer(0);
  ESP32PWM::allocateTimer(1);
  ESP32PWM::allocateTimer(2);
  ESP32PWM::allocateTimer(3);
  PitchAxis.setPeriodHertz(50);
  PitchAxis.attach(XPin, 500, 2400); 
  RollAxis.setPeriodHertz(50);
  RollAxis.attach(YPin, 500, 2400); 
  ESC.setPeriodHertz(50);
  ESC.attach(ESCPin, 1000, 2000);

  baseline = getPressure();
  
  Serial.print("baseline pressure: ");
  Serial.print(baseline);
  Serial.println(" mb");  

  PitchPID.SetMode(AUTOMATIC);
  RollPID.SetMode(AUTOMATIC);

  SetpointPitch = 0;
  SetpointRoll = 0;

  PitchAxis.write(90);
  RollAxis.write(90);

  PitchPID.SetOutputLimits(-180, 180);
  RollPID.SetOutputLimits(-180, 180);
}

void loop() {
  if(mpu.update()){

    float ax = mpu.getAccX() + 1;
    float ay = mpu.getAccY();
    float az = mpu.getAccZ() - 1;

    pitch = 180 * atan (az/sqrt(ay*ay + ax*ax))/M_PI;
    roll = 180 * atan (ay/sqrt(az*az + ax*ax))/M_PI;

    double P;
    P = getPressure();

    RawPitch = pitch;
    RawRoll = roll;
    RawAltitude = pressure.altitude(P, baseline);

    FilterPitch.Filter(RawPitch);
    FilterRoll.Filter(RawRoll);
    FilterAltitude.Filter(RawAltitude);

    FilteredPitch = FilterPitch.Current();
    FilteredRoll = FilterRoll.Current();
    FilteredAltitude = FilterAltitude.Current();

    InputPitch = RawPitch;
    InputRoll = RawRoll;

    PitchPID.Compute();
    RollPID.Compute();

    PitchOutput = OutputPitch + 90;
    RollOutput = OutputPitch + 90;

    if (PitchOutput >= MAXANGLE){
      PitchOutput = MAXANGLE;
    }
    if (PitchOutput <= MINANGLE){
      PitchOutput = MINANGLE;
    }
    if (RollOutput >= MAXANGLE){
      RollOutput = MAXANGLE;
    }
    if (RollOutput <= MINANGLE){
      RollOutput = MINANGLE;
    }

    PitchAxis.write(FilteredPitch + 90);
    RollAxis.write(FilteredRoll + 90);

    ESC.write(myData.val);

    Serial.print("Pitch:");
    Serial.print(FilteredPitch);
    Serial.print("  ,  ");
    Serial.print("PitchOutput:");
    Serial.print(PitchOutput);
    Serial.print("  ,  ");
    Serial.print("RawPitchOutput:");
    Serial.print(OutputPitch);
    Serial.print("  ,  ");
    Serial.print("Roll:");
    Serial.print(FilteredRoll);
    Serial.print("  ,  ");
    Serial.print("RollOutput:");
    Serial.print(RollOutput);
    Serial.print("  ,  ");
    Serial.print("RawRollOutput:");
    Serial.println(OutputRoll);



  }
}

I'm almost certain it's something blindingly obvious but i can't seem to pick out what's gone wrong...

Also any tips regarding filtering, I.E the best types of filters or how and when to apply the filters, would be greatly appreciated. I have come across quaternion filters and kalman filters which were included in the library I'm using, but this slow down the response time and readings of the IMU far too much so I ended up disabling it and using the exponential filter. So far it seems to do a good enough job of suppressing vibrations while still being sensitive enough to react when necessary.

Thanks in advance

2

For informed help, you need to post the theory of operation, which I suspect you have not worked out yet for yourself.

To start with the obvious, one of these two equations is wrong.

    pitch = 180 * atan (az/sqrt(ay*ay + ax*ax))/M_PI;
    roll = 180 * atan (ay/sqrt(az*az + ax*ax))/M_PI;

Pitch and roll are not independent, unless you work with very tiny angular increments, in which case using trigonometry is just a waste of computing power and time.

Recall that tan(theta) ~= theta if theta is small.

3

Nope...unfortunately I have no such formal skill set or ability in this arena. That being said, looks like I have a new topic to read up on! I have always found that theory vastly overcomplicates things as compared to application, that certainly isn't the case with more complex topics!

Apologies I should've mentioned. This is intentionally altered to adjust for the orientation of the IMU. The data that the IMU is giving back appears to be accurate. I've tested the device by placing it 90 degrees from it's original position in both axis and the readings reflected the movement accurately.

I understand they aren't independent of one another, the gimbaling of the motors won't be either, but will respond to both simultaneously for stabilisation. I do believe most gimballed model rockets operate largely on this same method so I'm confident (possible wrongly so) that this should work...

Fundamentally the PID loop should be responsive to the data that it's being feed, for some reason that doesn't seem to be the case with the roll but works ok for the pitch. Even just for basic stabilisation.

4

It looks like your output limits are all screwed up.
You set them to -180 to +180, then add 90 to make then -90 to +270, then truncate them to 0 to 180!

How far can you reasonably swing them before your rocket does cartwheels? I would use the limits of 0 to 180 for all three steps.

//Set Min/Max values for PID Output
int MAXANGLE = 180;
int MINANGLE = 0;

  PitchPID.SetOutputLimits(MINANGLE, MAXANGLE);
  RollPID.SetOutputLimits(MINANGLE, MAXANGLE);
    PitchOutput = OutputPitch;
    RollOutput = OutputPitch;

/* Not needed since the Output range is already limited
    if (PitchOutput >= MAXANGLE){
      PitchOutput = MAXANGLE;
    }
    if (PitchOutput <= MINANGLE){
      PitchOutput = MINANGLE;
    }
    if (RollOutput >= MAXANGLE){
      RollOutput = MAXANGLE;
    }
    if (RollOutput <= MINANGLE){
      RollOutput = MINANGLE;
    }
*/
5

I understood that, and they are STILL wrong. Here are the correct equations.
https://wiki.dfrobot.com/How_to_Use_a_Three-Axis_Accelerometer_for_Tilt_Sensing

You are of course free to cyclically permute the indices like this: x,y,z -> z,x,y -> y,z,x

  roll = atan2(y, z) * 57.3; //about x
  pitch = atan2((- x) , sqrt(y * y + z * z)) * 57.3;  //about y
  
 // replace x,y, z -> z, x, y
  
  roll = atan2(x, y) * 57.3; //about z
  pitch = atan2((- z) , sqrt(x * x + y * y)) * 57.3;  //about x
  
//  replace x, y, z -> y, z, x
  
  roll = atan2(y, z) * 57.3; //about y
  pitch = atan2((- x) , sqrt(y * y + z * z)) * 57.3;  //about z
6

I agree! Certainly about the messiest way to get the results. I've iterated through a bunch of designs which could handle different output angles which is the cause of the mess! I clearly must've confused myself because in actuality I should have had - 90 to 90 as the range. The reason for the + 90 was because 0 has to be the 'resting' position on each of the servos for the the motor to be pointed straight down.

So perhaps...

//Set Min/Max values for PID Output
int MAXANGLE = 90;
int MINANGLE = -90;

  PitchPID.SetOutputLimits(MINANGLE, MAXANGLE);
  RollPID.SetOutputLimits(MINANGLE, MAXANGLE);
    PitchOutput = OutputPitch + 90;
    RollOutput = OutputPitch + 90;

I'll give this a go and see how it works as soon as I get a chance. Thanks!

7

Ah I see. I did come across an overwhelming amount of different formulas for pitch and roll when researching this. I'll sub out the current calculation with this :slight_smile:

Looks like I have more to research! Thanks again for the push in the right direction with research topics for better understanding!

8

what do your x, y and z measurements mean with respect to pitch and roll? does pitch rotate around the y-axis and roll around the z-axis?

i suggest you build a simulation of your system on your laptop to exercise and test this code

9

Hi,
Pitch is around X and roll around the Y relative to the rocket. Because the IMU is verticle oriented I have swapped the X and Z axis around. So relative to the IMU and it's readings Pitch is around the Z axis and Roll is around the Y

10

You are filtering Pitch and Roll but I see you are using the RAW versions for the PID.

And then you DON'T USE THE PID OUTPUT AT ALL.

I see you are running the PID loops only when the MPU has new data. The PIDs need to run very frequently so the calculation intervals are close to constant. Each PID keeps a separate timer so it might be good to run them separately:

void loop()
{
  if (mpu.update())
  {
    float ax = mpu.getAccX() + 1;  // Why +1?
    float ay = mpu.getAccY();
    float az = mpu.getAccZ() - 1;  // Why -1?

    InputPitch = 180 * atan (az / sqrt(ay * ay + ax * ax)) / M_PI;
    InputRoll = 180 * atan (ay / sqrt(az * az + ax * ax)) / M_PI;
  }

  if (PitchPID.Compute())
  {
    PitchAxis.write(OutputPitch + 90);
  }

  if (RollPID.Compute())
  {
    RollAxis.write(OutputRoll + 90);
  }
}
11

are you really measuring acceleration?

shouldn't you measure position null the lateral positions?

12

I did this because I wasn't confident with the filter I was using. From what I can tell it seems to be the job so I'll certainly update that

This was just to test the servos, forgot to change it back. When analysing the data I had been using the serial readings not the movement of servos anyway just to be sure the errors aren't with the mechanical design.

This is a very good point. Thanks!

The -/+1 was another step to adjust for the orientation of the IMU. My last time approaching this project I'm sure the pitch and roll worked perfectly...It doesn't anymore so perhaps I have broken something in the code along the way.... None the less I'll apply the formula as suggested by Jremington and give it another wack.

I actually had this thought too, in all honesty I just don't know where to begin with this... Any pointers would be great! The closest thing I've seen so far is someone using turtle in python to simulate their PID loop...

13

i develop with gcc and make using cygwin on my laptop.

you could start with your code and add a main.cpp the executes setup() and repeatedly calls loop().

if you can't directly use library code, you would need to create functions for any of the Arduino libraries you use: Servo, SFE_BMP180, MPU9250, ExponentialFilter and PID. some of these (e.g. MPU9250) you'll need to simulate

start with just a few and add them as you gain understanding. already this must sound like a lot of work, but you're going to have to understand this stuff eventually if you want to succeed

i use make to not only build the code, but run it with any arguments (options) and filenames. i'll run make within a shell while loop in one window while editing the code in another. if there are errors, i quickly fix them and then when it runs i quickly view the output and start making modifications.

i can make a lot of progress in a short time this way.

one of the first things to do is create a model for the behavior of your target based on newton's laws (F = ma), integrating acceleration to determine position. adding some lateral force (e.g. wind) would create a movement that needs to be corrected.

any reasonable estimate is better than guessing or trial and error without being able to collect data.

at first, look at the PID corrections. at the very least make sure they are in the correct directions. then find a way to map the servo movement to some corrective force in the model

again, this may sound daunting, but you're going to have to figure out the gain in the PID mapping an input to output -- the K values

i'm guessing right now you're pushing on a circuit board with the MPU9250 and looking at servo movement -- and have no way of know if it's enough or too much

14

Garbage In, Garbage Out.

You can keep fudging stuff like this forever, without getting anywhere, or you could sit down and learn the theory.

Good luck with your project.

15

This was me trying to apply the theory I had been learning :confused:

As far as my understanding, the readings on the accelerometer measure units of gravity (g's). When the device is laid flat 1g of gravity is being applied to the Z axis. Because the device is not laid flat and is instead vertical, I have removed the 1g on the z axis and added it to the x axis so that in affect, the x axis is now the new z axis. This wasn't just a random number I plucked out of thin air or just a way for me to get the numbers to look nice... I thought it made perfect sense.

I have read up on the article you sent, an article I've actually come across a fair few times but I couldn't actually get the formula to work and output readings that where accurate. Nonetheless I trust you are infinity more knowledgable on the subject than I so have added the function in to test with when I get around to pulling out the PCB from the chasis. I had settled on the formula in the code above because it does work and shows accurate readings BUT there is indeed a problem which is that after 90 degrees it begins to go back downwards instead of continuing upwards which will ofcourse be a massive issue moving forward. I do suspect the +/-1 might have something to do with that but also don't know a better way to handle that particular issue.

16

Firstly, thanks! This is incredibly helpful and will give me a good place to start.

Exactly right, I will have to do tuning which I really do want to minimise having to do because while testing and tuning I will inevitably break chasis and have to rebuild and print parts (which I'd like to avoid as much as possible)

Only issue for the moment is I don't have thrust measurements from the motor yet and have yet to do testing with various propellors and configurations to optimise it, presumably knowing those details will be important to the model. I am curious how I would simulate things like vibration and torque, which are the main things I think will be problematic when it becomes time to actually take it for a spin. Perhaps this will become obvious as I begin to get into the simulation...?

17

i started looking at this (curious).

i realized that lateral forces, when the thrust vector is not thru the center of mass, result in a rotation around the y & z axis. this rotation further angles the thrust vector. is this what you mean by "torque"

i added a random value to the thrust angle (2d so far). are you suggesting vibration will not hold the thrust angle reliably

i believe working on a simulation helps "reveal" these issues. once a simulation is available, the thrust force can easily be increased to see effects. perhaps another concern is the response time of the servos, how long it takes for them to reach a specified position.

18

Gravity is always being measured by the device, unless it is in free fall. A reading corresponding to 1g will be present along ANY vertical axis, or that 1g will be divided among the three axes if one axis is not perfectly vertical. You cannot arbitrarily add or subtract 1g to an axis to make that reading go away, and if you do so, the formulas for tilt and roll will give totally incorrect angles.

You should play with the sensor on the bench until you understand what the readings actually mean.

How tilt and roll is calculated from the accelerometer readings is presented here: How_to_Use_a_Three-Axis_Accelerometer_for_Tilt_Sensing-DFRobot and much more completely, here: https://www.nxp.com/docs/en/application-note/AN3461.pdf

19

This topic was automatically closed 120 days after the last reply. New replies are no longer allowed.