Getting Pitch and Roll from Acceleromter data

Projects Programming
1

Hi Everyone,

I've been doing some digging around with getting angular (pitch & roll) data from accelerometer data. I wish to return the angular data in degrees. I've found this in a forum post and implemented it in to calculate the yaw and roll

  roll = atan2 (Ax,Az) * RAD_TO_DEG;
  pitch = atan2 (Ay, Az) * RAD_TO_DEG;

Now this does appear to do something... but doesn't appear to result in the correct values. When I rotate the device approx 90 degrees it returns values closer to half the actual value. I'm cautious of just doubling this value and calling it a day because I can't say with any confidence that it is indeed 90 degrees I am holding the flight controller at.

I have seen a series of other posts suggesting other solutions such as

pitch = 180 * atan2(accelX, sqrt(accelY*accelY + accelZ*accelZ))/PI;
roll = 180 * atan2(accelY, sqrt(accelX*accelX + accelZ*accelZ))/PI;

This example seems to result in a value even further from reality and returns a value in the 30s when held at approx 90 degrees. I've also seen the following which seems to suggest I need to do something with the raw data of the accelerometer. It seems to create two signed integers but doesn't assign the variable a name or value so not entirely sure what to make of that other than to perhaps multiply the accel data by 3.9.

ccelerationX = (signed int)(((signed int)rawData_X) * 3.9);
accelerationY = (signed int)(((signed int)rawData_Y) * 3.9);
accelerationZ = (signed int)(((signed int)rawData_Z) * 3.9);
pitch = 180 * atan (accelerationX/sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))/M_PI;
roll = 180 * atan (accelerationY/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

.

I've also noticed in both of these examples both the pitch and the roll change when rotating the device even if rotating it along only one axis. Again no idea why...

Context: This is all to go into my flight control for an RC Starship project i'm working on. I'm using an MPU9250 for IMU data and using the "Bolder Flight System" library to read the IMU.

Below is the the whole of the code so far just incase there is something else I am doing wrong. Any criticism is welcome too. I intend to change the PID system to QuickPID rather than the version I've created so I can use the autotune features as recommended by Dlloyd,

#include <Arduino.h>
#include <ESP32Servo.h>
#include <MPU9250.h>

MPU9250 IMU(Wire,0x68);
int status;

Servo Roll;
Servo Pitch;

int YawPos = 90;
int PitchPos= 90;

int pot = 0;

unsigned long start = 0;
unsigned long current = 0;
unsigned long iterationtime = 0;

float PitchBias = 0;
float RollBias = 0;

float pitch = 0;
float roll = 0;

int Kp = 0;
int Ki = 0;
int Kd = 0;

long output (int setpoint, int Input, int Kp, int Ki, int Kd, int delaytime)  {

    static float integral_prior = 0;
    static float error_prior = 0;

    start = millis();

    static float error = setpoint - Input;

    float integral = integral_prior + error * iterationtime;

    float derivative = (error - error_prior) /iterationtime;

    long output = Kp * error + Ki * integral + Kd * derivative;

    error_prior = error;

    integral_prior = integral;

    delay(delaytime);
    
    current = millis();
    iterationtime = current - start;

    return output;

}
int getAvg(float axis){
    int iterations = 20;
    int totalacc = 0;
    float average = 0;

    for (int i = 0; i < iterations; i++){
        totalacc += axis;
        delay(50);
    }
    average = totalacc / iterations;

    return average;
}


void setup() {

  Serial.begin(115200);

  ESP32PWM::allocateTimer(0);
  ESP32PWM::allocateTimer(1);
  ESP32PWM::allocateTimer(2);
  ESP32PWM::allocateTimer(3);

  Roll.setPeriodHertz(50);
  Pitch.setPeriodHertz(50);

  Roll.attach(32, 500, 2400);
  Pitch.attach(4, 500, 2400);

  Roll.write(90);
  Pitch.write(90);

  status = IMU.begin();
  if (status < 0) {
    Serial.println("IMU initialization unsuccessful");
    Serial.println("Check IMU wiring or try cycling power");
    Serial.print("Status: ");
    Serial.println(status);
    while(1) {}
  }

  IMU.readSensor();
  float pitchval = 0;
  float rollval = 0;

  IMU.readSensor();

  float Ax, Ay, Az;

  Ax = IMU.getAccelX_mss();
  Ay = IMU.getAccelY_mss();
  Az = IMU.getAccelZ_mss();

  rollval = atan2 (Ax,Az) * RAD_TO_DEG;
  pitchval = atan2 (Ay, Az) * RAD_TO_DEG;

  RollBias = getAvg(rollval);
  PitchBias = getAvg(pitchval);

  Serial.print("Pitch Bias = ");
  Serial.print(PitchBias);
  Serial.print("  /  Roll Bias = ");
  Serial.println(RollBias);
}

void loop() {
  
  IMU.readSensor();

  float Ax, Ay, Az;

  Ax = IMU.getAccelX_mss();
  Ay = IMU.getAccelY_mss();
  Az = IMU.getAccelZ_mss();

  int pitch;
  int roll;

  roll = atan2 (Ax,Az) * RAD_TO_DEG;
  pitch = atan2 (Ay, Az) * RAD_TO_DEG;

  roll = roll - RollBias;
  pitch = pitch - PitchBias;

  float rolloutput = output(0, roll, Kp, Ki, Kd, 20);
  float pitchoutput = output(0, pitch, Kp, Ki, Kd, 20);

  Roll.write(rolloutput);
  Pitch.write(pitchoutput);

  Serial.print("Roll = ");
  Serial.print(roll);
  Serial.print("  /  Pitch = ");
  Serial.println(pitch);


}

Thanks in advance!

2

None of those formulas for pitch and roll are correct, but there are several correct ones depending on how pitch and roll are defined.

Euler angles are very confusing, as there are about 12 different definitions, the ORDER in which you apply the rotations matters, and there is no universal definition for a positive rotation angle.

Study this tutorial for a method that actually works. This definition for pitch and roll is generally useful:

  roll = atan2(y_Buff , z_Buff) * 57.3;
  pitch = atan2((- x_Buff) , sqrt(y_Buff * y_Buff + z_Buff * z_Buff)) * 57.3;
3

jremington:
Study this tutorial for a method that actually works. This definition for pitch and roll is generally useful:

  roll = atan2(y_Buff , z_Buff) * 57.3;

pitch = atan2((- x_Buff) , sqrt(y_Buff * y_Buff + z_Buff * z_Buff)) * 57.3;

That's very helpful thanks! I've looked over that document a handful of times and tend to get frustrated with the complicated algebraic expression that are often very comprehend-able in pseudocode form (At least to someone such as myself who has limited subject knowledge on maths in general). Also particularly when they lack a key or something that helps me get a grip of what each expression is referring to to be able to use it.

I seem to continually get caught up at eulers angles. I believe I've raised the question in a post I've created before. I've been trying to gather contextual knowledge to properly grasp my understanding. I understand why this would be something I'd need for attempting to manipulate the vehicles orientation but is the order of readings significant in the reading of this data as well? I understand there are 6 different possibilities of orders to read the data in but what is the significance to this. What different data does each combination describe? Is it simply to do with the order in which the rotations where applied? If so there is surely no way of knowing which order all rotations will be applied to the vehicle once it's been completed so this wouldn't work consistently unless I use a series of if statements to asses the order of rotation, which in itself, would be extremely difficult to determine.

I don't quite get what is expected to be stored in the "_buff" variables... From what I can see in the article it's just the raw data accelerometer data for the respective axis, is that right? Would just using that equation in my code be an oversimplification or a lazy choice?

4

I don't quite get what is expected to be stored in the "_buff" variables.

The X, Y and Z acceleration values, as floating point numbers.

Euler angles are covered in this Wikipedia article. They actually are not very useful for visualizing or representing orientations, which is why everyone now uses either a quaternion or a rotation matrix for that.

5

This function is nonsensical and wrong:

int getAvg(float axis){
    int iterations = 20;
    int totalacc = 0;
    float average = 0;

    for (int i = 0; i < iterations; i++){
        totalacc += axis;
        delay(50);
    }
    average = totalacc / iterations;

    return average;
}

It basically produces corrupt results.

To average you need to read the accelerometer in this loop so that you get different
values and then calculate the mean of those.

Hear you are taking a single float value, adding it to an integer variable 20 times, then
dividing by 20, which is equivalent to

  return (int (value) * 20) / 20 ;

plus a pointless delay.

You seem to have moved the reading of the accelerometer out of this calibration loop
and into setup() for some reason, breaking the averaging completely.

BTW you have the right atan2 formulae - perhaps if you printed out the
values you read from the accelerometer the issue will become clear?

6

Thanks for the feedback Mark and jremington!

Been away from this project for a little waiting for some PCB to arrive so I can test this more easily and in its functional format.

MarkT:
It basically produces corrupt results.

In what sense is the data corrupt? Perhaps it's purpose isn't clear, I never have gotten into the habit of adding comments to my work to clarify each section's purpose. The purpose for this isn't to calculate a running average its just to determine what the starting position is as accurately as possible so that I can subtract that value from the accelerometer data as I read it, hence why I placed it in the setup. The delay is in there to control time (obviously) with the aim to take an average reading over 10 seconds in the startup sequence to set the "home" position. Ultimately its only purpose is to give me "Nicer" numbers to work with and data which is easier to read.

Nonetheless it did work in achieving its intended purpose and set the starting position of the vehicle to 0 so all movement and rotation from that point onward is in reference to its starting position. Of course, if this is an inefficient way to do this I'm open to suggestions! I am a complete novice after all so I do appreciate any constructive criticism.

7

This does nothing useful:

    float average = 0;

    for (int i = 0; i < iterations; i++){
        totalacc += axis;
        delay(50);
    }
    average = totalacc / iterations;
8

jremington:
This does nothing useful:

Am I missing something obvious?
When my IMU begins displaying arbitrary values. It does react and change with movement but these numbers mean nothing to me . They are not an indication of degrees, they are just the values the accel/Gyro/Magnotometer are producing in their respective units. That data is a mess to read. Although ultimately I will be converting this to Yaw Pitch and Roll, at which point I will remove the function, the function allows me to clearly see values relative to the starting position, which is infinitely easier to read and also cleaner data than an arbitrary value increasing or decreasing...?
As I said, it does do that and it works well... so why is it useless? In the grand scheme once I have the yaw pitch and roll working I intend to do the same. If the values that return when the vehicle is stationary in it's starting position is not 0 I will run a similar function to set it to zero. Am I the only one who sees this as an obvious improvement to a random starting position and using those random values to stabilise... After all, crap in, crap out right?
I'm very aware I can just store the starting values in a variable and reference this throughout the code but for telemetry data reading after the fact I just feel this is neater. Isn't having clean easily comprehend-able data not at the core of any control system?

9

amanbasra:
Am I missing something obvious?

Seems so.

amanbasra:
so why is it useless?

It's a very somehow complicated way to compute axis, which is the only input.

Could you please explain the usefulness of the code?

10

Whandall:
Could you please explain the usefulness of the code?

I thought the below explained this.

amanbasra:
the function allows me to clearly see values relative to the starting position, which is infinitely easier to read and also cleaner data than an arbitrary value increasing or decreasing...?

I guess the only other thing I could do is just calculate the change in readings to determine the angle of rotation relative to the starting position, but both are just different ways to do the same thing. Without this function the data means not a-lot to me and wouldn't be very useful to feed into the PID loops as the values will be different each time it turns on. And again... it does work and does this successfully...If I take it out...it doesn't... Again I agree there are different ways to achieve the same results, and probably are better ways to do it, but what am I missing that renders this useless.

Whandall:
Seems so.

Please enlighten me as to what it is I'm missing.

11

what am I missing that renders this useless.

What you are missing is the understanding of what that bit of code does.

Work through it, line by line, with some assumed values for the variables, and then explain to us why you think the result is useful.

12

jremington:
What you are missing is the understanding of what that bit of code does.

Work through it, line by line, with some assumed values for the variables, and then explain to us why you think the result is useful.

Ok I think the penny has dropped. The loop is incorrectly written as opposed to the loop's purpose being useless itself.
Simply changing the totalacc to a static variable and changing it to a float would presumably sufficed? (as MarkT had initially pointed out, but I had failed to comprehend )
Either way i've probably severely overcomplicated this and probably even worked in a few more errors. Instead of doing it the above way I've stored each reading into an array, then totalled the array to then divided that by the amount of iterations to produce the average (Or at least that's what I tried to do).

float AvgData(float axis){
  float average[] = {20};
  for (int i = 0; i < 20; i++){
    average[i] = axis;
    delay(50);
  }
  float total;
  for (int i = 0; i < 20; i++){
    total += average[i];
  }
  float avg = total/20;
  return avg;
}

Surprisingly this is one of the simplest parts of the overall code which is now, in my amateur opinion, fairly sophisticated programme. Regardless, please scrutinise if necessary!

13 
float average[] = {20};
  for (int i = 0; i < 20; i++){
    average[i] = axis;
    delay(50);

That is a one element array. Tryfloat average[20] = {0};

In fact, the array is pointless - summing the same value twenty times does not make an average.

Also, "total" is uninitialised.

Bin time.

14

TheMemberFormerlyKnownAsAWOL:
In fact, the array is pointless - summing the same value twenty times does not make an average.

Also, "total" is uninitialised.

Ok, so I think the penny has actually dropped this time.... I don't know why I thought the axis variable would update in the loop and it makes sense now why I'd need to run this function in a loop. I was trying to be lazy and use the same function to calculate the average for Yaw and Roll and just pass in the data as a variable but I guess I can just run this in setup.

float YawAvg(){
  float average[] = {20};
  for (int i = 0; i < 20; i++){
    average[i] = mpu.getYaw();
    delay(50);
  }
  float total = 0;
  for (int i = 0; i < 20; i++){
    total += average[i];
  }
  float avg = total/20;
  return avg;
}
float RollAvg(){
  float average[] = {20};
  for (int i = 0; i < 20; i++){
    average[i] = mpu.getRoll();
    delay(50);
  }
  float total = 0;
  for (int i = 0; i < 20; i++){
    total += average[i];
  }
  float avg = total/20;
  return avg;
}

I almost don't want to ask if I have made any fatal errors, if so I may just be a lost cause!

15

float average[] = {20};Again, a one element array.

Forget the array - unless you later intend to do a rolling average, it is unnecessary; just sum your twenty consecutive readings.

16

TheMemberFormerlyKnownAsAWOL:

float average[] = {20};

Again, a one element array.

Forget the array - unless you later intend to do a rolling average, it is unnecessary; just sum your twenty consecutive readings.

In all fairness I only added the array because I wanted to exercise the use of them. Clearly my understanding is incorrect in some capacity, to my knowledge that loop should be cycling through 20 different elements and storing the reading from that cycle into the the index that it's on. Regardless I've finally got a working function which does produce averages. I've written the results to serial monitor to confirm that it is doing what it needs to now.

float getPitchAvg(int iterations){
  Serial.println("Getting Average Pitch Data...");
  float total = 0;
  for(int i = 0; i <= iterations; i++){
    mpu.update();
    total += mpu.getPitch();
    Serial.print("Total = ");
    Serial.println(total);
  }
  float avg = total / iterations;
  return avg;
}
17

No, your array version loop is iterating over twenty elements of a one element array. See reply #12.

Your calculation is still incorrect - you're summing "iterations" plus one readings, but dividing the sum by "iterations"

18

TheMemberFormerlyKnownAsAWOL:
No, your array version loop is iterating over twenty elements of a one element array. See reply #12.

I see now, the array element number is put in the [] not the {} I ready your reply and didn't even notice! After a certain amount of time spent looking over the code the numbers all eventually just blur ah!

I had realised the iteration issue and changed it to < iterations rather than <= iterations

Thanks for all you're help on this! Got there in the end!

19

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